修正亚像素调整中的一个错误

camCalib/camCalib.cpp

@@ -29,7 +29,7 @@ void sg_outputCalibK(const char* fileName, cv::Mat& fitMap)
     return;
 }
 
-void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
+void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D, cv::Vec4f& pe)
 {
     std::ofstream sw(fileName);
 
@@ -50,6 +50,20 @@ void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
         temp[_c] = D.ptr<double>(0)[_c];
     sprintf_s(dataStr, 250, "%g, %g, %g, %g, %g", temp[0], temp[1], temp[2], temp[3], temp[4]);
     sw << dataStr << std::endl;
+
+    //ax+by+cz+d = 0
+    float a = (float)pe[0];
+    float b = (float)pe[1];
+    float c = (float)pe[2];
+    float d = (float)pe[3];
+    //将c变成-1，转成z=ax+by+c的形式，使用3个参数
+    a = -a / c;
+    b = -b / c;
+    d = -d / c;
+    c = -1;
+    sprintf_s(dataStr, 250, "%g, %g, %g", a, b, d);
+    sw << dataStr << std::endl;
+
     sw.close();
     return;
 }
@@ -87,6 +101,24 @@ void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
     return;
 }
 
+void saveSubpixData(char* filename, std::vector<cv::Point2f>& subpixPnt)
+{
+    if (subpixPnt.size() < 6)
+        return;
+
+    std::ofstream TXTFile(filename);
+    char TXTData[250];
+    int headOffset = 6;
+    for (int i = 6, i_max = (int)subpixPnt.size(); i < i_max; i++)
+    {
+        cv::Point2f a_subPix = subpixPnt[i];
+        snprintf(TXTData, sizeof(TXTData), "%.5f %.5f", a_subPix.x, a_subPix.y);
+        TXTFile << TXTData << std::endl;
+    }
+    TXTFile.close();
+}
+
+
 typedef struct
 {
     int nMin;	//< 最小值
@@ -130,13 +162,13 @@ int main()
     std::cout << "Hello World!\n";
 
     const char* calibDataPath[CALIB_TEST_GROUP] = {
-        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\撕裂原理相机标定图像\\",  //0
-        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\chessboard\\",   //1
-        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\circlePoint\\", //2
-        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\charuCo\\", //3
+        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\撕裂原理相机标定图像\\",  //0
+        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\chessboard\\",   //1
+        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\circlePoint\\", //2
+        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\charuCo\\", //3
     };
     const SWdNLRange fileIdx[CALIB_TEST_GROUP] = {
-        {3,39},{1,200},{1,166},{122,141}
+        {3,39},{1,33},{1,33},{1,10}
     };
     const int boardType[CALIB_TEST_GROUP] =
     {
@@ -148,7 +180,7 @@ int main()
 
     for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++)
     {
-        grp = 2;
+        grp = 1;
         int calibType = boardType[grp];
         cv::Size cbPattern;
         float cbSquareSize;
@@ -283,9 +315,7 @@ int main()
         std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl;
 
         //生成矫正图像
-        index = 3;
-        for (;; index++) {
-
+        for (index = startIndex; index <= endIndex; index++) {
             char filename[256];
             sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
             cv::Mat srcImg = cv::imread(filename);
@@ -306,10 +336,6 @@ int main()
             cv::imwrite(filename, calibImg);
         }
 
-        //output K and D
-        char calibKDName[256];
-        sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
-        sg_outputCalibKD(calibKDName, K, D);
 #else
         char calibKDName[256];
         sprintf_s(calibKDName, "%scalib_param_K_D.txt", cbImagePath);
@@ -354,136 +380,156 @@ int main()
 
 #endif
 
-        {
-            std::vector<cv::Point3f> all_pts3d;
+        std::vector<cv::Point3f> all_pts3d;
+        for (index = startIndex; index <= endIndex; index++) {
 
-            index = 3;
-            for (;; index++) {
+            char filename[256];
+            sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
+            cv::Mat srcImg = cv::imread(filename);
+            if (srcImg.empty())
+                break;
 
-                char filename[256];
-                sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
-                cv::Mat srcImg = cv::imread(filename);
-                if (srcImg.empty())
-                    break;
+            cv::Mat img;
+            cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
+            std::vector<cv::Point2f> corners;
+            detectCorners(img, cbPattern, corners);
+            if (corners.empty())
+                continue;
 
-                cv::Mat img;
-                cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
-                std::vector<cv::Point2f> corners;
-                detectCorners(img, cbPattern, corners);
-                if (corners.empty())
-                    continue;
-
-                // 创建棋盘格区域的掩码
-                cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
-                // 使用多边形近似来填充角点之间的区域
-                // 棋盘格区域需要比角点区域大一圈
-                std::vector<cv::Point2f> contour_line[4];
-                for (int i = 0; i < cbPattern.width; i++)
-                {
-                    cv::Point2f pt_c = corners[i];
-                    cv::Point2f pt_2 = corners[cbPattern.width + i];
-                    cv::Point2f pt_1;
-                    pt_1.x = pt_c.x * 2 - pt_2.x;
-                    pt_1.y = pt_c.y * 2 - pt_2.y;
-                    contour_line[0].push_back(pt_1);
-                }
-                for (int i = 0; i < cbPattern.height; i++)
-                {
-                    cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1];
-                    cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2];
-                    cv::Point2f pt_1;
-                    pt_1.x = pt_c.x * 2 - pt_2.x;
-                    pt_1.y = pt_c.y * 2 - pt_2.y;
-                    contour_line[1].push_back(pt_1);
-                }
-                for (int i = cbPattern.width - 1; i >= 0; i--)
-                {
-                    cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i];
-                    cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i];
-                    cv::Point2f pt_1;
-                    pt_1.x = pt_c.x * 2 - pt_2.x;
-                    pt_1.y = pt_c.y * 2 - pt_2.y;
-                    contour_line[2].push_back(pt_1);
-                }
-                for (int i = cbPattern.height-1; i >= 0; i--)
-                {
-                    cv::Point2f pt_c = corners[i * cbPattern.width];
-                    cv::Point2f pt_2 = corners[i * cbPattern.width + 1];
-                    cv::Point2f pt_1;
-                    pt_1.x = pt_c.x * 2 - pt_2.x;
-                    pt_1.y = pt_c.y * 2 - pt_2.y;
-                    contour_line[3].push_back(pt_1);
-                }
-                std::vector<cv::Point> contours;
-                //生成轮廓点
-                for (int n = 0; n < 4; n++)
-                {
-                    int num = contour_line[n].size();
-                    for (int i = 0; i < num; i++)
-                        contours.push_back(contour_line[n][i]);
-                    cv::Point2f pt_c = contour_line[n][num - 1];
-                    cv::Point2f pt_2 = contour_line[n][num - 2];
-                    cv::Point2f pt_1;
-                    pt_1.x = pt_c.x * 2 - pt_2.x;
-                    pt_1.y = pt_c.y * 2 - pt_2.y;
-                    contours.push_back(pt_1);
-                }
-                // 使用 fillPoly 填充多边形
-                cv::Scalar color(255); // 红色
-                cv::fillPoly(chessMask, contours, color);
-#if 1
-                sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index);
-                cv::imwrite(filename, chessMask);
-#endif
-
-                cv::Vec4f pe;
-                fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);
-
-                sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index);
-                cv::Mat srcLaserImg = cv::imread(filename);
-                if (srcLaserImg.empty())
-                    break;
-
-                cv::Mat laserImg_unMask;
-                cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE);
-                //与Mask相与，保证待处理的激光线在标定板上
-                cv::Mat laserImg;
-                cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
-#if 1
-                sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
-                cv::imwrite(filename, laserImg);
-#endif
-                std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
-                //显示亚像素点
-                cv::Mat enlargeImg;
-                if (laserImg.channels() == 1)
-                    laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
-                else
-                    enlargeImg = laserImg.clone();
-                cv::Size objSize = laserImg.size();
-                objSize.width = objSize.width * 5;
-                cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);
-
-                for (int i = 0, i_max = pts2d.size(); i < i_max; i++)
-                {
-                    cv::Point2f a_subPix = pts2d[i];
-                    a_subPix.x += 0.5;
-                    int row = (int)(a_subPix.y + 0.5);
-                    int col = (int)(a_subPix.x * 5 + 0.5);
-                    enlargeImg.at<cv::Vec3b>(row, col)[0] = 0;
-                    enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
-                    enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
-                }
-                sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index);
-                cv::imwrite(filename, enlargeImg);
-                std::vector<cv::Point3f> pts3d = project2DTo3D(pts2d, pe, K, D);
-
-                all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end());
+            // 创建棋盘格区域的掩码
+            cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
+            // 使用多边形近似来填充角点之间的区域
+            // 棋盘格区域需要比角点区域大一圈
+            std::vector<cv::Point2f> contour_line[4];
+            for (int i = 0; i < cbPattern.width; i++)
+            {
+                cv::Point2f pt_c = corners[i];
+                cv::Point2f pt_2 = corners[cbPattern.width + i];
+                cv::Point2f pt_1;
+                pt_1.x = pt_c.x * 2 - pt_2.x;
+                pt_1.y = pt_c.y * 2 - pt_2.y;
+                contour_line[0].push_back(pt_1);
             }
