From 701c7e568536d061746d95277faf12667275540f Mon Sep 17 00:00:00 2001 From: Vladislav Sovrasov Date: Tue, 5 Sep 2017 16:05:13 +0300 Subject: [PATCH 1/2] imgproc: add stop criteria tuning in undistortPoints --- .../calib3d/test/test_undistort_points.cpp | 25 ++++++ modules/imgproc/include/opencv2/imgproc.hpp | 8 ++ modules/imgproc/src/undistort.cpp | 90 +++++++++++++++---- 3 files changed, 104 insertions(+), 19 deletions(-) diff --git a/modules/calib3d/test/test_undistort_points.cpp b/modules/calib3d/test/test_undistort_points.cpp index 51600ae04f..79e25f68e6 100644 --- a/modules/calib3d/test/test_undistort_points.cpp +++ b/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_(3,3,CV_64F) << 857.48296979, 0, 968.06224829, + 0, 876.71824265, 556.37145899, + 0, 0, 1); + Mat distCoeffs = (Mat_(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 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 pt_undist_vec; + undistortPoints(pt_distorted_vec, pt_undist_vec, cameraMatrix, distCoeffs, criteria); + + std::vector pt_redistorted_vec; + std::vector 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); +} diff --git a/modules/imgproc/include/opencv2/imgproc.hpp b/modules/imgproc/include/opencv2/imgproc.hpp index 9b88221f02..13032e8e82 100644 --- a/modules/imgproc/include/opencv2/imgproc.hpp +++ b/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 diff --git a/modules/imgproc/src/undistort.cpp b/modules/imgproc/src/undistort.cpp index 4931d0231f..ce2ba14251 100644 --- a/modules/imgproc/src/undistort.cpp +++ b/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(k[12], k[13], NULL, NULL, NULL, &invMatTilt); + cv::detail::computeTiltProjectionMatrix(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::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 From b421ebef86df409b75ccda1d224dbf4eff2a4d80 Mon Sep 17 00:00:00 2001 From: Vladislav Sovrasov Date: Thu, 14 Sep 2017 11:13:36 +0300 Subject: [PATCH 2/2] imgproc: slightly change the signature of undistortPoints overload --- modules/calib3d/test/test_undistort_points.cpp | 2 +- modules/imgproc/include/opencv2/imgproc.hpp | 5 ++--- modules/imgproc/src/undistort.cpp | 6 +++--- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/modules/calib3d/test/test_undistort_points.cpp b/modules/calib3d/test/test_undistort_points.cpp index 79e25f68e6..cc9006cce5 100644 --- a/modules/calib3d/test/test_undistort_points.cpp +++ b/modules/calib3d/test/test_undistort_points.cpp @@ -107,7 +107,7 @@ TEST(Calib3d_Undistort, stop_criteria) const double maxError = 1e-6; TermCriteria criteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 100, maxError); std::vector pt_undist_vec; - undistortPoints(pt_distorted_vec, pt_undist_vec, cameraMatrix, distCoeffs, criteria); + undistortPoints(pt_distorted_vec, pt_undist_vec, cameraMatrix, distCoeffs, noArray(), noArray(), criteria); std::vector pt_redistorted_vec; std::vector pt_undist_vec_homogeneous; diff --git a/modules/imgproc/include/opencv2/imgproc.hpp b/modules/imgproc/include/opencv2/imgproc.hpp index 13032e8e82..d1950e47cc 100644 --- a/modules/imgproc/include/opencv2/imgproc.hpp +++ b/modules/imgproc/include/opencv2/imgproc.hpp @@ -3040,10 +3040,9 @@ CV_EXPORTS_W void undistortPoints( InputArray src, OutputArray dst, @note Default version of cv::undistortPoints does 5 iterations to compute undistorted points. */ -CV_EXPORTS_AS(undistortPointsExt) void undistortPoints( InputArray src, OutputArray dst, +CV_EXPORTS_AS(undistortPointsIter) void undistortPoints( InputArray src, OutputArray dst, InputArray cameraMatrix, InputArray distCoeffs, - TermCriteria criteria, - InputArray R = noArray(), InputArray P = noArray()); + InputArray R, InputArray P, TermCriteria criteria); //! @} imgproc_transform diff --git a/modules/imgproc/src/undistort.cpp b/modules/imgproc/src/undistort.cpp index ce2ba14251..0ee79974db 100644 --- a/modules/imgproc/src/undistort.cpp +++ b/modules/imgproc/src/undistort.cpp @@ -475,15 +475,15 @@ void cv::undistortPoints( InputArray _src, OutputArray _dst, InputArray _Rmat, InputArray _Pmat ) { - undistortPoints(_src, _dst, _cameraMatrix, _distCoeffs, TermCriteria(TermCriteria::MAX_ITER, 5, 0.01), _Rmat, _Pmat); + undistortPoints(_src, _dst, _cameraMatrix, _distCoeffs, _Rmat, _Pmat, TermCriteria(TermCriteria::MAX_ITER, 5, 0.01)); } void cv::undistortPoints( InputArray _src, OutputArray _dst, InputArray _cameraMatrix, InputArray _distCoeffs, - TermCriteria criteria, InputArray _Rmat, - InputArray _Pmat ) + InputArray _Pmat, + TermCriteria criteria) { Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat(); Mat distCoeffs = _distCoeffs.getMat(), R = _Rmat.getMat(), P = _Pmat.getMat();