const CvMat* _distCoeffs,
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 _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_Assert( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
cvConvert( _cameraMatrix, &matA );
+ int iters = 0;
+
if( _distCoeffs )
{
CV_Assert( CV_IS_MAT(_distCoeffs) &&
cvMatMul( &_PP, &_RR, &_RR );
}
- srcf = (const CvPoint2D32f*)_src->data.ptr;
- srcd = (const CvPoint2D64f*)_src->data.ptr;
- dstf = (CvPoint2D32f*)_dst->data.ptr;
- dstd = (CvPoint2D64f*)_dst->data.ptr;
- stype = CV_MAT_TYPE(_src->type);
- dtype = CV_MAT_TYPE(_dst->type);
- sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
- dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
-
- n = _src->rows + _src->cols - 1;
-
- fx = A[0][0];
- fy = A[1][1];
- ifx = 1./fx;
- ify = 1./fy;
- cx = A[0][2];
- cy = A[1][2];
-
- for( i = 0; i < n; i++ )
+ const CvPoint2D32f* srcf = (const CvPoint2D32f*)_src->data.ptr;
+ const CvPoint2D64f* srcd = (const CvPoint2D64f*)_src->data.ptr;
+ CvPoint2D32f* dstf = (CvPoint2D32f*)_dst->data.ptr;
+ CvPoint2D64f* dstd = (CvPoint2D64f*)_dst->data.ptr;
+ int stype = CV_MAT_TYPE(_src->type);
+ int dtype = CV_MAT_TYPE(_dst->type);
+ int sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
+ int dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
+
+ 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];
+
+ int n = _src->rows + _src->cols - 1;
+ for( int i = 0; i < n; i++ )
{
- double x, y, x0, y0;
+ double x, y, x0 = 0, y0 = 0;
if( stype == CV_32FC2 )
{
x = srcf[i*sstep].x;
x = (x - cx)*ifx;
y = (y - cy)*ify;
- // 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);
+ if( iters ) {
+ // 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( j = 0; j < iters; j++ )
+ 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);