添加二维码标定流程

camCalib/camCalib.cpp

@@ -8,7 +8,7 @@
 #include "sourceCode/MonoLaserCalibrate.h"
 #include "sourceCode/FitMapParam.h"
 
-#define _DO_CAMERA_CALIB    0
+#define _DO_CAMERA_CALIB    1
 
 void sg_outputCalibK(const char* fileName, cv::Mat& fitMap)
 {
@@ -88,7 +88,7 @@ void sg_readCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
             for (int _c = 0; _c < 3; _c++)
                 K.ptr<double>(line)[_c] = data[_c];
         }
-        else
+        else if(line == 3)
         {
             double data[5];
             sscanf_s(linedata.c_str(), "%lf, %lf, %lf, %lf, %lf", &data[0], &data[1], &data[2], &data[3], &data[4]);
@@ -180,10 +180,11 @@ int main()
 
     for(int grp = 0; grp < CALIB_TEST_GROUP; grp ++)
     {
-        grp = 1;
+        grp = 3;
         int calibType = boardType[grp];
         cv::Size cbPattern;
         float cbSquareSize;
+        float markSize;
         if (CALIB_CHESS_BOARD == calibType)
         {
             cbPattern = cv::Size(8, 11); // 10);
@@ -196,13 +197,15 @@ int main()
         }
         else if (CALIB_CHARUCO == calibType)
         {
-            cbPattern = cv::Size(20, 46); // 10);
+            cbPattern = cv::Size(47, 21); // 10);
-            cbSquareSize = 50.0f; // 10.f;
+            cbSquareSize = 0.05f;
+            markSize = 0.037f;
         }
         else
             continue;
 
         std::vector<std::vector<cv::Point2f>> cbCornersList;
+        std::vector<std::vector<int>> cbCornersIdList;
         cv::Size imageSize;
 
         int startIndex = fileIdx[grp].nMin;
@@ -237,6 +240,9 @@ int main()
                     cv::imwrite(filename, color);
                 }
 #endif
+                if (corners.empty())
+                    continue;
+                cbCornersList.push_back(corners);
             }
             else if (CALIB_CIRCLE_GRID == calibType)
             {
@@ -251,26 +257,55 @@ int main()
                     cv::imwrite(filename, color);
                 }
 #endif
-
+                if (corners.empty())
+                    continue;
+                cbCornersList.push_back(corners);
             }
             else if (CALIB_CHARUCO == calibType)
             {
+                std::vector<int> markerIds;
+                std::vector<std::vector<cv::Point2f> > markerCorners;
+                std::vector<int> charucoIds;
+                detectCharucoCorners(img,
+                    cbPattern,
+                    cbSquareSize,
+                    markSize,
+                    markerIds,
+                    markerCorners,
+                    charucoIds,
+                    corners);
+                cv::Mat imageCopy = img.clone();
+                if (markerIds.size() > 0)
+                {
+                    cv::aruco::drawDetectedMarkers(imageCopy, markerCorners, markerIds);
                 }
+                if (charucoIds.size() > 0)
+                {
+                    cv::aruco::drawDetectedCornersCharuco(imageCopy, corners, charucoIds, cv::Scalar(0, 255, 255));
+                }
+                char markFilename[256];
+                sprintf_s(markFilename, "%scalib_%03d_markers.bmp", calibDataPath[grp], index);
+                cv::imwrite(markFilename, imageCopy);
+
                 if (corners.empty())
                     continue;
-
-            imageSize = cv::Size(img.cols, img.rows);
                 cbCornersList.push_back(corners);
+                cbCornersIdList.push_back(charucoIds);
+            }
+            imageSize = cv::Size(img.cols, img.rows);
         }
 
         cv::Mat K, D;
         std::vector<double> reprojectionError;
-        monocularCalibration(cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError);
+        if (CALIB_CHARUCO == calibType)
+            monocularCalibration_charuco(cbCornersIdList, cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError);
+        else
+            monocularCalibration_chessboard(cbCornersList, imageSize, cbPattern, cbSquareSize, K, D, reprojectionError);
         for(int i = 0; i < reprojectionError.size(); i ++)
             std::cout << reprojectionError[i] << std::endl;
 
         // 输出映射类型，通常使用CV_32FC1或CV_16SC2
-        cv::Mat map_x, map_y;
+        cv::Mat backwardMap_x, backwardMap_y;
         cv::Mat newCamMatrix;
         // 生成畸变矫正映射
 #if ENABLE_FISH_EYE
