in modules/calib3d/src/calibration.cpp [528:920]
CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
const CvMat* r_vec,
const CvMat* t_vec,
const CvMat* A,
const CvMat* distCoeffs,
CvMat* imagePoints, CvMat* dpdr,
CvMat* dpdt, CvMat* dpdf,
CvMat* dpdc, CvMat* dpdk,
double aspectRatio )
{
Ptr<CvMat> matM, _m;
Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
int i, j, count;
int calc_derivatives;
const CvPoint3D64f* M;
CvPoint2D64f* m;
double r[3], R[9], dRdr[27], t[3], a[9], k[12] = {0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
!CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
/*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
if(total % 3 != 0)
{
//we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
}
count = total / 3;
if( CV_IS_CONT_MAT(objectPoints->type) &&
(CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
(objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
(objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
{
matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
cvConvert(objectPoints, matM);
}
else
{
// matM = cvCreateMat( 1, count, CV_64FC3 );
// cvConvertPointsHomogeneous( objectPoints, matM );
CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
}
if( CV_IS_CONT_MAT(imagePoints->type) &&
(CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) ||
(imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2) ||
(imagePoints->rows == 2 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
{
_m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
cvConvert(imagePoints, _m);
}
else
{
// _m = cvCreateMat( 1, count, CV_64FC2 );
CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
}
M = (CvPoint3D64f*)matM->data.db;
m = (CvPoint2D64f*)_m->data.db;
if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
(((r_vec->rows != 1 && r_vec->cols != 1) ||
r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
"floating-point rotation vector, or 3x3 rotation matrix" );
if( r_vec->rows == 3 && r_vec->cols == 3 )
{
_r = cvMat( 3, 1, CV_64FC1, r );
cvRodrigues2( r_vec, &_r );
cvRodrigues2( &_r, &matR, &_dRdr );
cvCopy( r_vec, &matR );
}
else
{
_r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
cvConvert( r_vec, &_r );
cvRodrigues2( &_r, &matR, &_dRdr );
}
if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
(t_vec->rows != 1 && t_vec->cols != 1) ||
t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
CV_Error( CV_StsBadArg,
"Translation vector must be 1x3 or 3x1 floating-point vector" );
_t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
cvConvert( t_vec, &_t );
if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
A->rows != 3 || A->cols != 3 )
CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
cvConvert( A, &_a );
fx = a[0]; fy = a[4];
cx = a[2]; cy = a[5];
if( fixedAspectRatio )
fx = fy*aspectRatio;
if( distCoeffs )
{
if( !CV_IS_MAT(distCoeffs) ||
(CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
(distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
(distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12) )
CV_Error( CV_StsBadArg, cvDistCoeffErr );
_k = cvMat( distCoeffs->rows, distCoeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
cvConvert( distCoeffs, &_k );
}
if( dpdr )
{
if( !CV_IS_MAT(dpdr) ||
(CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
dpdr->rows != count*2 || dpdr->cols != 3 )
CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
{
_dpdr.reset(cvCloneMat(dpdr));
}
else
_dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
dpdr_p = _dpdr->data.db;
dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
}
if( dpdt )
{
if( !CV_IS_MAT(dpdt) ||
(CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
dpdt->rows != count*2 || dpdt->cols != 3 )
CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
{
_dpdt.reset(cvCloneMat(dpdt));
}
else
_dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
dpdt_p = _dpdt->data.db;
dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
}
if( dpdf )
{
if( !CV_IS_MAT(dpdf) ||
(CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
dpdf->rows != count*2 || dpdf->cols != 2 )
CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
{
_dpdf.reset(cvCloneMat(dpdf));
}
else
_dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
dpdf_p = _dpdf->data.db;
dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
}
if( dpdc )
{
if( !CV_IS_MAT(dpdc) ||
(CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
dpdc->rows != count*2 || dpdc->cols != 2 )
CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
{
_dpdc.reset(cvCloneMat(dpdc));
}
else
_dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
dpdc_p = _dpdc->data.db;
dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
}
if( dpdk )
{
if( !CV_IS_MAT(dpdk) ||
(CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
dpdk->rows != count*2 || (dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
CV_Error( CV_StsBadArg, "dp/df must be 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
if( !distCoeffs )
CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
{
_dpdk.reset(cvCloneMat(dpdk));
}
else
_dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
dpdk_p = _dpdk->data.db;
dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
}
calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk;
for( i = 0; i < count; i++ )
{
double X = M[i].x, Y = M[i].y, Z = M[i].z;
double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
double r2, r4, r6, a1, a2, a3, cdist, icdist2;
double xd, yd;
z = z ? 1./z : 1;
x *= z; y *= z;
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);
xd = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
yd = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
m[i].x = xd*fx + cx;
m[i].y = yd*fy + cy;
if( calc_derivatives )
{
if( dpdc_p )
{
dpdc_p[0] = 1; dpdc_p[1] = 0;
dpdc_p[dpdc_step] = 0;
dpdc_p[dpdc_step+1] = 1;
dpdc_p += dpdc_step*2;
}
if( dpdf_p )
{
if( fixedAspectRatio )
{
dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio;
dpdf_p[dpdf_step] = 0;
dpdf_p[dpdf_step+1] = yd;
}
else
{
dpdf_p[0] = xd; dpdf_p[1] = 0;
dpdf_p[dpdf_step] = 0;
dpdf_p[dpdf_step+1] = yd;
}
dpdf_p += dpdf_step*2;
}
if( dpdk_p )
{
dpdk_p[0] = fx*x*icdist2*r2;
dpdk_p[1] = fx*x*icdist2*r4;
dpdk_p[dpdk_step] = fy*y*icdist2*r2;
dpdk_p[dpdk_step+1] = fy*y*icdist2*r4;
if( _dpdk->cols > 2 )
{
dpdk_p[2] = fx*a1;
dpdk_p[3] = fx*a2;
dpdk_p[dpdk_step+2] = fy*a3;
dpdk_p[dpdk_step+3] = fy*a1;
if( _dpdk->cols > 4 )
{
dpdk_p[4] = fx*x*icdist2*r6;
dpdk_p[dpdk_step+4] = fy*y*icdist2*r6;
if( _dpdk->cols > 5 )
{
dpdk_p[5] = fx*x*cdist*(-icdist2)*icdist2*r2;
dpdk_p[dpdk_step+5] = fy*y*cdist*(-icdist2)*icdist2*r2;
dpdk_p[6] = fx*x*cdist*(-icdist2)*icdist2*r4;
dpdk_p[dpdk_step+6] = fy*y*cdist*(-icdist2)*icdist2*r4;
dpdk_p[7] = fx*x*cdist*(-icdist2)*icdist2*r6;
dpdk_p[dpdk_step+7] = fy*y*cdist*(-icdist2)*icdist2*r6;
if( _dpdk->cols > 8 )
{
dpdk_p[8] = fx*r2; //s1
dpdk_p[9] = fx*r4; //s2
dpdk_p[10] = 0;//s3
dpdk_p[11] = 0;//s4
dpdk_p[dpdk_step+8] = 0; //s1
dpdk_p[dpdk_step+9] = 0; //s2
dpdk_p[dpdk_step+10] = fy*r2; //s3
dpdk_p[dpdk_step+11] = fy*r4; //s4
}
}
}
}
dpdk_p += dpdk_step*2;
}
if( dpdt_p )
{
double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
for( j = 0; j < 3; j++ )
{
double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
double dmxdt = fx*(dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
double dmydt = fy*(dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
dpdt_p[j] = dmxdt;
dpdt_p[dpdt_step+j] = dmydt;
}
dpdt_p += dpdt_step*2;
}
if( dpdr_p )
{
double dx0dr[] =
{
X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
};
double dy0dr[] =
{
X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
};
double dz0dr[] =
{
X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
};
for( j = 0; j < 3; j++ )
{
double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
double dydr = z*(dy0dr[j] - y*dz0dr[j]);
double dr2dr = 2*x*dxdr + 2*y*dydr;
double dcdist_dr = k[0]*dr2dr + 2*k[1]*r2*dr2dr + 3*k[4]*r4*dr2dr;
double dicdist2_dr = -icdist2*icdist2*(k[5]*dr2dr + 2*k[6]*r2*dr2dr + 3*k[7]*r4*dr2dr);
double da1dr = 2*(x*dydr + y*dxdr);
double dmxdr = fx*(dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr) + k[8]*dr2dr + 2*r2*k[9]*dr2dr);
double dmydr = fy*(dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr + k[10]*dr2dr + 2*r2*k[11]*dr2dr);
dpdr_p[j] = dmxdr;
dpdr_p[dpdr_step+j] = dmydr;
}
dpdr_p += dpdr_step*2;
}
}
}
if( _m != imagePoints )
cvConvert( _m, imagePoints );
if( _dpdr != dpdr )
cvConvert( _dpdr, dpdr );
if( _dpdt != dpdt )
cvConvert( _dpdt, dpdt );
if( _dpdf != dpdf )
cvConvert( _dpdf, dpdf );
if( _dpdc != dpdc )
cvConvert( _dpdc, dpdc );
if( _dpdk != dpdk )
cvConvert( _dpdk, dpdk );
}