+            for (int i = 0; i < cbPattern.height; i++)
+            {
+                cv::Point2f pt_c = corners[i * cbPattern.width + cbPattern.width - 1];
+                cv::Point2f pt_2 = corners[i * cbPattern.width + cbPattern.width - 2];
+                cv::Point2f pt_1;
+                pt_1.x = pt_c.x * 2 - pt_2.x;
+                pt_1.y = pt_c.y * 2 - pt_2.y;
+                contour_line[1].push_back(pt_1);
+            }
+            for (int i = cbPattern.width - 1; i >= 0; i--)
+            {
+                cv::Point2f pt_c = corners[(cbPattern.height - 1) * cbPattern.width + i];
+                cv::Point2f pt_2 = corners[(cbPattern.height - 2) * cbPattern.width + i];
+                cv::Point2f pt_1;
+                pt_1.x = pt_c.x * 2 - pt_2.x;
+                pt_1.y = pt_c.y * 2 - pt_2.y;
+                contour_line[2].push_back(pt_1);
+            }
+            for (int i = cbPattern.height-1; i >= 0; i--)
+            {
+                cv::Point2f pt_c = corners[i * cbPattern.width];
+                cv::Point2f pt_2 = corners[i * cbPattern.width + 1];
+                cv::Point2f pt_1;
+                pt_1.x = pt_c.x * 2 - pt_2.x;
+                pt_1.y = pt_c.y * 2 - pt_2.y;
+                contour_line[3].push_back(pt_1);
+            }
+            std::vector<cv::Point> contours;
+            //生成轮廓点
+            for (int n = 0; n < 4; n++)
+            {
+                int num = contour_line[n].size();
+                for (int i = 0; i < num; i++)
+                    contours.push_back(contour_line[n][i]);
+                cv::Point2f pt_c = contour_line[n][num - 1];
+                cv::Point2f pt_2 = contour_line[n][num - 2];
+                cv::Point2f pt_1;
+                pt_1.x = pt_c.x * 2 - pt_2.x;
+                pt_1.y = pt_c.y * 2 - pt_2.y;
+                contours.push_back(pt_1);
+            }
+            // 使用 fillPoly 填充多边形
+            cv::Scalar color(255); // 红色
+            cv::fillPoly(chessMask, contours, color);
+#if 1
+            sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index);
+            cv::imwrite(filename, chessMask);
+#endif
 
