提交前向Map函数

camCalib/camCalib.cpp

@@ -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,14 +341,14 @@ 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);
         }
 
@@ -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;