Merge pull request #20247 from IanMaquignaz:inverseRectification_update

Update to initInverseRectificationMap()

* update to initInverseRectificationMap() documentation

* Restructured Calib3d_InitInverseRectificationMap unit test per feedback from alalek

* whitespace

modules/calib3d/include/opencv2/calib3d.hpp

@@ -3772,15 +3772,15 @@ of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion
 computed by #stereoRectify can be passed here. If the matrix is empty, the identity transformation
 is assumed.
 @param newCameraMatrix New camera matrix \f$A'=\vecthreethree{f_x'}{0}{c_x'}{0}{f_y'}{c_y'}{0}{0}{1}\f$.
-@param size Undistorted image size.
+@param size Distorted image size.
-@param m1type Type of the first output map that can be CV_32FC1, CV_32FC2 or CV_16SC2, see #convertMaps
+@param m1type Type of the first output map. Can be CV_32FC1, CV_32FC2 or CV_16SC2, see #convertMaps
 @param map1 The first output map for #remap.
 @param map2 The second output map for #remap.
  */
 CV_EXPORTS_W
 void initInverseRectificationMap( InputArray cameraMatrix, InputArray distCoeffs,
                            InputArray R, InputArray newCameraMatrix,
-                           Size size, int m1type, OutputArray map1, OutputArray map2 );
+                           const Size& size, int m1type, OutputArray map1, OutputArray map2 );
 
 //! initializes maps for #remap for wide-angle
 CV_EXPORTS

modules/calib3d/perf/perf_undistort.cpp

@@ -16,7 +16,7 @@ PERF_TEST(Undistort, InitUndistortMap)
     SANITY_CHECK_NOTHING();
 }
 