@@ -279,12 +314,12 @@ int main()
         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);
-        initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, map_x, map_y);
+        initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, backwardMap_x, backwardMap_y);
 #endif
 
         // 生成系数表
-        cv::Mat fitMap_x = GetFitParamMap(map_x, 1);
+        cv::Mat fitMap_x = GetFitParamMap(backwardMap_x, 1);
-        cv::Mat fitMap_y = GetFitParamMap(map_y, 1);
+        cv::Mat fitMap_y = GetFitParamMap(backwardMap_y, 1);
         //输出系数文件
         char calibParamName[256];
         sprintf_s(calibParamName, "%scalib_param_x.txt", calibDataPath[grp]);
@@ -298,8 +333,8 @@ int main()
         //搜索最大和平均误差
             // 计算绝对差异
         cv::Mat diff_x, diff_y;
-        cv::absdiff(map_x, mapGen_x, diff_x);
+        cv::absdiff(backwardMap_x, mapGen_x, diff_x);
-        cv::absdiff(map_y, mapGen_y, diff_y);
+        cv::absdiff(backwardMap_y, mapGen_y, diff_y);
         // 查找最大值和最小值
         double minVal_x, maxVal_x;
         cv::minMaxLoc(diff_x, &minVal_x, &maxVal_x);
@@ -325,7 +360,7 @@ int main()
             cv::Mat img;
             cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
             cv::Mat calibImg;
-            remap(img,
+            cv::remap(img,
                 calibImg,
                 mapGen_x,
                 mapGen_y,
@@ -341,6 +376,14 @@ int main()
         sprintf_s(calibKDName, "%scalib_param_K_D.txt", calibDataPath[grp]);
         cv::Mat K, D;
         sg_readCalibKD(calibKDName, K, D);
+
+        //生成opencv校正表
+        cv::Mat backwardMap_x, backwardMap_y;
+        double alpha = 0.4; // 0.4;
+        imageSize = cv::Size(1200, 2048);
+        cv::Mat newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
+        cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, backwardMap_x, backwardMap_y);
+
 #endif
 
 #if ENABLE_GEN_IMAGE
@@ -392,11 +435,37 @@ int main()
             cv::Mat img;
             cv::rotate(srcImg, img, cv::ROTATE_90_COUNTERCLOCKWISE);
             std::vector<cv::Point2f> corners;
+            if (CALIB_CHESS_BOARD == calibType)
+            {
                 detectCorners(img, cbPattern, corners);
+            }
+            else if (CALIB_CIRCLE_GRID == calibType)
+            {
+                detectCirclePoints(img, cbPattern, corners);
+            }
+            else if (CALIB_CHARUCO == calibType)
+            {
+                std::vector<int> markerIds;
+                std::vector<std::vector<cv::Point2f> > markerCorners;
+                std::vector<int> charucoIds;
+                detectCharucoCorners(img,
+                    cbPattern,
+                    cbSquareSize,
+                    markSize,
+                    markerIds,
+                    markerCorners,
+                    charucoIds,
+                    corners);
+            }
             if (corners.empty())
                 continue;
         
             // 创建棋盘格区域的掩码
+            cv::Mat chessMask;
+            if (CALIB_CHARUCO == calibType)
+                chessMask = cv::Mat::ones(img.size(), CV_8UC1);
+            else
+            {
                 cv::Mat chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
                 // 使用多边形近似来填充角点之间的区域
                 // 棋盘格区域需要比角点区域大一圈
@@ -458,6 +527,7 @@ int main()
                 sprintf_s(filename, "%schessMask_%03d.png", calibDataPath[grp], index);
                 cv::imwrite(filename, chessMask);
 #endif
+            }
 
             cv::Vec4f pe;
             fitChessboardPlane(corners, K, D, cbPattern, cbSquareSize, pe);
@@ -475,19 +545,20 @@ int main()
 #if 1
             sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
             cv::imwrite(filename, laserImg);
-/*
+
-            cv::Mat calibImg;
+            cv::Mat laserCalibImg;
-            remap(laserImg,
+            cv::remap(laserImg,
-                calibImg,
+                laserCalibImg,
-                mapGen_x,
+                backwardMap_x,
-                mapGen_y,
+                backwardMap_y,
                 cv::INTER_LINEAR,
                 cv::BORDER_CONSTANT,
                 cv::Scalar(0, 0, 0));
+            cv::Size laserImgSize = laserCalibImg.size();
+            laserImgSize.width = laserImgSize.width * 5;
+            cv::resize(laserCalibImg, laserCalibImg, laserImgSize, 0, 0, cv::INTER_NEAREST);
             sprintf_s(filename, "%slaser_%03d_calib.bmp", calibDataPath[grp], index);
-            cv::imwrite(filename, calibImg);
+            cv::imwrite(filename, laserCalibImg);
-*/
-
 #endif
             std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
             //显示亚像素点
@@ -530,9 +601,8 @@ int main()
         //output K and D
         char calibKDPName[256];
         sprintf_s(calibKDPName, "%scalib_param_K_D.txt", calibDataPath[grp]);
-        sg_outputCalibKD(calibKDName, K, D, pe);
+        sg_outputCalibKD(calibKDPName, K, D, pe);
     }
