📄 cvcalibration.cpp
字号:
a3*_r_x_[k] + a4*d_r_x_[i*9+k];
}
}
}
}
else if( src->cols == 3 && src->rows == 3 )
{
double R[9], U[9], V[9], W[3], rx, ry, rz;
CvMat _R = cvMat( 3, 3, CV_64F, R );
CvMat _U = cvMat( 3, 3, CV_64F, U );
CvMat _V = cvMat( 3, 3, CV_64F, V );
CvMat _W = cvMat( 3, 1, CV_64F, W );
double theta, s, c;
int step = dst->rows > 1 ? dst->step / elem_size : 1;
if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) &&
(dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1))
CV_ERROR( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );
cvConvert( src, &_R );
if( !cvCheckArr( &_R, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) )
{
cvZero(dst);
if( jacobian )
cvZero(jacobian);
EXIT;
}
cvSVD( &_R, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T );
rx = R[7] - R[5];
ry = R[2] - R[6];
rz = R[3] - R[1];
s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
c = (R[0] + R[4] + R[8] - 1)*0.5;
c = c > 1. ? 1. : c < -1. ? -1. : c;
theta = acos(c);
if( s < 1e-5 )
{
double t;
if( c > 0 )
rx = ry = rz = 0;
else
{
t = (R[0] + 1)*0.5;
rx = theta*sqrt(MAX(t,0.));
t = (R[4] + 1)*0.5;
ry = theta*sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.);
t = (R[8] + 1)*0.5;
rz = theta*sqrt(MAX(t,0.))*(R[2] < 0 ? -1. : 1.);
}
if( jacobian )
{
memset( J, 0, sizeof(J) );
if( c > 0 )
{
J[5] = J[15] = J[19] = -0.5;
J[7] = J[11] = J[21] = 0.5;
}
}
}
else
{
double vth = 1/(2*s);
if( jacobian )
{
double t, dtheta_dtr = -1./s;
// var1 = [vth;theta]
// var = [om1;var1] = [om1;vth;theta]
double dvth_dtheta = -vth*c/s;
double d1 = 0.5*dvth_dtheta*dtheta_dtr;
double d2 = 0.5*dtheta_dtr;
// dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR
double dvardR[5*9] =
{
0, 0, 0, 0, 0, 1, 0, -1, 0,
0, 0, -1, 0, 0, 0, 1, 0, 0,
0, 1, 0, -1, 0, 0, 0, 0, 0,
d1, 0, 0, 0, d1, 0, 0, 0, d1,
d2, 0, 0, 0, d2, 0, 0, 0, d2
};
// var2 = [om;theta]
double dvar2dvar[] =
{
vth, 0, 0, rx, 0,
0, vth, 0, ry, 0,
0, 0, vth, rz, 0,
0, 0, 0, 0, 1
};
double domegadvar2[] =
{
theta, 0, 0, rx*vth,
0, theta, 0, ry*vth,
0, 0, theta, rz*vth
};
CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );
CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );
double t0[3*5];
CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
cvMatMul( &_t0, &_dvardR, &_J );
// transpose every row of _J (treat the rows as 3x3 matrices)
CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t);
CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t);
CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t);
}
vth *= theta;
rx *= vth; ry *= vth; rz *= vth;
}
if( depth == CV_32F )
{
dst->data.fl[0] = (float)rx;
dst->data.fl[step] = (float)ry;
dst->data.fl[step*2] = (float)rz;
}
else
{
dst->data.db[0] = rx;
dst->data.db[step] = ry;
dst->data.db[step*2] = rz;
}
}
if( jacobian )
{
if( depth == CV_32F )
{
if( jacobian->rows == _J.rows )
cvConvert( &_J, jacobian );
else
{
float Jf[3*9];
CvMat _Jf = cvMat( _J.rows, _J.cols, CV_32FC1, Jf );
cvConvert( &_J, &_Jf );
cvTranspose( &_Jf, jacobian );
}
}
else if( jacobian->rows == _J.rows )
cvCopy( &_J, jacobian );
else
cvTranspose( &_J, jacobian );
}
result = 1;
__END__;
return result;
}
CV_IMPL void
cvProjectPoints2( const CvMat* obj_points,
const CvMat* r_vec,
const CvMat* t_vec,
const CvMat* A,
const CvMat* dist_coeffs,
CvMat* img_points, CvMat* dpdr,
CvMat* dpdt, CvMat* dpdf,
CvMat* dpdc, CvMat* dpdk )
{
CvMat *_M = 0, *_m = 0;
CvMat *_dpdr = 0, *_dpdt = 0, *_dpdc = 0, *_dpdf = 0, *_dpdk = 0;
CV_FUNCNAME( "cvProjectPoints2" );
__BEGIN__;
int i, j, count;
int calc_derivatives;
const CvPoint3D64f* M;
CvPoint2D64f* m;
double r[3], R[9], dRdr[27], t[3], a[9], k[4] = {0,0,0,0}, fx, fy, cx, cy;
CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
CvMat _R = 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;
if( !CV_IS_MAT(obj_points) || !CV_IS_MAT(r_vec) ||
!CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
/*!CV_IS_MAT(dist_coeffs) ||*/ !CV_IS_MAT(img_points) )
CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" );
count = MAX(obj_points->rows, obj_points->cols);
if( CV_IS_CONT_MAT(obj_points->type) && CV_MAT_DEPTH(obj_points->type) == CV_64F &&
(obj_points->rows == 1 && CV_MAT_CN(obj_points->type) == 3 ||
obj_points->rows == count && CV_MAT_CN(obj_points->type)*obj_points->cols == 3))
_M = (CvMat*)obj_points;
else
{
CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 ));
CV_CALL( cvConvertPointsHomogenious( obj_points, _M ));
}
if( CV_IS_CONT_MAT(img_points->type) && CV_MAT_DEPTH(img_points->type) == CV_64F &&
(img_points->rows == 1 && CV_MAT_CN(img_points->type) == 2 ||
img_points->rows == count && CV_MAT_CN(img_points->type)*img_points->cols == 2))
_m = img_points;
else
CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 ));
M = (CvPoint3D64f*)_M->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 );
CV_CALL( cvRodrigues2( r_vec, &_r ));
CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr ));
cvCopy( r_vec, &_R );
}
else
{
_r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
CV_CALL( cvConvert( r_vec, &_r ));
CV_CALL( cvRodrigues2( &_r, &_R, &_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 );
CV_CALL( 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" );
CV_CALL( cvConvert( A, &_a ));
fx = a[0]; fy = a[4];
cx = a[2]; cy = a[5];
if( dist_coeffs )
{
if( !CV_IS_MAT(dist_coeffs) ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F ||
dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(dist_coeffs->type)), k );
CV_CALL( cvConvert( dist_coeffs, &_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 = dpdr;
else
CV_CALL( _dpdr = 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 = dpdt;
else
CV_CALL( _dpdt = 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 = dpdf;
else
CV_CALL( _dpdf = 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 = dpdc;
else
CV_CALL( _dpdc = 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 != 4 && dpdk->cols != 2) )
CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx4 or 2Nx2 floating-point matrix" );
if( !dist_coeffs )
CV_ERROR( CV_StsNullPtr, "dist_coeffs is NULL while dpdk is not" );
if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
_dpdk = dpdk;
else
CV_CALL( _dpdk = 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, a1, a2, a3, cdist;
double xd, yd;
z = z ? 1./z : 1;
x *= z; y *= z;
r2 = x*x + y*y;
r4 = r2*r2;
a1 = 2*x*y;
a2 = r2 + 2*x*x;
a3 = r2 + 2*y*y;
cdist = 1 + k[0]*r2 + k[1]*r4;
xd = x*cdist + k[2]*a1 + k[3]*a2;
yd = y*cdist + k[2]*a3 + k[3]*a1;
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 )
{
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*r2;
dpdk_p[1] = fx*x*r4;
dpdk_p[dpdk_step] = fy*y*r2;
dpdk_p[dpdk_step+1] = fy*y*r4;
if( _dpdk->cols > 2 )
{
dpdk_p[2] = fx*a1;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -