imgproc: add stop criteria tuning in undistortPoints

modules/calib3d/test/test_undistort_points.cpp

@@ -92,3 +92,28 @@ void CV_UndistortTest::run(int /* start_from */)
 }
 
 TEST(Calib3d_Undistort, accuracy) { CV_UndistortTest test; test.safe_run(); }
+
+TEST(Calib3d_Undistort, stop_criteria)
+{
+    Mat cameraMatrix = (Mat_<double>(3,3,CV_64F) << 857.48296979, 0, 968.06224829,
+                                                        0, 876.71824265, 556.37145899,
+                                                        0, 0, 1);
+    Mat distCoeffs = (Mat_<double>(5,1,CV_64F) <<
+                      -2.57614020e-01, 8.77086999e-02, -2.56970803e-04, -5.93390389e-04, -1.52194091e-02);
+    RNG rng(2);
+    Point2d pt_distorted(rng.uniform(0.0, 1920.0), rng.uniform(0.0, 1080.0));
+    std::vector<Point2d> pt_distorted_vec;
+    pt_distorted_vec.push_back(pt_distorted);
+    const double maxError = 1e-6;
+    TermCriteria criteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 100, maxError);
+    std::vector<Point2d> pt_undist_vec;
+    undistortPoints(pt_distorted_vec, pt_undist_vec, cameraMatrix, distCoeffs, criteria);
+
+    std::vector<Point2d> pt_redistorted_vec;
+    std::vector<Point3d> pt_undist_vec_homogeneous;
+    pt_undist_vec_homogeneous.push_back( Point3d(pt_undist_vec[0].x, pt_undist_vec[0].y, 1.0) );
+    projectPoints(pt_undist_vec_homogeneous, Mat::zeros(3,1,CV_64F), Mat::zeros(3,1,CV_64F), cameraMatrix, distCoeffs, pt_redistorted_vec);
+    const double obtainedError = sqrt( pow(pt_distorted.x - pt_redistorted_vec[0].x, 2) + pow(pt_distorted.y - pt_redistorted_vec[0].y, 2) );
+
+    ASSERT_LE(obtainedError, maxError);
+}

modules/imgproc/include/opencv2/imgproc.hpp

@@ -3036,6 +3036,14 @@ cv::stereoRectify can be passed here. If the matrix is empty, the identity new c
 CV_EXPORTS_W void undistortPoints( InputArray src, OutputArray dst,
                                    InputArray cameraMatrix, InputArray distCoeffs,
                                    InputArray R = noArray(), InputArray P = noArray());
+/** @overload
+    @note Default version of cv::undistortPoints does 5 iterations to compute undistorted points.
+
+ */
+CV_EXPORTS_AS(undistortPointsExt) void undistortPoints( InputArray src, OutputArray dst,
+                                   InputArray cameraMatrix, InputArray distCoeffs,
+                                   TermCriteria criteria,
+                                   InputArray R = noArray(), InputArray P = noArray());
 
 //! @} imgproc_transform
 

modules/imgproc/src/undistort.cpp

@@ -294,15 +294,15 @@ cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs,
     CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
 }
 
-
-void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
+static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
                    const CvMat* _distCoeffs,
-                   const CvMat* matR, const CvMat* matP )
+                   const CvMat* matR, const CvMat* matP, cv::TermCriteria criteria)
 {
     double A[3][3], RR[3][3], k[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
     CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
     CvMat _RR=cvMat(3, 3, CV_64F, RR);
     cv::Matx33d invMatTilt = cv::Matx33d::eye();
+    cv::Matx33d matTilt = cv::Matx33d::eye();
 
     CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
         (_src->rows == 1 || _src->cols == 1) &&
@@ -316,7 +316,6 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
 
     cvConvert( _cameraMatrix, &matA );
 
-    int iters = 0;
 
     if( _distCoeffs )
     {
@@ -332,9 +331,11 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
             CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
 
         cvConvert( _distCoeffs, &_Dk );
-        iters = 5;
         if (k[12] != 0 || k[13] != 0)
+        {
             cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], NULL, NULL, NULL, &invMatTilt);
+            cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], &matTilt, NULL, NULL);
+        }
     }
 
     if( matR )
