添加了charuco的修正系数，以补偿标定板的制作误差

camCalib/camCalib.cpp

@@ -306,6 +306,7 @@ int main()
 
         // 输出映射类型，通常使用CV_32FC1或CV_16SC2
         cv::Mat backwardMap_x, backwardMap_y;
+        cv::Mat forwardMap_x, forwardMap_y;
         cv::Mat newCamMatrix;
         // 生成畸变矫正映射
 #if ENABLE_FISH_EYE
@@ -313,13 +314,13 @@ int main()
 #else
         double alpha = 0.4; // 0.4;
         newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
-        //cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, map_x, map_y);
+        cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, backwardMap_x, backwardMap_y);
-        initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, backwardMap_x, backwardMap_y);
+        initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, forwardMap_x, forwardMap_y);
 #endif
 
         // 生成系数表
-        cv::Mat fitMap_x = GetFitParamMap(backwardMap_x, 1);
+        cv::Mat fitMap_x = GetFitParamMap(forwardMap_x, 1);
-        cv::Mat fitMap_y = GetFitParamMap(backwardMap_y, 1);
+        cv::Mat fitMap_y = GetFitParamMap(forwardMap_y, 1);
         //输出系数文件
         char calibParamName[256];
         sprintf_s(calibParamName, "%scalib_param_x.txt", calibDataPath[grp]);
@@ -333,8 +334,8 @@ int main()
         //搜索最大和平均误差
             // 计算绝对差异
         cv::Mat diff_x, diff_y;
-        cv::absdiff(backwardMap_x, mapGen_x, diff_x);
+        cv::absdiff(forwardMap_x, mapGen_x, diff_x);
-        cv::absdiff(backwardMap_y, mapGen_y, diff_y);
+        cv::absdiff(forwardMap_y, mapGen_y, diff_y);
         // 查找最大值和最小值
         double minVal_x, maxVal_x;
         cv::minMaxLoc(diff_x, &minVal_x, &maxVal_x);
@@ -362,8 +363,8 @@ int main()
             cv::Mat calibImg;
             cv::remap(img,
                 calibImg,
-                mapGen_x,
+                backwardMap_x,
-                mapGen_y,
+                backwardMap_y,
                 cv::INTER_LINEAR,
                 cv::BORDER_CONSTANT,
                 cv::Scalar(0, 0, 0));
@@ -434,7 +435,24 @@ int main()
 
             cv::Mat img;
             cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
+#if 0
+            cv::Mat charucoCalibImg;
+            cv::remap(img,
+                charucoCalibImg,
+                backwardMap_x,
+                backwardMap_y,
+                cv::INTER_LINEAR,
+                cv::BORDER_CONSTANT,
+                cv::Scalar(0, 0, 0));
+            cv::Size _size = charucoCalibImg.size();
+            _size.width = _size.width * 5;
+            //cv::resize(charucoCalibImg, charucoCalibImg, _size, 0, 0, cv::INTER_NEAREST);
+            sprintf_s(filename, "%scalib_%03d_calib.bmp", calibDataPath[grp], index);
+            cv::imwrite(filename, charucoCalibImg);
+#endif
+
             std::vector<cv::Point2f> corners;
