camCalib

version 1.0.0 : base version released

CamAlgo.sln

@@ -1,7 +1,7 @@
 
 Microsoft Visual Studio Solution File, Format Version 12.00
-# Visual Studio Version 16
+# Visual Studio Version 17
-VisualStudioVersion = 16.0.33027.164
+VisualStudioVersion = 17.14.37411.7 d17.14
 MinimumVisualStudioVersion = 10.0.40219.1
 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "PeakSearch", "CamAlgo\CamAlgo.vcxproj", "{2B6D42C0-7146-47D2-B6AA-9BC125C4CF93}"
 EndProject
@@ -18,6 +18,8 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camAlgoSW_test", "camAlgoSW
 EndProject
 Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "camCalib", "camCalib\camCalib.vcxproj", "{3C3A4670-25E6-4E17-9723-2897316D2437}"
 EndProject
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "simDataGenerator", "simDataGenerator\simDataGenerator.vcxproj", "{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}"
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|x64 = Debug|x64
@@ -74,6 +76,14 @@ Global
 		{3C3A4670-25E6-4E17-9723-2897316D2437}.Release|x64.Build.0 = Release|x64
 		{3C3A4670-25E6-4E17-9723-2897316D2437}.Release|x86.ActiveCfg = Release|Win32
 		{3C3A4670-25E6-4E17-9723-2897316D2437}.Release|x86.Build.0 = Release|Win32
