cvCalcMatMulDeriv：
// reimplementation of dAB.m
/*这个矩阵求导和传统的矩阵求导不太一样 求dcda 假设a：M*N c：I*J 最后生成矩阵 IJ * MN -> PQ
求得的元素是 将c按行主序 展开成向量C，C(0,0) ~ C(0,I*J) , a也 按行主序 展开成向量A，A(0,0) ~ A(0,M*N )
那么 dcda = D = d(p,q) =
dC(0,0)/dA(0,0) dC(0,0)/dA(0,1) dC(0,0)/dA(0,2) ... dC(0,0)/dA(0,MN)
dC(0,1)/dA(0,0) dC(0,1)/dA(0,1) dC(0,1)/dA(0,2) ... dC(0,1)/dA(0,MN)
...
...
dC(0,IJ)/dA(0,0) dC(0,IJ)/dA(0,1) dC(0,IJ)/dA(0,2) ... dC(0,IJ)/dA(0,MN)
*/
CV_IMPL void cvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB )
{
//矩阵对矩阵求偏导：相当于被偏导的矩阵每个位置上的元素 对 求导矩阵的每个值求偏导。 若A m*n B:n*k AB:m*k dABdA = m*
int i, j, M, N, L;
int bstep;
CV_Assert( CV_IS_MAT(A) && CV_IS_MAT(B) );
CV_Assert( CV_ARE_TYPES_EQ(A, B) &&
(CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) );
CV_Assert( A->cols == B->rows );
M = A->rows;
L = A->cols;
N = B->cols;
bstep = B->step/CV_ELEM_SIZE(B->type);//B行有多少个
if( dABdA )
{
CV_Assert( CV_ARE_TYPES_EQ(A, dABdA) &&
dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols );
}
if( dABdB )
{
CV_Assert( CV_ARE_TYPES_EQ(A, dABdB) &&
dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols );
}
if( CV_MAT_TYPE(A->type) == CV_32F )
{
for( i = 0; i < M*N; i++ )
{
int i1 = i / N, i2 = i % N;
if( dABdA )
{
float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i);
const float* b = (const float*)B->data.ptr + i2;
for( j = 0; j < M*L; j++ )
dcda[j] = 0;
for( j = 0; j < L; j++ )
dcda[i1*L + j] = b[j*bstep];//实际上是列
}
if( dABdB )
{
float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i);//step行的长度 dcdb 实际上指的是这一列
const float* a = (const float*)(A->data.ptr + A->step*i1);
for( j = 0; j < L*N; j++ )
dcdb[j] = 0;
for( j = 0; j < L; j++ )
dcdb[j*N + i2] = a[j];
}
}
}
else
{
for( i = 0; i < M*N; i++ )
{
int i1 = i / N, i2 = i % N;
if( dABdA )
{
double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i);
const double* b = (const double*)B->data.ptr + i2;
for( j = 0; j < M*L; j++ )
dcda[j] = 0;
for( j = 0; j < L; j++ )
dcda[i1*L + j] = b[j*bstep];
}
if( dABdB )
{
double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i);
const double* a = (const double*)(A->data.ptr + A->step*i1);
for( j = 0; j < L*N; j++ )
dcdb[j] = 0;
for( j = 0; j < L; j++ )
dcdb[j*N + i2] = a[j];
}
}
}
}cvComposeRT：
// reimplementation of compose_motion.m
/*R3 = R2 * R1 Rrw = Rrl * Rlw
t3 = R2t1 + t2 trw = Rrl * tlw + trl
*/
CV_IMPL void cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1,
const CvMat* _rvec2, const CvMat* _tvec2,
CvMat* _rvec3, CvMat* _tvec3,
CvMat* dr3dr1, CvMat* dr3dt1,
CvMat* dr3dr2, CvMat* dr3dt2,
CvMat* dt3dr1, CvMat* dt3dt1,
CvMat* dt3dr2, CvMat* dt3dt2 )
{
double _r1[3], _r2[3];
double _R1[9], _d1[9*3], _R2[9], _d2[9*3];
CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2);
CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2);
CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2);
CV_Assert( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) );
CV_Assert( CV_MAT_TYPE(_rvec1->type) == CV_32F ||
CV_MAT_TYPE(_rvec1->type) == CV_64F );
CV_Assert( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) );
cvConvert( _rvec1, &r1 );
cvConvert( _rvec2, &r2 );
//旋转向量 变 旋转矩阵 dR1dr1 dR2dr2 是对应偏导数
cvRodrigues2( &r1, &R1, &dR1dr1 );
cvRodrigues2( &r2, &R2, &dR2dr2 );
//
if( _rvec3 || dr3dr1 || dr3dr2 )
{
//参数绑定
double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3];
double _W1[9*3], _W2[3*3];
CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3);
CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2);
CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3);
CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2);
//矩阵乘法 dst=src1*src2
cvMatMul( &R2, &R1, &R3 );
//求偏导
cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 );
//罗德里格斯公式 将旋转矩阵变为旋转向量
cvRodrigues2( &R3, &r3, &dr3dR3 );
if( _rvec3 )
cvConvert( &r3, _rvec3 );
//链式法则
if( dr3dr1 )
{
cvMatMul( &dr3dR3, &dR3dR1, &W1 );
cvMatMul( &W1, &dR1dr1, &W2 );
cvConvert( &W2, dr3dr1 );
}
if( dr3dr2 )
{
cvMatMul( &dr3dR3, &dR3dR2, &W1 );
cvMatMul( &W1, &dR2dr2, &W2 );
cvConvert( &W2, dr3dr2 );
}
}
if( dr3dt1 )
cvZero( dr3dt1 );
if( dr3dt2 )
cvZero( dr3dt2 );
//原理基本同上
if( _tvec3 || dt3dr2 || dt3dt1 )
{
double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3];
CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2);
CvMat t3 = cvMat(3,1,CV_64F,_t3);
CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2);
CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1);
CvMat W3 = cvMat(3, 3, CV_64F, _W3);
CV_Assert( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) );
CV_Assert( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) );
cvConvert( _tvec1, &t1 );
cvConvert( _tvec2, &t2 );
cvMatMulAdd( &R2, &t1, &t2, &t3 );
if( _tvec3 )
cvConvert( &t3, _tvec3 );
if( dt3dr2 || dt3dt1 )
{
cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 );
if( dt3dr2 )
{
cvMatMul( &dxdR2, &dR2dr2, &W3 );
cvConvert( &W3, dt3dr2 );
}
if( dt3dt1 )
cvConvert( &dxdt1, dt3dt1 );
}
}
//基本同上
if( dt3dt2 )
cvSetIdentity( dt3dt2 );
if( dt3dr1 )
cvZero( dt3dr1 );
}
cvProjectPoints2Internal：
//计算 3d点到2d点的映射， 并且计算 点p关于外参r t，焦距f，偏置c，畸变k的偏导
static void cvProjectPoints2Internal( const CvMat* objectPoints,
const CvMat* r_vec,
const CvMat* t_vec,
const CvMat* A,
const CvMat* distCoeffs,
CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
CvMat* dpdo CV_DEFAULT(NULL),
double aspectRatio CV_DEFAULT(0) )
{
//设定不同指针
Ptr<CvMat> matM, _m;
Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
Ptr<CvMat> _dpdo;
//计数
int i, j, count;
int calc_derivatives;
//点指针
const CvPoint3D64f* M;
CvPoint2D64f* m;
//绑定内存
double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
Matx33d matTilt = Matx33d::eye();
Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
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;
double* dpdo_p = 0;
int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
int dpdo_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;
//参数类型检查 matM是3d点
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" );
}
//_m是2d点
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" );
}
//3d点 和 2d点指针
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 &&
distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
CV_Error( CV_StsBadArg, cvDistCoeffErr );
_k = cvMat( distCoeffs->rows, distCoeffs->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
cvConvert( distCoeffs, &_k );
/*
计算传感器倾斜矩阵，（倾斜参数x，倾斜参数y，倾斜矩阵Tilt，偏导）
*/
if(k[12] != 0 || k[13] != 0)
{
detail::computeTiltProjectionMatrix(k[12], k[13],
&matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
}
}
//做类型检测，然后指针初始化，并获取每行的元素个数
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 != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 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]);
}
if( dpdo )
{
if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
&& CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
|| dpdo->rows != count * 2 || dpdo->cols != count * 3 )
CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
{
_dpdo.reset( cvCloneMat( dpdo ) );
}
else
_dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
cvZero(_dpdo);
dpdo_p = _dpdo->data.db;
dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
}
//要不要算偏导
calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
//遍历所有点进行运算
for( i = 0; i < count; i++ )
{
//X Y Z 是3d点坐标
double X = M[i].x, Y = M[i].y, Z = M[i].z;
//利用外参R t ，对齐坐标系：世界坐标系 和 相机坐标系，xyz是相机坐标系下点坐标
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, xd0, yd0, invProj;
Vec3d vecTilt;
Vec3d dVecTilt;
Matx22d dMatTilt;
Vec2d dXdYd;
//逆深度 z不是0 就取倒数
double z0 = z;
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);
//加入畸变
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*Vec3d(xd0, yd0, 1);
invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
xd = invProj * vecTilt(0);
yd = invProj * vecTilt(1);
// 考虑内参，投影到像素平面
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; // dp_xdc_x; dp_xdc_y
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; // dp_xdf_x; dp_xdf_y
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;
}
for (int row = 0; row < 2; ++row)
for (int col = 0; col < 2; ++col)
dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
- matTilt(2,col)*vecTilt(row);
double invProjSquare = (invProj*invProj);
dMatTilt *= invProjSquare;
if( dpdk_p )
{
dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
dpdk_p[0] = fx*dXdYd(0);
dpdk_p[dpdk_step] = fy*dXdYd(1);
dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
dpdk_p[1] = fx*dXdYd(0);
dpdk_p[dpdk_step+1] = fy*dXdYd(1);
if( _dpdk->cols > 2 )
{
dXdYd = dMatTilt*Vec2d(a1, a3);
dpdk_p[2] = fx*dXdYd(0);
dpdk_p[dpdk_step+2] = fy*dXdYd(1);
dXdYd = dMatTilt*Vec2d(a2, a1);
dpdk_p[3] = fx*dXdYd(0);
dpdk_p[dpdk_step+3] = fy*dXdYd(1);
if( _dpdk->cols > 4 )
{
dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
dpdk_p[4] = fx*dXdYd(0);
dpdk_p[dpdk_step+4] = fy*dXdYd(1);
if( _dpdk->cols > 5 )
{
dXdYd = dMatTilt*Vec2d(
x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
dpdk_p[5] = fx*dXdYd(0);
dpdk_p[dpdk_step+5] = fy*dXdYd(1);
dXdYd = dMatTilt*Vec2d(
x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
dpdk_p[6] = fx*dXdYd(0);
dpdk_p[dpdk_step+6] = fy*dXdYd(1);
dXdYd = dMatTilt*Vec2d(
x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
dpdk_p[7] = fx*dXdYd(0);
dpdk_p[dpdk_step+7] = fy*dXdYd(1);
if( _dpdk->cols > 8 )
{
dXdYd = dMatTilt*Vec2d(r2, 0);
dpdk_p[8] = fx*dXdYd(0); //s1
dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
dXdYd = dMatTilt*Vec2d(r4, 0);
dpdk_p[9] = fx*dXdYd(0); //s2
dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
dXdYd = dMatTilt*Vec2d(0, r2);
dpdk_p[10] = fx*dXdYd(0);//s3
dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
dXdYd = dMatTilt*Vec2d(0, r4);
dpdk_p[11] = fx*dXdYd(0);//s4
dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
if( _dpdk->cols > 12 )
{
dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
dpdk_p[12] = fx * invProjSquare * (
dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
dpdk_p[dpdk_step+12] = fy*invProjSquare * (
dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
dpdk_p[13] = fx * invProjSquare * (
dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
dpdk_p[dpdk_step+13] = fy * invProjSquare * (
dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
}
}
}
}
}
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 = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
dpdt_p[j] = fx*dXdYd(0);
dpdt_p[dpdt_step+j] = fy*dXdYd(1);
}
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] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
double da1dr = 2*(x*dydr + y*dxdr);
double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
dpdr_p[j] = fx*dXdYd(0);
dpdr_p[dpdr_step+j] = fy*dXdYd(1);
}
dpdr_p += dpdr_step*2;
}
if( dpdo_p )
{
double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
z * ( R[1] - x * z * z0 * R[7] ),
z * ( R[2] - x * z * z0 * R[8] ) };
double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
z * ( R[4] - y * z * z0 * R[7] ),
z * ( R[5] - y * z * z0 * R[8] ) };
for( j = 0; j < 3; j++ )
{
double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
double dr4do = 2 * r2 * dr2do;
double dr6do = 3 * r4 * dr2do;
double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
double da2do = dr2do + 4 * x * dxdo[j];
double da3do = dr2do + 4 * y * dydo[j];
double dcdist_do
= k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
double dicdist2_do = -icdist2 * icdist2
* ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
double dxd0_do = cdist * icdist2 * dxdo[j]
+ x * icdist2 * dcdist_do + x * cdist * dicdist2_do
+ k[2] * da1do + k[3] * da2do + k[8] * dr2do
+ k[9] * dr4do;
double dyd0_do = cdist * icdist2 * dydo[j]
+ y * icdist2 * dcdist_do + y * cdist * dicdist2_do
+ k[2] * da3do + k[3] * da1do + k[10] * dr2do
+ k[11] * dr4do;
dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
}
dpdo_p += dpdo_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 );
if( _dpdo != dpdo )
cvConvert( _dpdo, dpdo );
}cvFindExtrinsicCameraParams2
//获取外参，获取相机坐标系和世界坐标系下之间的旋转向量和平移向量 本质上和pnp差不多 //给初值，然后LM优化
CV_IMPL void cvFindExtrinsicCameraParams2( const CvMat* objectPoints,
const CvMat* imagePoints, const CvMat* A,
const CvMat* distCoeffs, CvMat* rvec, CvMat* tvec,
int useExtrinsicGuess )
{
const int max_iter = 20;
Ptr<CvMat> matM, _Mxy, _m, _mn, matL;
//变量初始定义
int i, count;
double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9];
double MM[9], U[9], V[9], W[3];
cv::Scalar Mc;
double param[6];
CvMat matA = cvMat( 3, 3, CV_64F, a );
CvMat _Ar = cvMat( 3, 3, CV_64F, ar );
CvMat matR = cvMat( 3, 3, CV_64F, R );
CvMat _r = cvMat( 3, 1, CV_64F, param );
CvMat _t = cvMat( 3, 1, CV_64F, param + 3 );
CvMat _Mc = cvMat( 1, 3, CV_64F, Mc.val );
CvMat _MM = cvMat( 3, 3, CV_64F, MM );
CvMat matU = cvMat( 3, 3, CV_64F, U );
CvMat matV = cvMat( 3, 3, CV_64F, V );
CvMat matW = cvMat( 3, 1, CV_64F, W );
CvMat _param = cvMat( 6, 1, CV_64F, param );
CvMat _dpdr, _dpdt;
//变量类型检测
CV_Assert( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) &&
CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) );
count = MAX(objectPoints->cols, objectPoints->rows);
matM.reset(cvCreateMat( 1, count, CV_64FC3 ));
_m.reset(cvCreateMat( 1, count, CV_64FC2 ));
//转成齐次坐标
cvConvertPointsHomogeneous( objectPoints, matM );
cvConvertPointsHomogeneous( imagePoints, _m );
cvConvert( A, &matA );
//变量类型检测
CV_Assert( (CV_MAT_DEPTH(rvec->type) == CV_64F || CV_MAT_DEPTH(rvec->type) == CV_32F) &&
(rvec->rows == 1 || rvec->cols == 1) && rvec->rows*rvec->cols*CV_MAT_CN(rvec->type) == 3 );
CV_Assert( (CV_MAT_DEPTH(tvec->type) == CV_64F || CV_MAT_DEPTH(tvec->type) == CV_32F) &&
(tvec->rows == 1 || tvec->cols == 1) && tvec->rows*tvec->cols*CV_MAT_CN(tvec->type) == 3 );
CV_Assert((count >= 4) || (count == 3 && useExtrinsicGuess)); // it is unsafe to call LM optimisation without an extrinsic guess in the case of 3 points. This is because there is no guarantee that it will converge on the correct solution.
_mn.reset(cvCreateMat( 1, count, CV_64FC2 ));
_Mxy.reset(cvCreateMat( 1, count, CV_64FC2 ));
// normalize image points
// (unapply the intrinsic matrix transformation and distortion)
//将点去除畸变，同时放到归一化坐标系（Ar是单位阵） 待解读
cvUndistortPoints( _m, _mn, &matA, distCoeffs, 0, &_Ar );
//如果给定了预先的外参
if( useExtrinsicGuess )
{
CvMat _r_temp = cvMat(rvec->rows, rvec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
CvMat _t_temp = cvMat(tvec->rows, tvec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3);
cvConvert( rvec, &_r_temp );
cvConvert( tvec, &_t_temp );
}
//没有给外参 就算一个外参
else
{
//求所有3d点的中心 均值 这里实际上把所有点的变换到，点中心是【0，0，0】的xy平面上，求了一个icp ref:Least-Squares Fitting of Two 3-D Point Sets
Mc = cvAvg(matM);
cvReshape( matM, matM, 1, count );
// Calculates _MM = (matM-_Mc)^T*(matM-_Mc)
cvMulTransposed( matM, &_MM, 1, &_Mc );
//svd 将_MM进行分解， _MM = UWV 其中U不输出， CV_SVD_MODIFY_A指定后_MM将改变 CV_SVD_V_T表示V是转置后生成的 （A，W,U,V,） 目的是看一看这些点的自由度是几维度的
cvSVD( &_MM, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
// 比较特征值
// 因为 我们相当于求的是 每个点相对于均值点的偏差，如果所有点位于同一个平面上，它们的差值将会只在x，y两个方向上存在数值，即只在这两个尺度上发生变化，SVD分解后，特征值对角上由于3d上的点（取了个根号），只在两个方向上存在自由度，所以只存在两个有意义的特征值，因此第三个特征值数量级很小很小。
if( W[2]/W[1] < 1e-3)
{
// a planar structure case (all M's lie in the same plane)
double tt[3], h[9], h1_norm, h2_norm;
CvMat* R_transform = &matV;
CvMat T_transform = cvMat( 3, 1, CV_64F, tt );
CvMat matH = cvMat( 3, 3, CV_64F, h );
CvMat _h1, _h2, _h3;
//这里求解要求A满秩，如果V最后一列为0，呢么A必不满秩，所有点在一条线上，线平移即可，不需要旋转
if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
cvSetIdentity( R_transform );
//求行列式，如果行列式小于0，就将R_transform取负
if( cvDet(R_transform) < 0 )
cvScale( R_transform, R_transform, -1 );
//dst = (alpha*src1)*src2+(beta*src3) CV_GEMM_B_T表示转置 src2 T_transform = - R_transform * _Mc.T
cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
//_Mxy 就是点平移后的阵，这时候是2d的，丢掉了第三维度
for( i = 0; i < count; i++ )
{
const double* Rp = R_transform->data.db;
const double* Tp = T_transform.data.db;
const double* src = matM->data.db + i*3;
double* dst = _Mxy->data.db + i*2;
dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0];
dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1];
}
//找到物体平面和图像（视图）平面之间的透视变换 matH是透视变换矩阵 本质是这个函数实现的 findHomography /calib3d/src/fundam.cpp
cvFindHomography( _Mxy, _mn, &matH );
//判断这个值怎么样，如果太大或者不合法，就把matR设置为单位阵，否则会给一个R 这块操作没看懂 我感觉 这里是一个归一化的过程，把点映射到归一化平面上
if( cvCheckArr(&matH, CV_CHECK_QUIET) )
{
//将_h1定义为h的第一列，_h2是第二列，_h3第三列
cvGetCol( &matH, &_h1, 0 );
_h2 = _h1; _h2.data.db++;
_h3 = _h2; _h3.data.db++;
h1_norm = std::sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]);
h2_norm = std::sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]);
//线性变换 cvConvertScale( const CvArr* src, CvArr* dst, double scale CV_DEFAULT(1), double shift CV_DEFAULT(0) ) dst = scale * src + shift
cvScale( &_h1, &_h1, 1./MAX(h1_norm, DBL_EPSILON) );
cvScale( &_h2, &_h2, 1./MAX(h2_norm, DBL_EPSILON) );
cvScale( &_h3, &_t, 2./MAX(h1_norm + h2_norm, DBL_EPSILON));
//求两个3d向量叉积
cvCrossProduct( &_h1, &_h2, &_h3 );
cvRodrigues2( &matH, &_r );
cvRodrigues2( &_r, &matH );
cvMatMulAdd( &matH, &T_transform, &_t, &_t );
cvMatMul( &matH, R_transform, &matR );
}
else
{
cvSetIdentity( &matR );
cvZero( &_t );
}
cvRodrigues2( &matR, &_r );
}
//畸变校准中 所有点 必位于同一个平面 不可能else 所以忽略
else
{
// non-planar structure. Use DLT method
CV_CheckGE(count, 6, "DLT algorithm needs at least 6 points for pose estimation from 3D-2D point correspondences.");
double* L;
double LL[12*12], LW[12], LV[12*12], sc;
CvMat _LL = cvMat( 12, 12, CV_64F, LL );
CvMat _LW = cvMat( 12, 1, CV_64F, LW );
CvMat _LV = cvMat( 12, 12, CV_64F, LV );
CvMat _RRt, _RR, _tt;
CvPoint3D64f* M = (CvPoint3D64f*)matM->data.db;
CvPoint2D64f* mn = (CvPoint2D64f*)_mn->data.db;
matL.reset(cvCreateMat( 2*count, 12, CV_64F ));
L = matL->data.db;
for( i = 0; i < count; i++, L += 24 )
{
double x = -mn[i].x, y = -mn[i].y;
L[0] = L[16] = M[i].x;
L[1] = L[17] = M[i].y;
L[2] = L[18] = M[i].z;
L[3] = L[19] = 1.;
L[4] = L[5] = L[6] = L[7] = 0.;
L[12] = L[13] = L[14] = L[15] = 0.;
L[8] = x*M[i].x;
L[9] = x*M[i].y;
L[10] = x*M[i].z;
L[11] = x;
L[20] = y*M[i].x;
L[21] = y*M[i].y;
L[22] = y*M[i].z;
L[23] = y;
}
cvMulTransposed( matL, &_LL, 1 );
cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
_RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
cvGetCols( &_RRt, &_RR, 0, 3 );
cvGetCol( &_RRt, &_tt, 3 );
if( cvDet(&_RR) < 0 )
cvScale( &_RRt, &_RRt, -1 );
sc = cvNorm(&_RR);
CV_Assert(fabs(sc) > DBL_EPSILON);
cvSVD( &_RR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T );
cvScale( &_tt, &_t, cvNorm(&matR)/sc );
cvRodrigues2( &matR, &_r );
}
}
cvReshape( matM, matM, 3, 1 );
cvReshape( _mn, _mn, 2, 1 );
//定义了一个求解器，采用LM优化R t
// refine extrinsic parameters using iterative algorithm 优化变量个数 样本个数 迭代停止标准 雅克比矩阵的对称原则，true 则 m_ij=m_ji i < j
CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true);
cvCopy( &_param, solver.param );
//把所有点放进去迭代求解
for(;;)
{
CvMat *matJ = 0, *_err = 0;
const CvMat *__param = 0;
bool proceed = solver.update( __param, matJ, _err );
cvCopy( __param, &_param );
if( !proceed || !_err )
break;
cvReshape( _err, _err, 2, 1 );
if( matJ )
{
cvGetCols( matJ, &_dpdr, 0, 3 );
cvGetCols( matJ, &_dpdt, 3, 6 );
cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
_err, &_dpdr, &_dpdt, 0, 0, 0 );
}
else
{
cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
_err, 0, 0, 0, 0, 0 );
}
cvSub(_err, _m, _err);
cvReshape( _err, _err, 1, 2*count );
}
cvCopy( solver.param, &_param );
_r = cvMat( rvec->rows, rvec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
_t = cvMat( tvec->rows, tvec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 );
cvConvert( &_r, rvec );
cvConvert( &_t, tvec );
}HomographyEstimatorCallback runKernel
//主要方法
int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
{
//输入参数，获取点的数量
Mat m1 = _m1.getMat(), m2 = _m2.getMat();
int i, count = m1.checkVector(2);
const Point2f* M = m1.ptr<Point2f>();
const Point2f* m = m2.ptr<Point2f>();
//变量初始化
double LtL[9][9], W[9][1], V[9][9];
Mat _LtL( 9, 9, CV_64F, &LtL[0][0] );
Mat matW( 9, 1, CV_64F, W );
Mat matV( 9, 9, CV_64F, V );
Mat _H0( 3, 3, CV_64F, V[8] );
Mat _Htemp( 3, 3, CV_64F, V[7] );
Point2d cM(0,0), cm(0,0), sM(0,0), sm(0,0);
//求和
for( i = 0; i < count; i++ )
{
cm.x += m[i].x; cm.y += m[i].y;
cM.x += M[i].x; cM.y += M[i].y;
}
//求平均
cm.x /= count;
cm.y /= count;
cM.x /= count;
cM.y /= count;
//求每个点相对于平均点的偏差
for( i = 0; i < count; i++ )
{
sm.x += fabs(m[i].x - cm.x);
sm.y += fabs(m[i].y - cm.y);
sM.x += fabs(M[i].x - cM.x);
sM.y += fabs(M[i].y - cM.y);
}
//如果所有点在一条线上，就没有意义
if( fabs(sm.x) < DBL_EPSILON || fabs(sm.y) < DBL_EPSILON ||
fabs(sM.x) < DBL_EPSILON || fabs(sM.y) < DBL_EPSILON )
return 0;
//求所有点相对于中心点偏差 的倒数sm 是么m2 sM 是 m1
sm.x = count/sm.x; sm.y = count/sm.y;
sM.x = count/sM.x; sM.y = count/sM.y;
double invHnorm[9] = { 1./sm.x, 0, cm.x, 0, 1./sm.y, cm.y, 0, 0, 1 };
double Hnorm2[9] = { sM.x, 0, -cM.x*sM.x, 0, sM.y, -cM.y*sM.y, 0, 0, 1 };
Mat _invHnorm( 3, 3, CV_64FC1, invHnorm );
Mat _Hnorm2( 3, 3, CV_64FC1, Hnorm2 );
_LtL.setTo(Scalar::all(0));
for( i = 0; i < count; i++ )
{
double x = (m[i].x - cm.x)*sm.x, y = (m[i].y - cm.y)*sm.y;
double X = (M[i].x - cM.x)*sM.x, Y = (M[i].y - cM.y)*sM.y;
double Lx[] = { X, Y, 1, 0, 0, 0, -x*X, -x*Y, -x };
double Ly[] = { 0, 0, 0, X, Y, 1, -y*X, -y*Y, -y };
int j, k;
for( j = 0; j < 9; j++ )
for( k = j; k < 9; k++ )
LtL[j][k] += Lx[j]*Lx[k] + Ly[j]*Ly[k];
}
completeSymm( _LtL );
eigen( _LtL, matW, matV );
_Htemp = _invHnorm*_H0;
_H0 = _Htemp*_Hnorm2;
_H0.convertTo(_model, _H0.type(), 1./_H0.at<double>(2,2) );
return 1;
}代码阅读基本到这里结束，省掉了3d到2d映射的求导检查以及n点透视变换的代码阅读。接下来会对omni的双目标定进行检查，重写相关代码。
今天的文章cv2.imread函数_cv2.imread函数分享到此就结束了，感谢您的阅读。
版权声明：本文内容由互联网用户自发贡献，该文观点仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 举报，一经查实，本站将立刻删除。
如需转载请保留出处：https://bianchenghao.cn/63445.html
