Skip to content

Commit 701c7e5

Browse files
sovrasovsovrasov
committed
imgproc: add stop criteria tuning in undistortPoints
1 parent 9640bbe commit 701c7e5

File tree

3 files changed

+104
-19
lines changed

3 files changed

+104
-19
lines changed

‎modules/calib3d/test/test_undistort_points.cpp

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -92,3 +92,28 @@ void CV_UndistortTest::run(int /* start_from */)
9292
}
9393

9494
TEST(Calib3d_Undistort, accuracy) { CV_UndistortTest test; test.safe_run(); }
95+
96+
TEST(Calib3d_Undistort, stop_criteria)
97+
{
98+
Mat cameraMatrix = (Mat_<double>(3,3,CV_64F) << 857.48296979, 0, 968.06224829,
99+
0, 876.71824265, 556.37145899,
100+
0, 0, 1);
101+
Mat distCoeffs = (Mat_<double>(5,1,CV_64F) <<
102+
-2.57614020e-01, 8.77086999e-02, -2.56970803e-04, -5.93390389e-04, -1.52194091e-02);
103+
RNG rng(2);
104+
Point2d pt_distorted(rng.uniform(0.0, 1920.0), rng.uniform(0.0, 1080.0));
105+
std::vector<Point2d> pt_distorted_vec;
106+
pt_distorted_vec.push_back(pt_distorted);
107+
const double maxError = 1e-6;
108+
TermCriteria criteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 100, maxError);
109+
std::vector<Point2d> pt_undist_vec;
110+
undistortPoints(pt_distorted_vec, pt_undist_vec, cameraMatrix, distCoeffs, criteria);
111+
112+
std::vector<Point2d> pt_redistorted_vec;
113+
std::vector<Point3d> pt_undist_vec_homogeneous;
114+
pt_undist_vec_homogeneous.push_back( Point3d(pt_undist_vec[0].x, pt_undist_vec[0].y, 1.0) );
115+
projectPoints(pt_undist_vec_homogeneous, Mat::zeros(3,1,CV_64F), Mat::zeros(3,1,CV_64F), cameraMatrix, distCoeffs, pt_redistorted_vec);
116+
const double obtainedError = sqrt( pow(pt_distorted.x - pt_redistorted_vec[0].x, 2) + pow(pt_distorted.y - pt_redistorted_vec[0].y, 2) );
117+
118+
ASSERT_LE(obtainedError, maxError);
119+
}

‎modules/imgproc/include/opencv2/imgproc.hpp

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3036,6 +3036,14 @@ cv::stereoRectify can be passed here. If the matrix is empty, the identity new c
30363036
CV_EXPORTS_W void undistortPoints( InputArray src, OutputArray dst,
30373037
InputArray cameraMatrix, InputArray distCoeffs,
30383038
InputArray R = noArray(), InputArray P = noArray());
3039+
/** @overload
3040+
@note Default version of cv::undistortPoints does 5 iterations to compute undistorted points.
3041+
3042+
*/
3043+
CV_EXPORTS_AS(undistortPointsExt) void undistortPoints( InputArray src, OutputArray dst,
3044+
InputArray cameraMatrix, InputArray distCoeffs,
3045+
TermCriteria criteria,
3046+
InputArray R = noArray(), InputArray P = noArray());
30393047

30403048
//! @} imgproc_transform
30413049

‎modules/imgproc/src/undistort.cpp

Lines changed: 71 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -294,15 +294,15 @@ cvInitUndistortRectifyMap( const CvMat* Aarr, const CvMat* dist_coeffs,
294294
CV_Assert( mapx0.data == mapx.data && mapy0.data == mapy.data );
295295
}
296296

297-
298-
void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
297+
static void cvUndistortPointsInternal( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
299298
const CvMat* _distCoeffs,
300-
const CvMat* matR, const CvMat* matP )
299+
const CvMat* matR, const CvMat* matP, cv::TermCriteria criteria)
301300
{
302301
double A[3][3], RR[3][3], k[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
303302
CvMat matA=cvMat(3, 3, CV_64F, A), _Dk;
304303
CvMat _RR=cvMat(3, 3, CV_64F, RR);
305304
cv::Matx33d invMatTilt = cv::Matx33d::eye();
305+
cv::Matx33d matTilt = cv::Matx33d::eye();
306306

307307
CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
308308
(_src->rows == 1 || _src->cols == 1) &&
@@ -316,7 +316,6 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
316316

317317
cvConvert( _cameraMatrix, &matA );
318318

319-
int iters = 0;
320319

321320
if( _distCoeffs )
322321
{
@@ -332,9 +331,11 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
332331
CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
333332

334333
cvConvert( _distCoeffs, &_Dk );
335-
iters = 5;
336334
if (k[12] != 0 || k[13] != 0)
335+
{
337336
cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], NULL, NULL, NULL, &invMatTilt);
337+
cv::detail::computeTiltProjectionMatrix<double>(k[12], k[13], &matTilt, NULL, NULL);
338+
}
338339
}
339340