+		{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Debug|x64.ActiveCfg = Debug|x64
+		{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Debug|x64.Build.0 = Debug|x64
+		{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Debug|x86.ActiveCfg = Debug|Win32
+		{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Debug|x86.Build.0 = Debug|Win32
+		{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Release|x64.ActiveCfg = Release|x64
+		{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Release|x64.Build.0 = Release|x64
+		{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Release|x86.ActiveCfg = Release|Win32
+		{D6675A67-4BD9-43DF-90E9-89EE7784C2A8}.Release|x86.Build.0 = Release|Win32
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE

camCalib/camCalib.cpp

@@ -10,6 +10,13 @@
 
 #define _DO_CAMERA_CALIB    1
 
+//version 1.0.0 : base version released
+std::string	m_strVersion = "camCalib 1.0.0";
+const char* wd_camCalibVersion(void)
+{
+	return m_strVersion.c_str();
+}
+
 void sg_outputCalibK(const char* fileName, cv::Mat& fitMap)
 {
     std::ofstream sw(fileName);
@@ -157,6 +164,67 @@ void initForwardRectMap(const cv::Mat& K, const cv::Mat& D, const cv::Mat& R,
     return;
 }
 
+void lineFitting(std::vector< cv::Point2f>& inliers, double* _k, double* _b)
+{
+    //最小二乘拟合直线参数
+    double xx_sum = 0;
+    double x_sum = 0;
+    double y_sum = 0;
+    double xy_sum = 0;
+    int num = 0;
+    for (int i = 0; i < inliers.size(); i++)
+    {
+        x_sum += inliers[i].x; //x的累加和
+        y_sum += inliers[i].y; //y的累加和
+        xx_sum += inliers[i].x * inliers[i].x; //x的平方累加和
+        xy_sum += inliers[i].x * inliers[i].y; //x，y的累加和
+        num++;
+    }
+    *_k = (num * xy_sum - x_sum * y_sum) / (num * xx_sum - x_sum * x_sum); //根据公式求解k
+    *_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//根据公式求解b
+}
+//拟合成通用直线方程ax+by+c=0，包括垂直
+void lineFitting_abc(std::vector< cv::Point2f>& inliers, double* _a, double* _b, double* _c)
+{
+    //判断是否为垂直
+    int dataSize = (int)inliers.size();
+    if (dataSize < 2)
+        return;
+
+    double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x);
+    double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y);
+    std::vector< cv::Point2f> fittingData;
+    if (deltaX < deltaY)
+    {
+        //x=ky+b 拟合
+        for (int i = 0; i < dataSize; i++)
+        {
+            cv::Point2f a_fitPt;
+            a_fitPt.x = inliers[i].y;
+            a_fitPt.y = inliers[i].x;
+            fittingData.push_back(a_fitPt);
+        }
+        double k = 0, b = 0;
+        lineFitting(fittingData, &k, &b);
+        //ax+by+c
+        *_a = 1.0;
+        *_b = -k;
+        *_c = -b;
+    }
+    else
+    {
+        //y = kx+b拟合
+        double k = 0, b = 0;
+        lineFitting(inliers, &k, &b);
+        //ax+by+c
+        *_a = k;
+        *_b = -1;
+        *_c = b;
+    }
+    return;
+}
+
+
 #define MONO_CALIB_TEST_GROUP 5
 int MonoCamLaserCalib()
 {
@@ -493,7 +561,40 @@ int MonoCamLaserCalib()
             // 创建棋盘格区域的掩码
             cv::Mat chessMask;
             if (CALIB_CHARUCO == calibType)
-                chessMask = cv::Mat::ones(img.size(), CV_8UC1);
+            {
+                chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
+                float roi_L = corners[0].x;
+                float roi_R = roi_L;
+                float roi_T = corners[0].y;
+                float roi_B = roi_T;
+                for (int i = 1; i < (int)corners.size(); i++)
+                {
+                    roi_L = roi_L > corners[i].x ? corners[i].x : roi_L;
+                    roi_R = roi_R < corners[i].x ? corners[i].x : roi_R;
+                    roi_T = roi_T > corners[i].y ? corners[i].y : roi_T;
+                    roi_B = roi_B < corners[i].y ? corners[i].y : roi_B;
+                }
+                int left = (int)roi_L;
+                if (left < 0)
+                    left = 0;
+                int right = (int)roi_R;
+                if (right >= chessMask.cols)
+                    right = chessMask.cols - 1;
+                int top = (int)roi_T - 100;
+                if (top < 0)
+                    top = 0;
+                int btm = (int)roi_B + 100;
+                if (btm >= chessMask.rows)
+                    btm = chessMask.rows - 1;
+
+                for (int y = top; y <= btm; y++)
+                {
+                    for (int x = left; x <= right; x++)
+                    {
+                        chessMask.at<uchar>(y, x) = 0xff;
+                    }
+                }
+            }
             else
             {
                 chessMask = cv::Mat::zeros(img.size(), CV_8UC1);
@@ -576,7 +677,7 @@ int MonoCamLaserCalib()
             cv::Mat laserImg;
             cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
 #if 1
-            sprintf_s(filename, "%slaser_mask_%d.png", calibDataPath[grp], index);
+            sprintf_s(filename, "%sresult/laser_mask_%d.png", calibDataPath[grp], index);
             cv::imwrite(filename, laserImg);
 
             cv::Mat laserCalibImg;
@@ -587,13 +688,19 @@ int MonoCamLaserCalib()
                 cv::INTER_LINEAR,
                 cv::BORDER_CONSTANT,
                 cv::Scalar(0, 0, 0));
-            cv::Size laserImgSize = laserCalibImg.size();
+            sprintf_s(filename, "%sresult/laser_%d_calib.bmp", calibDataPath[grp], index);
-            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, laserCalibImg);
+
+            cv::Mat enlargeLaserImg = laserCalibImg.clone();
+            cv::Size laserImgSize = enlargeLaserImg.size();
+            laserImgSize.width = laserImgSize.width * 5;
+            cv::resize(enlargeLaserImg, enlargeLaserImg, laserImgSize, 0, 0, cv::INTER_NEAREST);
+            sprintf_s(filename, "%sresult/laser_%d_enlarge_calib.bmp", calibDataPath[grp], index);
+            cv::imwrite(filename, enlargeLaserImg);
 #endif
-            std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
+            bool multiPeakFlag = false; //每行一个Peak
+            std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg,  multiPeakFlag);
+#if 0
             //显示亚像素点
             cv::Mat enlargeImg;
             if (laserImg.channels() == 1)
@@ -603,38 +710,76 @@ int MonoCamLaserCalib()
             cv::Size objSize = laserImg.size();
             objSize.width = objSize.width * 5;
             cv::resize(enlargeImg, enlargeImg, objSize, 0, 0, cv::INTER_NEAREST);
+#endif
 
-            if (pts2d.size() > 0)
+            // 去畸变
+            if (pts2d.size() == 0)
+                continue;
+            std::vector<cv::Point2f> undistortedPoints;
+            cv::undistortPoints(pts2d, undistortedPoints,
+                K, D, cv::noArray(), newCamMatrix);
+
+#if 1
+            //直线拟合
+            double _a, _b, _c;
+            lineFitting_abc(undistortedPoints, &_a, &_b, &_c);
+
+            std::vector<cv::Point2f> fittingPts2d;
+            for (int i = 0; i < (int)undistortedPoints.size(); i++)
             {
-                sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpixData.txt", calibDataPath[grp], index);
+                cv::Point2f a_pt;
-                saveSubpixData(filename, pts2d);
+                a_pt.y = undistortedPoints[i].y;
+                a_pt.x = -(a_pt.y * _b + _c) / _a;
+                fittingPts2d.push_back(a_pt);
             }
-            for (int i = 0, i_max = (int)pts2d.size(); i < i_max; i++)
+#else
+            std::vector<cv::Point2f> fittingPts2d;
+            fittingPts2d.insert(fittingPts2d.end(), undistortedPoints.begin(), undistortedPoints.end());
+#endif
+
+            if (fittingPts2d.size() > 0)
             {
-                cv::Point2f a_subPix = pts2d[i];
+                sprintf_s(filename, "%sresult/laser_enlarge_%d_subpixData.txt", calibDataPath[grp], index);
+                saveSubpixData(filename, fittingPts2d);
+            }
+
+            cv::Mat cloneEnlageImg = enlargeLaserImg.clone();
+            for (int i = 0, i_max = (int)undistortedPoints.size(); i < i_max; i++)
+            {
+                cv::Point2f a_subPix = undistortedPoints[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;
+                cloneEnlageImg.at<cv::Vec3b>(row, col)[0] = 0;
-                enlargeImg.at<cv::Vec3b>(row, col)[1] = 0;
+                cloneEnlageImg.at<cv::Vec3b>(row, col)[1] = 0;
-                enlargeImg.at<cv::Vec3b>(row, col)[2] = 255;
+                cloneEnlageImg.at<cv::Vec3b>(row, col)[2] = 255;
             }
-            sprintf_s(filename, "%slaser_rotate_enlarge_%03d_subpix.png", calibDataPath[grp], index);
+            sprintf_s(filename, "%sresult/laser_enlarge_%d_raw_subpix.png", calibDataPath[grp], index);
-            cv::imwrite(filename, enlargeImg);
+            cv::imwrite(filename, cloneEnlageImg);
-            std::vector<cv::Point3f> pts3d = project2DTo3D(pts2d, pe, K, D);
-#if 1
-            //保存3D点
 
-#endif
+            for (int i = 0, i_max = (int)fittingPts2d.size(); i < i_max; i++)
+            {
+                cv::Point2f a_subPix = fittingPts2d[i];
+                int row = (int)(a_subPix.y + 0.5);
+                int col = (int)(a_subPix.x * 5 + 0.5);
+                enlargeLaserImg.at<cv::Vec3b>(row, col)[0] = 0;
+                enlargeLaserImg.at<cv::Vec3b>(row, col)[1] = 0;
+                enlargeLaserImg.at<cv::Vec3b>(row, col)[2] = 255;
+            }
+            sprintf_s(filename, "%sresult/laser_enlarge_%d_subpix.png", calibDataPath[grp], index);
+            cv::imwrite(filename, enlargeLaserImg);
+
+            std::vector<cv::Point3f> pts3d = project2DTo3D(fittingPts2d, pe, K, D);
             all_pts3d.insert(all_pts3d.end(), pts3d.begin(), pts3d.end());
+            printf("  %sImage_%d_Laser.png  ... done\n", calibDataPath[grp], index);
         }
 
-        cv::Vec4f pe = fitPlaneToPoints(all_pts3d);
+        cv::Vec4f laser_pe = fitPlaneToPoints(all_pts3d);
-        std::cout << "pe: " << pe << std::endl;
+        std::cout << "pe: " << laser_pe << std::endl;
 
         //output K and D
         char calibKDPName[256];
-        sprintf_s(calibKDPName, "%scalib_param_K_D.txt", calibDataPath[grp]);
+        sprintf_s(calibKDPName, "%sresult/calib_param_K_D.txt", calibDataPath[grp]);
-        sg_outputCalibKD(calibKDPName, K, D, pe);
+        sg_outputCalibKD(calibKDPName, K, D, laser_pe);
     }
     return 0;
 }

camCalib/camCalibData/chessboard/calib_param_K_D.txt

@@ -1,4 +1,5 @@
-1004.94, 0, 619.327
+1006.74, 0, 620.098
-0, 1004.94, 1023.07
+0, 1006.74, 1023.51
 0, 0, 1
--0.0869453, 0.0919085, -1.61443e-05, -2.84163e-05, -0.0224749
+-0.0817425, 0.0844376, -2.2634e-05, 0.000762232, -0.0165931
+-2.67733, 0.0501805, 631.944

camCalib/sourceCode/MonoLaserCalibrate.cpp

@@ -655,7 +655,7 @@ void getLinePeaks(double* lineData, int dataSize, int row, int scaleWin, double
 }
 
 std::vector<cv::Point2f> detectLaserLine(
-    const cv::Mat& inputImage)
+    const cv::Mat& inputImage, bool multiPeaksPerLine)
 {
     std::vector<cv::Point2f> laserPoints;
     if (inputImage.empty())
@@ -673,8 +673,8 @@ std::vector<cv::Point2f> detectLaserLine(
     //高斯滤波
     cv::Mat img;
     gray.convertTo(img, CV_64FC1);
-    cv::GaussianBlur(img, img, cv::Size(3, 3), 0.9, 0.9);
+    cv::GaussianBlur(img, img, cv::Size(0, 0), 2.0, 2.0);
-    //cv::imwrite("gauss_blur_src.bmp", img);
+    cv::imwrite("gauss_blur_src.bmp", img);
     // 提取最大值点
     int scaleWin = 5;
     double minPkValue = 20;
@@ -684,6 +684,23 @@ std::vector<cv::Point2f> detectLaserLine(
         std::vector< cv::Point> linePeaks;
         double* lineData = img.ptr<double>(i);
         getLinePeaks(lineData, img.cols, i, scaleWin, minPkValue, linePeaks);
+        if ((false == multiPeaksPerLine) && (linePeaks.size() > 1))
+        {
+            //每行保留一个Peak,
+            cv::Point bestOne = linePeaks[0];
+            double bestValue = lineData[bestOne.x];
+            for (int i = 1; i < (int)linePeaks.size(); i++)
+            {
+                double value = lineData[linePeaks[i].x];
+                if (bestValue < value)
+                {
+                    bestValue = value;
+                    bestOne = linePeaks[i];
+                }
+            }
+            linePeaks.clear();
+            linePeaks.push_back(bestOne);
+        }
         pkPoints.insert(pkPoints.end(), linePeaks.begin(), linePeaks.end());
     }
     //取亚像素值
@@ -701,27 +718,22 @@ std::vector<cv::Point2f> detectLaserLine(
 #endif
 /*Breif: 根据棋盘格平面和2d坐标计算3d值*/
 std::vector<cv::Point3f> project2DTo3D(
-    const std::vector<cv::Point2f>& imagePoints,
+    const std::vector<cv::Point2f>& undistortedPoints,
     const cv::Vec4f& planeEquation,  // [a,b,c,d] for ax+by+cz+d=0
     const cv::Mat& cameraMatrix,     // 相机内参K
     const cv::Mat& distCoeffs)       // 畸变系数D
 {
     std::vector<cv::Point3f> objectPoints;
-    if (imagePoints.empty()) 
+    if (undistortedPoints.empty())
         return objectPoints;
 
-    // 1. 去畸变
+    // 1. 提取平面参数
-    std::vector<cv::Point2f> undistortedPoints;
-    cv::undistortPoints(imagePoints, undistortedPoints,
-        cameraMatrix, distCoeffs, cv::noArray(), cameraMatrix);
-
-    // 2. 提取平面参数
     const float a = planeEquation[0], b = planeEquation[1];
     const float c = planeEquation[2], d = planeEquation[3];
     const float denom = a * a + b * b + c * c;
     if (fabs(denom) < 1e-6f) return objectPoints;
 
-    // 3. 构造射线并求交平面
+    // 2. 构造射线并求交平面
     const cv::Mat invK = cameraMatrix.inv();
     for (const auto& pt : undistortedPoints) {
         // 生成归一化射线方向

camCalib/sourceCode/MonoLaserCalibrate.h

@@ -69,7 +69,7 @@ void fitChessboardPlane_charuco(
 
 /*Brief: 激光线检测函数*/
 std::vector<cv::Point2f> detectLaserLine(
-    const cv::Mat& inputImage);
+    const cv::Mat& inputImage, bool multiPeaksPerLine);
 
 /*Breif: 根据棋盘格平面和2d坐标计算3d值*/
 std::vector<cv::Point3f> project2DTo3D(