提交前向Map函数

camCalib/camCalib.cpp

@@ -40,7 +40,7 @@ void sg_outputCalibKD(const char* fileName, cv::Mat& K, cv::Mat& D)
         double temp[3];
         for (int _c = 0; _c < 3; _c++)
             temp[_c] = K.ptr<double>(i)[_c];
-       
+
         //sprintf_s(dataStr, 250, "%lf, %lf, %lf, %lf, %lf, %lf, %lf, %lf", K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7]);
         sprintf_s(dataStr, 250, "%g, %g, %g", temp[0], temp[1], temp[2]);
         sw << dataStr << std::endl;
@@ -97,6 +97,33 @@ typedef struct
 #define CALIB_CIRCLE_GRID   2
 #define CALIB_CHARUCO       3
 
+void initForwardRectMap(const cv::Mat& K, const cv::Mat& D, const cv::Mat& R,
+    const cv::Mat& newK, const cv::Size& size, cv::Mat& mapX, cv::Mat& mapY) {
+
+    std::vector<cv::Point2f> srcPts;
+    for (int r = 0; r < size.height; r++) {
+        for (int c = 0; c < size.width; c++) {
+            srcPts.push_back(cv::Point2f(c, r));
+        }
+    }
+
+    std::vector<cv::Point2f> dstPts;
+    cv::undistortPoints(srcPts, dstPts, K, D, R, newK);
+
+    mapX = cv::Mat::zeros(size.height, size.width, CV_32FC1);
+    mapY = cv::Mat::zeros(size.height, size.width, CV_32FC1);
+    int idx = 0;
+    for (int r = 0; r < size.height; r++) {
+        for (int c = 0; c < size.width; c++) {
+            mapX.ptr<float>(r)[c] = dstPts[idx].x;
+            mapY.ptr<float>(r)[c] = dstPts[idx].y;
+            idx++;
+        }
+    }
+
+    return;
+}
+
 #define CALIB_TEST_GROUP 4
 int main()
 {
@@ -219,7 +246,8 @@ 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, map_x, map_y);
+        initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, map_x, map_y);
 #endif
 
         // 生成系数表
@@ -289,7 +317,7 @@ int main()
         sg_readCalibKD(calibKDName, K, D);
 #endif
 
- #if ENABLE_GEN_IMAGE
+#if ENABLE_GEN_IMAGE
         cv::Vec4f laserPE;
         generateLaserLine(10.f, 5.f, laserPE);
         std::cout << "generateLaserLine pe: " << laserPE << std::endl;
@@ -313,17 +341,17 @@ int main()
 
             cv::Mat image = generateVirtualLaserLineImage(laserPE, pe, K, D, imageSize);
 
-    #if ENABLE_DEBUG
+#if ENABLE_DEBUG
             cv::Mat color = image.clone();
             cv::resize(color, color, cv::Size(), 0.5, 0.5);
             cv::imshow("image", color);
             cv::waitKey(10);·
 #endif
 
-            sprintf_s(filename, "%s%d.bmp", laserImagePath, index);
+                sprintf_s(filename, "%s%d.bmp", laserImagePath, index);
             cv::imwrite(filename, image);
         }
-    
+
 #endif
 
         {
@@ -421,10 +449,10 @@ int main()
                 //与Mask相与，保证待处理的激光线在标定板上
                 cv::Mat laserImg;
                 cv::bitwise_and(laserImg_unMask, laserImg_unMask, laserImg, chessMask);
-    #if 1
+#if 1
                 sprintf_s(filename, "%slaser_rotate_mask_%03d.png", calibDataPath[grp], index);
                 cv::imwrite(filename, laserImg);
-    #endif
+#endif
                 std::vector<cv::Point2f> pts2d = detectLaserLine(laserImg);
                 //显示亚像素点
                 cv::Mat enlargeImg;