-PERF_TEST(Undistort, InitInverseRectificationMap)
+PERF_TEST(Undistort, DISABLED_InitInverseRectificationMap)
 {
     Size size_w_h(512 + 3, 512);
     Mat k(3, 3, CV_32FC1);

modules/calib3d/src/undistort.dispatch.cpp

@@ -166,7 +166,7 @@ void initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoeffs,
 
 void initInverseRectificationMap( InputArray _cameraMatrix, InputArray _distCoeffs,
                               InputArray _matR, InputArray _newCameraMatrix,
-                              Size size, int m1type, OutputArray _map1, OutputArray _map2 )
+                              const Size& size, int m1type, OutputArray _map1, OutputArray _map2 )
 {
     // Parameters
     Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
@@ -208,25 +208,23 @@ void initInverseRectificationMap( InputArray _cameraMatrix, InputArray _distCoef
     CV_Assert( Size(3,3) == R.size() );
 
     // Init distortion vector
-    if( !distCoeffs.empty() )
+    if( !distCoeffs.empty() ){
         distCoeffs = Mat_<double>(distCoeffs);
-    else
+
-    {
+        // Fix distortion vector orientation
-        distCoeffs.create(14, 1, CV_64F);
+        if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() ) {
-        distCoeffs = 0.;
+            distCoeffs = distCoeffs.t();
+        }
     }
 
     // Validate distortion vector size
-    CV_Assert( distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) ||
+    CV_Assert(  distCoeffs.empty() || // Empty allows cv::undistortPoints to skip distortion
+                distCoeffs.size() == Size(1, 4) || distCoeffs.size() == Size(4, 1) ||
                 distCoeffs.size() == Size(1, 5) || distCoeffs.size() == Size(5, 1) ||
                 distCoeffs.size() == Size(1, 8) || distCoeffs.size() == Size(8, 1) ||
                 distCoeffs.size() == Size(1, 12) || distCoeffs.size() == Size(12, 1) ||
                 distCoeffs.size() == Size(1, 14) || distCoeffs.size() == Size(14, 1));
 
-    // Fix distortion vector orientation
-    if( distCoeffs.rows != 1 && !distCoeffs.isContinuous() )
-        distCoeffs = distCoeffs.t();
-
     // Create objectPoints
     std::vector<cv::Point2i> p2i_objPoints;
     std::vector<cv::Point2f> p2f_objPoints;

modules/calib3d/test/test_undistort.cpp

@@ -890,12 +890,87 @@ int CV_InitInverseRectificationMapTest::prepare_test_case(int test_case_idx)
 
 void CV_InitInverseRectificationMapTest::prepare_to_validation(int/* test_case_idx*/)
 {
-    cvtest::initInverseRectificationMap(test_mat[INPUT][0],
+    // Configure Parameters
-                             zero_distortion ? cv::Mat() : test_mat[INPUT][1],
+    Mat _a0 = test_mat[INPUT][0];
-                             zero_R ? cv::Mat() : test_mat[INPUT][2],
+    Mat _d0 = zero_distortion ? cv::Mat() : test_mat[INPUT][1];
-                             zero_new_cam ? test_mat[INPUT][0] : test_mat[INPUT][3],
+    Mat _R0 = zero_R ? cv::Mat() : test_mat[INPUT][2];
-                             img_size, test_mat[REF_OUTPUT][0], test_mat[REF_OUTPUT][1],
+    Mat _new_cam0 = zero_new_cam ? test_mat[INPUT][0] : test_mat[INPUT][3];
-                             test_mat[REF_OUTPUT][0].type());
+    Mat _mapx(img_size, CV_32F), _mapy(img_size, CV_32F);
+
+    double a[9], d[5]={0,0,0,0,0}, R[9]={1, 0, 0, 0, 1, 0, 0, 0, 1}, a1[9];
+    Mat _a(3, 3, CV_64F, a), _a1(3, 3, CV_64F, a1);
+    Mat _d(_d0.rows,_d0.cols, CV_MAKETYPE(CV_64F,_d0.channels()),d);
+    Mat _R(3, 3, CV_64F, R);
+    double fx, fy, cx, cy, ifx, ify, cxn, cyn;
+
+    // Camera matrix
+    CV_Assert(_a0.size() == Size(3, 3));
+    _a0.convertTo(_a, CV_64F);
+    if( !_new_cam0.empty() )
+    {
+        CV_Assert(_new_cam0.size() == Size(3, 3));
+        _new_cam0.convertTo(_a1, CV_64F);
+    }
+    else
+    {
+        _a.copyTo(_a1);
+    }
+
+    // Distortion
+    CV_Assert(_d0.empty() ||
+              _d0.size() == Size(5, 1) ||
+              _d0.size() == Size(1, 5) ||
+              _d0.size() == Size(4, 1) ||
+              _d0.size() == Size(1, 4));
+    if( !_d0.empty() )
+        _d0.convertTo(_d, CV_64F);
+
+    // Rotation
+    if( !_R0.empty() )
+    {
+        CV_Assert(_R0.size() == Size(3, 3));
+        Mat tmp;
+        _R0.convertTo(_R, CV_64F);
+    }
+
+    // Copy camera matrix
+    fx = a[0]; fy = a[4]; cx = a[2]; cy = a[5];
+
+    // Copy new camera matrix
+    ifx = a1[0]; ify = a1[4]; cxn = a1[2]; cyn = a1[5];
+
+    // Undistort
+    for( int v = 0; v < img_size.height; v++ )
+    {
+        for( int u = 0; u < img_size.width; u++ )
+        {
+            // Convert from image to pin-hole coordinates
+            double x = (u - cx)/fx;
+            double y = (v - cy)/fy;
+
+            // Undistort
+            double x2 = x*x, y2 = y*y;
+            double r2 = x2 + y2;
+            double cdist = 1./(1 + (d[0] + (d[1] + d[4]*r2)*r2)*r2); // (1 + (d[5] + (d[6] + d[7]*r2)*r2)*r2) == 1 as d[5-7]=0;
+            double x_ = x*cdist - d[2]*2*x*y + d[3]*(r2 + 2*x2);
+            double y_ = y*cdist - d[3]*2*x*y + d[2]*(r2 + 2*y2);
+
+            // Rectify
+            double X = R[0]*x_ + R[1]*y_ + R[2];
+            double Y = R[3]*x_ + R[4]*y_ + R[5];
+            double Z = R[6]*x_ + R[7]*y_ + R[8];
+            double x__ = X/Z;
+            double y__ = Y/Z;
+
+            // Convert from pin-hole to image coordinates
+            _mapy.at<float>(v, u) = (float)(y__*ify + cyn);
+            _mapx.at<float>(v, u) = (float)(x__*ifx + cxn);
+        }
+    }
+
+    // Convert
+    _mapx.convertTo(test_mat[REF_OUTPUT][0], test_mat[REF_OUTPUT][0].type());
+    _mapy.convertTo(test_mat[REF_OUTPUT][1], test_mat[REF_OUTPUT][0].type());
 }
 
 void CV_InitInverseRectificationMapTest::run_func()
@@ -918,7 +993,7 @@ double CV_InitInverseRectificationMapTest::get_success_error_level( int /*test_c
 TEST(Calib3d_DefaultNewCameraMatrix, accuracy) { CV_DefaultNewCameraMatrixTest test; test.safe_run(); }
 TEST(Calib3d_UndistortPoints, accuracy) { CV_UndistortPointsTest test; test.safe_run(); }
 TEST(Calib3d_InitUndistortRectifyMap, accuracy) { CV_InitUndistortRectifyMapTest test; test.safe_run(); }
-TEST(Calib3d_InitInverseRectificationMap, accuracy) { CV_InitInverseRectificationMapTest test; test.safe_run(); }
+TEST(DISABLED_Calib3d_InitInverseRectificationMap, accuracy) { CV_InitInverseRectificationMapTest test; test.safe_run(); }
 
 ////////////////////////////// undistort /////////////////////////////////
 

modules/ts/src/ts_func.cpp

@@ -2881,86 +2881,6 @@ void initUndistortMap( const Mat& _a0, const Mat& _k0, const Mat& _R0, const Mat
     _mapy.convertTo(__mapy, map_type);
 }
 
-void initInverseRectificationMap( const Mat& _a0, const Mat& _k0, const Mat& _R0, const Mat& _new_cam0, Size sz, Mat& __mapx, Mat& __mapy, int map_type )
-{
-    Mat _mapx(sz, CV_32F), _mapy(sz, CV_32F);
-
-    double a[9], k[5]={0,0,0,0,0}, iR[9]={1, 0, 0, 0, 1, 0, 0, 0, 1}, a1[9];
-    Mat _a(3, 3, CV_64F, a), _a1(3, 3, CV_64F, a1);
-    Mat _k(_k0.rows,_k0.cols, CV_MAKETYPE(CV_64F,_k0.channels()),k);
-    Mat _iR(3, 3, CV_64F, iR);
-    double fx, fy, cx, cy, ifx, ify, cxn, cyn;
-
-    // Camera matrix
-    CV_Assert(_a0.size() == Size(3, 3));
-    _a0.convertTo(_a, CV_64F);
-    if( !_new_cam0.empty() )
-    {
-        CV_Assert(_new_cam0.size() == Size(3, 3));
-        _new_cam0.convertTo(_a1, CV_64F);
-    }
-    else
-    {
-        _a.copyTo(_a1);
-    }
-
-    // Distortion
-    CV_Assert(_k0.empty() ||
-              _k0.size() == Size(5, 1) ||
-              _k0.size() == Size(1, 5) ||
-              _k0.size() == Size(4, 1) ||
-              _k0.size() == Size(1, 4));
-    if( !_k0.empty() )
-        _k0.convertTo(_k, CV_64F);
-
-    // Rotation
-    if( !_R0.empty() )
-    {
-        CV_Assert(_R0.size() == Size(3, 3));
-        Mat tmp;
-        _R0.convertTo(_iR, CV_64F);
-        //invert(tmp, _iR, DECOMP_LU);
-    }
-
-    // Copy camera matrix
-    fx = a[0]; fy = a[4]; cx = a[2]; cy = a[5];
-
-    // Copy new camera matrix
-    ifx = a1[0]; ify = a1[4]; cxn = a1[2]; cyn = a1[5];
-
-    // Undistort
-    for( int v = 0; v < sz.height; v++ )
-    {
-        for( int u = 0; u < sz.width; u++ )
-        {
-            // Convert from image to pin-hole coordinates
-            double x = (u - cx)/fx;
-            double y = (v - cy)/fy;
-
-            // Undistort
-            double x2 = x*x, y2 = y*y;
-            double r2 = x2 + y2;
-            double cdist = 1./(1 + (k[0] + (k[1] + k[4]*r2)*r2)*r2); // (1 + (k[5] + (k[6] + k[7]*r2)*r2)*r2) == 1 as K[5-7]=0;
-            double x_ = x*cdist - k[2]*2*x*y + k[3]*(r2 + 2*x2);
-            double y_ = y*cdist - k[3]*2*x*y + k[2]*(r2 + 2*y2);
-
-            // Rectify
-            double X = iR[0]*x_ + iR[1]*y_ + iR[2];
-            double Y = iR[3]*x_ + iR[4]*y_ + iR[5];
-            double Z = iR[6]*x_ + iR[7]*y_ + iR[8];
-            double x__ = X/Z;
-            double y__ = Y/Z;
-
-            // Convert from pin-hole to image coordinates
-            _mapy.at<float>(v, u) = (float)(y__*ify + cyn);
-            _mapx.at<float>(v, u) = (float)(x__*ifx + cxn);
-        }
-    }
-
-    _mapx.convertTo(__mapx, map_type);
-    _mapy.convertTo(__mapy, map_type);
-}
-
 std::ostream& operator << (std::ostream& out, const MatInfo& m)
 {
     if( !m.m || m.m->empty() )