+            std::vector<int> charucoIds;
             if (CALIB_CHESS_BOARD == calibType)
             {
                 detectCorners(img, cbPattern, corners);
@@ -447,7 +465,6 @@ int main()
             {
                 std::vector<int> markerIds;
                 std::vector<std::vector<cv::Point2f> > markerCorners;
-                std::vector<int> charucoIds;
                 detectCharucoCorners(img,
                     cbPattern,
                     cbSquareSize,
@@ -466,7 +483,7 @@ int main()
                 chessMask = cv::Mat::ones(img.size(), CV_8UC1);
             else
             {
-                cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
+                chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
                 // 使用多边形近似来填充角点之间的区域
                 // 棋盘格区域需要比角点区域大一圈
                 std::vector<cv::Point2f> contour_line[4];
@@ -530,7 +547,10 @@ int main()
             }
 
             cv::Vec4f pe;
-            fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);
+            if (CALIB_CHARUCO == calibType)
+                fitChessboardPlane_charuco(charucoIds,corners, K, D, cbPattern, cbSquareSize, pe);
+            else
+                fitChessboardPlane_chessboard(corners, K, D, cbPattern, cbSquareSize, pe);
 
             sprintf_s(filename, "%slaser_%03d.bmp", calibDataPath[grp], index);
             cv::Mat srcLaserImg = cv::imread(filename);

camCalib/sourceCode/MonoLaserCalibrate.cpp

@@ -191,58 +191,48 @@ void detectCharucoCorners(const cv::Mat& img,
         cv::Ptr<cv::aruco::CharucoBoard> ptrBoard = cv::makePtr<cv::aruco::CharucoBoard>(board);
         cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, gray, ptrBoard, charucoCorners, charucoIds);
         // if at least one charuco corner detected
+#if 1
         if (charucoIds.size() > 0)
             // 亚像素精确化
             cv::cornerSubPix(gray, charucoCorners, cv::Size(11, 11), cv::Size(-1, -1),
                 cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 50, 0.1));
+#endif
     }
     return;
 }
 
 void gen3DCoordinate_chessboard(
-    const std::vector<std::vector<cv::Point2f>>& imagePoints, 
+    const std::vector<cv::Point2f>& corners, 
     const cv::Size& patternSize,
     const float squareSize,
-    std::vector<std::vector<cv::Point3f>>& objectPoints
+    std::vector<cv::Point3f>& objectPoints
     )
 {
-    // 根据角点生成3d点
+    // 准备3D世界坐标点 (z=0)
-    for (const auto& corners : imagePoints) {
+    for (int i = 0; i < patternSize.height; ++i)
+        for (int j = 0; j < patternSize.width; ++j)
+            objectPoints.emplace_back(j * squareSize, i * squareSize, 0);
 
-        // 准备3D世界坐标点 (z=0)
-        std::vector<cv::Point3f> obj;
-        for (int i = 0; i < patternSize.height; ++i)
-            for (int j = 0; j < patternSize.width; ++j)
-                obj.emplace_back(j * squareSize, i * squareSize, 0);
-
-        objectPoints.push_back(obj);
-    }
     return;
 }
 
 void gen3DCoordinate_charuco(
-    std::vector<std::vector<int>>& charucoIds,
+    std::vector<int>& charucoIds,
-    std::vector<std::vector<cv::Point2f>>& charucoCorners,
+    std::vector<cv::Point2f>& charucoCorners,
     const cv::Size& patternSize,
     const float squareSize,
-    std::vector<std::vector<cv::Point3f>>& objectPoints)
+    std::vector<cv::Point3f>& objectPoints)
 {
-    // 根据角点生成3d点
+    float ratio = 1.002;
-    for (int i = 0, i_max = (int)charucoCorners.size(); i < i_max; i ++) {
+    // 准备3D世界坐标点 (z=0)
-
+    for (int j = 0, j_max = (int)charucoCorners.size(); j < j_max; j++)
-        std::vector<cv::Point2f>& a_cornerList = charucoCorners[i];
+    {
-        std::vector<int>& a_idList = charucoIds[i];
+        int id = charucoIds[j];
-        // 准备3D世界坐标点 (z=0)
+        int id_row = id / (patternSize.width-1);
-        std::vector<cv::Point3f> obj;
+        int id_col = id % (patternSize.width-1);
-        for (int j = 0, j_max = (int)a_cornerList.size(); j < j_max; j++)
+        objectPoints.emplace_back(id_col * squareSize/ ratio, id_row *squareSize, 0);
-        {
-            int id = a_idList[j];
-            int id_row = id / (patternSize.width-1);
-            int id_col = id % (patternSize.width-1);
-            obj.emplace_back(id_col * squareSize, id_row *squareSize, 0);
-        }
-        objectPoints.push_back(obj);
     }
+
     return;
 }
 
@@ -265,7 +255,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::CALIB_FIX_ASPECT_RATIO);
+    cv::calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs); // , cv::CALIB_FIX_ASPECT_RATIO);
 #endif
     // 重投影三维点到二维图像点
     // 计算重投影误差