340341
if( matR )
@@ -373,7 +374,7 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
373374
int n = _src->rows + _src->cols - 1;
374375
for( int i = 0; i < n; i++ )
375376
{
376-
double x, y, x0 = 0, y0 = 0;
377+
double x, y, x0 = 0, y0 = 0, u, v;
377378
if( stype == CV_32FC2 )
378379
{
379380
x = srcf[i*sstep].x;
@@ -384,27 +385,61 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
384385
x = srcd[i*sstep].x;
385386
y = srcd[i*sstep].y;
386387
}
387-
388+
u = x; v = y;
388389
x = (x - cx)*ifx;
389390
y = (y - cy)*ify;
390391

391-
if( iters ) {
392+
if( _distCoeffs ) {
392393
// compensate tilt distortion
393394
cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1);
394395
double invProj = vecUntilt(2) ? 1./vecUntilt(2) : 1;
395396
x0 = x = invProj * vecUntilt(0);
396397
y0 = y = invProj * vecUntilt(1);
397-
}
398398

399-
// compensate distortion iteratively
400-
for( int j = 0; j < iters; j++ )
401-
{
402-
double r2 = x*x + y*y;
403-
double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
404-
double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2;
405-
double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2;
406-
x = (x0 - deltaX)*icdist;
407-
y = (y0 - deltaY)*icdist;
399+
double error = std::numeric_limits<double>::max();
400+
// compensate distortion iteratively
401+
402+
for( int j = 0; ; j++ )
403+
{
404+
if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
405+
break;
406+
if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)
407+
break;
408+
double r2 = x*x + y*y;
409+
double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
410+
double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2;
411+
double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2;
412+
x = (x0 - deltaX)*icdist;
413+
y = (y0 - deltaY)*icdist;
414+
415+
if(criteria.type & cv::TermCriteria::EPS)
416+
{
417+
double r4, r6, a1, a2, a3, cdist, icdist2;
418+
double xd, yd, xd0, yd0;
419+
cv::Vec3d vecTilt;
420+
421+
r2 = x*x + y*y;
422+
r4 = r2*r2;
423+
r6 = r4*r2;
424+
a1 = 2*x*y;
425+
a2 = r2 + 2*x*x;
426+
a3 = r2 + 2*y*y;
427+
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
428+
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
429+
xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
430+
yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
431+
432+
vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);
433+
invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
434+
xd = invProj * vecTilt(0);
435+
yd = invProj * vecTilt(1);
436+
437+
double x_proj = xd*fx + cx;
438+
double y_proj = yd*fy + cy;
439+
440+
error = sqrt( pow(x_proj - u, 2) + pow(y_proj - v, 2) );
441+
}
442+
}
408443
}
409444

410445
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
426461
}
427462
}
428463

464+
void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
465+
const CvMat* _distCoeffs,
466+
const CvMat* matR, const CvMat* matP )
467+
{
468+
cvUndistortPointsInternal(_src, _dst, _cameraMatrix, _distCoeffs, matR, matP,
469+
cv::TermCriteria(cv::TermCriteria::COUNT, 5, 0.01));
470+
}
471+
472+
void cv::undistortPoints( InputArray _src, OutputArray _dst,
473+
InputArray _cameraMatrix,
474+
InputArray _distCoeffs,
475+
InputArray _Rmat,
476+
InputArray _Pmat )
477+
{
478+
undistortPoints(_src, _dst, _cameraMatrix, _distCoeffs, TermCriteria(TermCriteria::MAX_ITER, 5, 0.01), _Rmat, _Pmat);
479+
}
429480

430481
void cv::undistortPoints( InputArray _src, OutputArray _dst,
431482
InputArray _cameraMatrix,
432483
InputArray _distCoeffs,
484+
TermCriteria criteria,
433485
InputArray _Rmat,
434486
InputArray _Pmat )
435487
{
@@ -450,7 +502,7 @@ void cv::undistortPoints( InputArray _src, OutputArray _dst,
450502
pP = &(matP = P);
451503
if( !distCoeffs.empty() )
452504
pD = &(_cdistCoeffs = distCoeffs);
453-
cvUndistortPoints(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP);
505+
cvUndistortPointsInternal(&_csrc, &_cdst, &_ccameraMatrix, pD, pR, pP, criteria);
454506
}
455507

456508
namespace cv

0 commit comments

Comments
 (0)