mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-07-25 14:47:07 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #9560 from sovrasov:undistort_stop_criteria

Browse Source
This commit is contained in:
Vadim Pisarevsky 2017-09-15 12:34:16 +00:00
commit 48cc1b351d
3 changed files with 103 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
modules/calib3d/test/test_undistort_points.cpp
View File

@ -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, noArray(), noArray(), 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);
}

7
modules/imgproc/include/opencv2/imgproc.hpp
View File

@ -3036,6 +3036,13 @@ 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(undistortPointsIter) void undistortPoints( InputArray src, OutputArray dst,
InputArray cameraMatrix, InputArray distCoeffs,
InputArray R, InputArray P, TermCriteria criteria);
//! @} imgproc_transform

90
modules/imgproc/src/undistort.cpp
View File

@ -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,12 +461,29 @@ 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, _Rmat, _Pmat, TermCriteria(TermCriteria::MAX_ITER, 5, 0.01));
}
void cv::undistortPoints( InputArray _src, OutputArray _dst,
InputArray _cameraMatrix,
InputArray _distCoeffs,
InputArray _Rmat,
InputArray _Pmat,
TermCriteria criteria)
{
Mat src = _src.getMat(), cameraMatrix = _cameraMatrix.getMat();
Mat distCoeffs = _distCoeffs.getMat(), R = _Rmat.getMat(), P = _Pmat.getMat();
@ -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