-
     return 0;
 }
 

camCalib/sourceCode/MonoLaserCalibrate.cpp

@@ -2,6 +2,8 @@
 //
 #include <iostream>
 #include <opencv2/opencv.hpp>
+#include "aruco.hpp"
+#include "aruco/charuco.hpp"
 #include <vector>
 #include <math.h>
 #include "MonoLaserCalibrate.h"
@@ -158,18 +160,53 @@ void detectCirclePoints(const cv::Mat& img,
     return;
 }
 
-/*Breif：单目相机标定函数*/
+/*Breif：二维码标定板角点函数*/
-void monocularCalibration(
+void detectCharucoCorners(const cv::Mat& img,
+    const cv::Size& patternSize,
+    float sqSize,
+    float mkSize,
+    std::vector<int>& markerIds,
+    std::vector<std::vector<cv::Point2f> >& markerCorners,
+    std::vector<int>& charucoIds,
+    std::vector<cv::Point2f>& charucoCorners)
+{
+    cv::Mat gray;
+    if (img.channels() == 3)
+        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
+    else
+        gray = img.clone();
+
+    cv::aruco::Dictionary dictionary(cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000));
+    cv::aruco::CharucoBoard board = cv::aruco::CharucoBoard(patternSize, sqSize, mkSize, dictionary);
+    cv::aruco::DetectorParameters params = cv::aruco::DetectorParameters();
+    params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE;
+
+    cv::Ptr<cv::aruco::Dictionary> ptrDictionary = cv::makePtr<cv::aruco::Dictionary>(dictionary);
+    cv::Ptr<cv::aruco::DetectorParameters> ptrParams = cv::makePtr<cv::aruco::DetectorParameters>(params);
+    cv::aruco::detectMarkers(gray, ptrDictionary, markerCorners, markerIds, ptrParams);
+
+    // if at least one marker detected
+    if (markerIds.size() > 0) {
+        //cv::aruco::drawDetectedMarkers(imageCopy, markerCorners, markerIds);
+        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 (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));
+    }
+    return;
+}
+
+void gen3DCoordinate_chessboard(
     const std::vector<std::vector<cv::Point2f>>& imagePoints, 
-    const cv::Size& imageSize,
     const cv::Size& patternSize,
     const float squareSize,
-    cv::Mat& cameraMatrix,
+    std::vector<std::vector<cv::Point3f>>& objectPoints
-    cv::Mat& distCoeffs,
+    )
-    std::vector<double>& reprojectionError)
 {
     // 根据角点生成3d点
-    std::vector<std::vector<cv::Point3f>> objectPoints;
     for (const auto& corners : imagePoints) {
 
         // 准备3D世界坐标点 (z=0)
@@ -180,7 +217,44 @@ void monocularCalibration(
 
         objectPoints.push_back(obj);
     }
+    return;
+}
 
+void gen3DCoordinate_charuco(
+    std::vector<std::vector<int>>& charucoIds,
+    std::vector<std::vector<cv::Point2f>>& charucoCorners,
+    const cv::Size& patternSize,
+    const float squareSize,
+    std::vector<std::vector<cv::Point3f>>& objectPoints)
+{
+    // 根据角点生成3d点
+    for (int i = 0, i_max = (int)charucoCorners.size(); i < i_max; i ++) {
+
+        std::vector<cv::Point2f>& a_cornerList = charucoCorners[i];
+        std::vector<int>& a_idList = charucoIds[i];
+        // 准备3D世界坐标点 (z=0)
+        std::vector<cv::Point3f> obj;
+        for (int j = 0, j_max = (int)a_cornerList.size(); j < j_max; j++)
+        {
+            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;
+}
+
+/*Breif：单目相机标定函数*/
+void monocularCalibration(
+    const std::vector<std::vector<cv::Point2f>>& imagePoints,
+    const cv::Size& imageSize,
+    std::vector<std::vector<cv::Point3f>>& objectPoints,
+    cv::Mat& cameraMatrix,
+    cv::Mat& distCoeffs,
+    std::vector<double>& reprojectionError)
+{
     // 执行相机标定
     std::vector<cv::Mat> rvecs, tvecs;
 #if ENABLE_FISH_EYE
@@ -236,6 +310,63 @@ void monocularCalibration(
     return;
 }
 
+/*Breif：棋盘格单目相机标定函数*/
+void monocularCalibration_chessboard(
+    const std::vector<std::vector<cv::Point2f>>& imagePoints,
+    const cv::Size& imageSize,
+    const cv::Size& patternSize,
+    const float squareSize,
+    cv::Mat& cameraMatrix,
+    cv::Mat& distCoeffs,
+    std::vector<double>& reprojectionError)
+{
+    std::vector<std::vector<cv::Point3f>> objectPoints;
+    gen3DCoordinate_chessboard(
+        imagePoints,
+        patternSize,
+        squareSize,
+        objectPoints
+    );
+
+    monocularCalibration(
+        imagePoints,
+        imageSize,
+        objectPoints,
+        cameraMatrix,
+        distCoeffs,
+        reprojectionError);
+    return;
+}
+
+/*Breif：二维码标定板单目相机标定函数*/
+void monocularCalibration_charuco(
+    std::vector<std::vector<int>>& charucoIds,
+    std::vector<std::vector<cv::Point2f>>& charucoCorners,
+    const cv::Size& imageSize,
+    const cv::Size& patternSize,
+    const float squareSize,
+    cv::Mat& cameraMatrix,
+    cv::Mat& distCoeffs,
+    std::vector<double>& reprojectionError)
+{
+    std::vector<std::vector<cv::Point3f>> objectPoints;
+    gen3DCoordinate_charuco(
+        charucoIds,
+        charucoCorners,
+        patternSize,
+        squareSize,
+        objectPoints);
+
+    monocularCalibration(
+        charucoCorners,
+        imageSize,
+        objectPoints,
+        cameraMatrix,
+        distCoeffs,
+        reprojectionError);
+    return;
+}
+
 /*Brief: 拟合棋盘格角点平面*/
 void fitChessboardPlane(
     const std::vector<cv::Point2f>& corners,

camCalib/sourceCode/MonoLaserCalibrate.h

@@ -17,8 +17,18 @@ void detectCirclePoints(const cv::Mat& img,
     const cv::Size& patternSize,
     std::vector<cv::Point2f>& corners);
 
-/*Breif：单目相机标定函数*/
+/*Breif：二维码标定板角点函数*/
-void monocularCalibration(
+void detectCharucoCorners(const cv::Mat& img,
+    const cv::Size& patternSize,
+    float sqSize,
+    float mkSize,
+    std::vector<int>& markerIds,
+    std::vector<std::vector<cv::Point2f> >& markerCorners,
+    std::vector<int>& charucoIds,
+    std::vector<cv::Point2f>& charucoCorners);
+
+/*Breif：棋盘格单目相机标定函数*/
+void monocularCalibration_chessboard(
     const std::vector<std::vector<cv::Point2f>>& imagePoints,
     const cv::Size& imageSize,
     const cv::Size& patternSize,
@@ -27,6 +37,17 @@ void monocularCalibration(
     cv::Mat& distCoeffs,
     std::vector<double>& reprojectionError);
 
+/*Breif：二维码标定板单目相机标定函数*/
+void monocularCalibration_charuco(
+    std::vector<std::vector<int>>& charucoIds,
+    std::vector<std::vector<cv::Point2f>>& charucoCorners,
+    const cv::Size& imageSize,
+    const cv::Size& patternSize,
+    const float squareSize,
+    cv::Mat& cameraMatrix,
+    cv::Mat& distCoeffs,
+    std::vector<double>& reprojectionError);
+
 /*Brief: 拟合棋盘格角点平面*/
 void fitChessboardPlane(
     const std::vector<cv::Point2f>& corners,