提交前向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
 
         // 生成系数表