@@ -320,13 +310,17 @@ void monocularCalibration_chessboard(
     cv::Mat& distCoeffs,
     std::vector<double>& reprojectionError)
 {
+    // 根据角点生成3d点
     std::vector<std::vector<cv::Point3f>> objectPoints;
-    gen3DCoordinate_chessboard(
+    for (const auto& corners : imagePoints) {
-        imagePoints,
+        std::vector<cv::Point3f> obj;
-        patternSize,
+        gen3DCoordinate_chessboard(
-        squareSize,
+            corners,
-        objectPoints
+            patternSize,
-    );
+            squareSize,
+            obj);
+        objectPoints.push_back(obj);
+    }
 
     monocularCalibration(
         imagePoints,
@@ -349,13 +343,19 @@ void monocularCalibration_charuco(
     cv::Mat& distCoeffs,
     std::vector<double>& reprojectionError)
 {
+    // 根据角点生成3d点
     std::vector<std::vector<cv::Point3f>> objectPoints;
-    gen3DCoordinate_charuco(
+    for (int i = 0, i_max = (int)charucoCorners.size(); i < i_max; i++) 
-        charucoIds,
+    {
-        charucoCorners,
+        std::vector<cv::Point3f> obj;
-        patternSize,
+        gen3DCoordinate_charuco(
-        squareSize,
+            charucoIds[i],
-        objectPoints);
+            charucoCorners[i],
+            patternSize,
+            squareSize,
+            obj);
+        objectPoints.push_back(obj);
+    }
 
     monocularCalibration(
         charucoCorners,
@@ -369,21 +369,12 @@ void monocularCalibration_charuco(
 
 /*Brief: 拟合棋盘格角点平面*/
 void fitChessboardPlane(
-    const std::vector<cv::Point2f>& corners,
+    std::vector<cv::Point2f>& corners,
+    std::vector<cv::Point3f>& objectPoints,
     const cv::Mat& cameraMatrix,
     const cv::Mat& distCoeffs,
-    const cv::Size& patternSize,
-    const float squareSize,
     cv::Vec4f& planeEquation)
 {
-    // 1. 生成3D世界坐标点 (z=0)
-    std::vector<cv::Point3f> objectPoints;
-    for (int i = 0; i < patternSize.height; ++i) {
-        for (int j = 0; j < patternSize.width; ++j) {
-            objectPoints.emplace_back(j * squareSize, i * squareSize, 0);
-        }
-    }
-
     // 2. 解算PnP获取棋盘格位姿
     cv::Mat rvec, tvec;
     cv::solvePnP(objectPoints, corners, cameraMatrix, distCoeffs, rvec, tvec);
@@ -412,6 +403,58 @@ void fitChessboardPlane(
     return;
 }
 
+/*Brief: 拟合棋盘格角点平面*/
+void fitChessboardPlane_chessboard(
+    std::vector<cv::Point2f>& corners,
+    const cv::Mat& cameraMatrix,
+    const cv::Mat& distCoeffs,
+    const cv::Size& patternSize,
+    const float squareSize,
+    cv::Vec4f& planeEquation)
+{
+    std::vector<cv::Point3f> objectPoints;
+    gen3DCoordinate_chessboard(
+        corners,
+        patternSize,
+        squareSize,
+        objectPoints);
+
+    fitChessboardPlane(
+        corners,
+        objectPoints,
+        cameraMatrix,
+        distCoeffs,
+        planeEquation);
+    return;
+}
+
+/*Brief: 拟合二维码标定板角点平面*/
+void fitChessboardPlane_charuco(
+    std::vector<int>& charucoIds,
+    std::vector<cv::Point2f>& corners,
+    const cv::Mat& cameraMatrix,
+    const cv::Mat& distCoeffs,
+    const cv::Size& patternSize,
+    const float squareSize,
+    cv::Vec4f& planeEquation)
+{
+    std::vector<cv::Point3f> objectPoints;
+    gen3DCoordinate_charuco(
+        charucoIds,
+        corners,
+        patternSize,
+        squareSize,
+        objectPoints);
+
+    fitChessboardPlane(
+        corners,
+        objectPoints,
+        cameraMatrix,
+        distCoeffs,
+        planeEquation);
+    return;
+}
+
 /*Brief: 构造激光出射平面方程*/
 void generateLaserLine(
     const float baseLine,

camCalib/sourceCode/MonoLaserCalibrate.h

@@ -49,8 +49,18 @@ void monocularCalibration_charuco(
     std::vector<double>& reprojectionError);
 
 /*Brief: 拟合棋盘格角点平面*/
-void fitChessboardPlane(
+void fitChessboardPlane_chessboard(
-    const std::vector<cv::Point2f>& corners,
+    std::vector<cv::Point2f>& corners,
+    const cv::Mat& cameraMatrix,
+    const cv::Mat& distCoeffs,
+    const cv::Size& patternSize,
+    const float squareSize,
+    cv::Vec4f& planeEquation);
+
+/*Brief: 拟合二维码标定板角点平面*/
+void fitChessboardPlane_charuco(
+    std::vector<int>& charucoIds,
+    std::vector<cv::Point2f>& corners,
     const cv::Mat& cameraMatrix,
     const cv::Mat& distCoeffs,
     const cv::Size& patternSize,