mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-11-29 05:29:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

undistortPoints: only consider distCoeffs if present

Browse Source 
iters should be 0 if we have no distortion. Also skip tilt distortion in
that case.
Furthermore move variable declarations to usage sites.
This commit is contained in:
Pavel Rojtberg 2016-11-09 14:19:23 +01:00
parent d1bbc0b6e6
commit 3609343acf
1 changed files with 28 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -280,16 +280,9 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
const CvMat* _distCoeffs, const CvMat* _distCoeffs,
const CvMat* matR, const CvMat* matP ) const CvMat* matR, const CvMat* matP )
{ {
double A[3][3], RR[3][3], k[14]={0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, ifx, ify, cx, cy; 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 matA=cvMat(3, 3, CV_64F, A), _Dk;
CvMat _RR=cvMat(3, 3, CV_64F, RR); CvMat _RR=cvMat(3, 3, CV_64F, RR);
const CvPoint2D32f* srcf;
const CvPoint2D64f* srcd;
CvPoint2D32f* dstf;
CvPoint2D64f* dstd;
int stype, dtype;
int sstep, dstep;
int i, j, n, iters = 1;
cv::Matx33d invMatTilt = cv::Matx33d::eye(); cv::Matx33d invMatTilt = cv::Matx33d::eye();
CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) && CV_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
@ -304,6 +297,8 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
cvConvert( _cameraMatrix, &matA ); cvConvert( _cameraMatrix, &matA );
int iters = 0;
if( _distCoeffs ) if( _distCoeffs )
{ {
CV_Assert( CV_IS_MAT(_distCoeffs) && CV_Assert( CV_IS_MAT(_distCoeffs) &&
@ -340,27 +335,26 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
cvMatMul( &_PP, &_RR, &_RR ); cvMatMul( &_PP, &_RR, &_RR );
} }
srcf = (const CvPoint2D32f*)_src->data.ptr; const CvPoint2D32f* srcf = (const CvPoint2D32f*)_src->data.ptr;
srcd = (const CvPoint2D64f*)_src->data.ptr; const CvPoint2D64f* srcd = (const CvPoint2D64f*)_src->data.ptr;
dstf = (CvPoint2D32f*)_dst->data.ptr; CvPoint2D32f* dstf = (CvPoint2D32f*)_dst->data.ptr;
dstd = (CvPoint2D64f*)_dst->data.ptr; CvPoint2D64f* dstd = (CvPoint2D64f*)_dst->data.ptr;
stype = CV_MAT_TYPE(_src->type); int stype = CV_MAT_TYPE(_src->type);
dtype = CV_MAT_TYPE(_dst->type); int dtype = CV_MAT_TYPE(_dst->type);
sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype); int sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype); int dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
n = _src->rows + _src->cols - 1; double fx = A[0][0];
double fy = A[1][1];
double ifx = 1./fx;
double ify = 1./fy;
double cx = A[0][2];
double cy = A[1][2];
fx = A[0][0]; int n = _src->rows + _src->cols - 1;
fy = A[1][1]; for( int i = 0; i < n; i++ )
ifx = 1./fx;
ify = 1./fy;
cx = A[0][2];
cy = A[1][2];
for( i = 0; i < n; i++ )
{ {
double x, y, x0, y0; double x, y, x0 = 0, y0 = 0;
if( stype == CV_32FC2 ) if( stype == CV_32FC2 )
{ {
x = srcf[i*sstep].x; x = srcf[i*sstep].x;
@ -375,14 +369,16 @@ void cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatr
x = (x - cx)*ifx; x = (x - cx)*ifx;
y = (y - cy)*ify; y = (y - cy)*ify;
// compensate tilt distortion if( iters ) {
cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1); // compensate tilt distortion
double invProj = vecUntilt(2) ? 1./vecUntilt(2) : 1; cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1);
x0 = x = invProj * vecUntilt(0); double invProj = vecUntilt(2) ? 1./vecUntilt(2) : 1;
y0 = y = invProj * vecUntilt(1); x0 = x = invProj * vecUntilt(0);
y0 = y = invProj * vecUntilt(1);
}
// compensate distortion iteratively // compensate distortion iteratively
for( j = 0; j < iters; j++ ) for( int j = 0; j < iters; j++ )
{ {
double r2 = x*x + y*y; 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 icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);