@@ -373,7 +374,7 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
     int n = _src->rows + _src->cols - 1;
     for( int i = 0; i < n; i++ )
     {
-        double x, y, x0 = 0, y0 = 0;
+        double x, y, x0 = 0, y0 = 0, u, v;
         if( stype == CV_32FC2 )
         {
             x = srcf[i*sstep].x;
@@ -384,27 +385,61 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
             x = srcd[i*sstep].x;
             y = srcd[i*sstep].y;
         }
-
+        u = x; v = y;
         x = (x - cx)*ifx;
         y = (y - cy)*ify;
 
-        if( iters ) {
+        if( _distCoeffs ) {
             // compensate tilt distortion
             cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1);
             double invProj = vecUntilt(2) ? 1./vecUntilt(2) : 1;
             x0 = x = invProj * vecUntilt(0);
             y0 = y = invProj * vecUntilt(1);
-        }
 
-        // compensate distortion iteratively
-        for( int j = 0; j < iters; j++ )
-        {
-            double r2 = x*x + y*y;
-            double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
-            double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2;
-            double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2;
-            x = (x0 - deltaX)*icdist;
-            y = (y0 - deltaY)*icdist;
+            double error = std::numeric_limits<double>::max();
+            // compensate distortion iteratively
+
+            for( int j = 0; ; j++ )
+            {
+                if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
+                    break;
+                if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)
+                    break;
+                double r2 = x*x + y*y;
+                double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
+                double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2;
+                double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2;
+                x = (x0 - deltaX)*icdist;
+                y = (y0 - deltaY)*icdist;
+
+                if(criteria.type & cv::TermCriteria::EPS)
+                {
+                    double r4, r6, a1, a2, a3, cdist, icdist2;
+                    double xd, yd, xd0, yd0;
+                    cv::Vec3d vecTilt;
+
+                    r2 = x*x + y*y;
+                    r4 = r2*r2;
+                    r6 = r4*r2;
+                    a1 = 2*x*y;
+                    a2 = r2 + 2*x*x;
+                    a3 = r2 + 2*y*y;
+                    cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
+                    icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
+                    xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
+                    yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
+
+                    vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);
+                    invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
+                    xd = invProj * vecTilt(0);
+                    yd = invProj * vecTilt(1);
+
+                    double x_proj = xd*fx + cx;
+                    double y_proj = yd*fy + cy;
+
+                    error = sqrt( pow(x_proj - u, 2) + pow(y_proj - v, 2) );
+                }
+            }
         }
 
         double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
@@ -426,10 +461,27 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
     }
 }
 
+void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
+                   const CvMat* _distCoeffs,
+                   const CvMat* matR, const CvMat* matP )
+{
+    cvUndistortPointsInternal(_src, _dst, _cameraMatrix, _distCoeffs, matR, matP,
+                              cv::TermCriteria(cv::TermCriteria::COUNT, 5, 0.01));
+}
+
+void cv::undistortPoints( InputArray _src, OutputArray _dst,
+                          InputArray _cameraMatrix,
+                          InputArray _distCoeffs,
+                          InputArray _Rmat,
+                          InputArray _Pmat )
+{
+    undistortPoints(_src, _dst, _cameraMatrix, _distCoeffs, TermCriteria(TermCriteria::MAX_ITER, 5, 0.01), _Rmat, _Pmat);
+}
 
 void cv::undistortPoints( InputArray _src, OutputArray _dst,
                           InputArray _cameraMatrix,
                           InputArray _distCoeffs,
+                          TermCriteria criteria,
                           InputArray _Rmat,
                           InputArray _Pmat )
 {
@@ -450,7 +502,7 @@ void cv::undistortPoints( InputArray _src, OutputArray _dst,
         pP = &(matP = P);
     if( !distCoeffs.empty() )
         pD = &(_cdistCoeffs = distCoeffs);
-    cvUndistortPoints(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP);
+    cvUndistortPointsInternal(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP, criteria);
 }
 
 namespace cv