代码同步

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;	//< 最小值
@@ -103,13 +135,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_git\\camCalib\\camCalibData\\撕裂原理相机标定图像\\",  //0
-        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\chessboard\\",   //1
+        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\chessboard\\",   //1
-        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\circlePoint\\", //2
+        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo_git\\camCalib\\camCalibData\\circlePoint\\", //2
-        "F:\\ShangGu\\ProductDev\\三角光相机\\相机开发\\CamAlgo\\camCalib\\camCalibData\\charuCo\\", //3
+        "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] =
     {
@@ -121,7 +153,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;
@@ -255,9 +287,7 @@ int main()
         std::cout << "Y Mean difference: " << meanVal_y[0] << std::endl;
 
         //生成矫正图像
-        index = 3;
+        for (index = startIndex; index <= endIndex; index++) {
-        for (;; index++) {
-
             char filename[256];
             sprintf_s(filename, "%scalib_%03d.bmp", calibDataPath[grp], index);
             cv::Mat srcImg = cv::imread(filename);
@@ -278,10 +308,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);
@@ -326,11 +352,8 @@ int main()
     
 #endif
 
-        {
         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);
@@ -424,6 +447,18 @@ int main()
 #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);
             //显示亚像素点
@@ -436,10 +471,14 @@ int main()
             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 = 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;
@@ -449,13 +488,20 @@ int main()
             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;