potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-31 08:13:04 +04:00
Code Packages 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
parent 5707304777 b421ebef86
commit 48cc1b351d
3 changed files with 103 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/imgproc/src/undistort.cpp
+71 -19
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