-            cv::Vec4f pe = fitPlaneToPoints(all_pts3d);
-            std::cout << "pe: " << pe << std::endl;
+            cv::Vec4f pe;
+            fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);
+
+            sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index);
+            cv::Mat srcLaserImg = cv::imread(filename);
+            if (srcLaserImg.empty())
+                break;
+
+            cv::Mat laserImg_unMask;
+            cv::rotate(srcLaserImg, laserImg_unMask, cv::ROTATE_90_COUNTERCLOCKWISE);
+            //与Mask相与，保证待处理的激光线在标定板上
+            cv::Mat laserImg;
+            cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
+#if 1
+            sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
+            cv::imwrite(filename, laserImg);
+
+            cv::Mat calibImg;
+            remap(laserImg,
+                calibImg,
+                mapGen_x,
+                mapGen_y,
+                cv::INTER_LINEAR,
+                cv::BORDER_CONSTANT,
+                cv::Scalar(0, 0, 0));
+            sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index);
+            cv::imwrite(filename, calibImg);
+
+#endif
+            std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
+            //显示亚像素点
+            cv::Mat enlargeImg;
+            if (laserImg.channels() == 1)
+                laserImg.convertTo(enlargeImg, cv::COLOR_GRAY2BGR);
+            else
+                enlargeImg = laserImg.clone();
+            cv::Size objSize = laserImg.size();
+            objSize.width = objSize.width * 5;
+            cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);
+
+            if (pts2d.size() > 0)
+            {
+                sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpixData.txt", calibDataPath[grp], index);
+                saveSubpixData(filename, pts2d);
+            }
+            for (int i = 0, i_max = (int)pts2d.size(); i < i_max; i++)
+            {
+                cv::Point2f a_subPix = pts2d[i];
+                int row = (int)(a_subPix.y + 0.5);
+                int col = (int)(a_subPix.x * 5 + 0.5);
+                enlargeImg.at<cv::Vec3b>(row, col)[0] = 0;
+                enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
+                enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
+            }
+            sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index);
+            cv::imwrite(filename, enlargeImg);
+            std::vector<cv::Point3f> pts3d = project2DTo3D(pts2d, pe, K, D);
+#if 1
+            //保存3D点
+
+#endif
+            all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end());
         }
+
+        cv::Vec4f pe = fitPlaneToPoints(all_pts3d);
+        std::cout << "pe: " << pe << std::endl;
+
+        //output K and D
+        char calibKDName[256];
+        sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
+        sg_outputCalibKD(calibKDName, K, D, pe);
     }
 
     return 0;

camCalib/lineDetection_steger.cpp

@@ -436,7 +436,7 @@ void computePointSubpix(
 			if ((subpix.x >= 0) && (subpix.y >= 0))
 			{
 				subpix.x += 0.5; //图像中的像素位置表示的是像素是左下角，而此处像素坐标是指向像素中间位置，所以有0.5像素差
-				subpix.x -= 0.5;  //此处图像的Y坐标系方向与通常坐标系的方向相反
+				subpix.y -= 0.5;  //此处图像的Y坐标系方向与通常坐标系的方向相反
 				posSubpix.push_back(subpix);
 				doSubPix = -1;
 			}

camCalib/sourceCode/MonoLaserCalibrate.cpp

@@ -191,7 +191,7 @@ void monocularCalibration(
     flags |= cv::fisheye::CALIB_FIX_SKEW;
     cv::fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);// , flags, cv::TermCriteria(3, 20, 1e-6));
 #else
-     cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);
+     cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, cv::CALIB_FIX_ASPECT_RATIO);
 #endif
     // 重投影三维点到二维图像点
     // 计算重投影误差