背景介绍
在opencv相机标定函数calibrateCamera中,根据标定板上特征点的3D坐标,以及对应的图像2D坐标,计算每个拍摄位置的初始位姿,以便后续的优化求解最终的内、外参数。cvFindExtrinsicCameraParams2函数中包含了两种外参数的估计方法(特征点在一个面上、特征点不在一个面上),以及迭代计算。该函数也是solvePnP函数选用CV_ITERATIVE时的主要执行方法。本文对cvFindExtrinsicCameraParams2函数中特征点在一个平面上时的外参估计方法进行解析,这个方法是平面标定板会执行的路线。
原理解析
因为是标定板上的特征点,3D的点近似在一个平面内,而图像2D点也是在平面上,很容易想到利用平面单应性的原理来解算3D-2D之间的关系。因此,第一步将是把3D点的坐标进行降维,变为2D点;然后第二步2D-2D点的单应性计算;最后第三步把降维矩阵和单应性矩阵汇总,得到我们想要的拍摄位姿。下面使用和代码中对应的符号来表达数据或矩阵,只是用于公式表达,和代码实际结构不一定相同,如代码中用齐次坐标。
- 倘若标定板上的点坐标一开始定义就是
,当然第一步就没有太多意义。而对于通用的情况,世界坐标系不在标定板上时,对于点集
去中心化坐标
每个点都减去均值。协方差矩阵为
那么对协方差矩阵
进行SVD分解,
的列就是特征向量,具体理论可以参考这个博文PCA,SVD以及协方差矩阵的关系。我们想把三维的
压缩为二维,就是取特征值最大的前两个特征向量,也就是取
矩阵的前两列
。
由此,将三维坐标
转为二维
。三维点如果近似在一个平面内,SVD分解得到的对角线矩阵
上的元素就是特征值,最后一个特征值相对来说很小,对应的
的第三列特征向量有
得到的
接近于0,就相当于把世界坐标系挪到了标定板上面来,并且
轴垂直于标定板。
- 根据二维
和图像上的特征点去除畸变的坐标
,求解单应性矩阵
,可参考我之前的博文平面单应性变换
- 第2步得到了标定板坐标系与像平面的单应性变化
,进一步地能得到标定板坐标系与相机坐标系的变换
(这部分的转换不在这里解析)。第1步虽然名义上的数据降维,其实也可以认为是世界坐标系到标定板坐标系的变换,
用上面的符号来改写成矩阵形式就是
写成
的形式的话,其中
,
。由第1步可得到世界坐标系到标定板坐标系的转换
,结合前面的矩阵得到世界坐标系到相机坐标系的转换,也就是得到了我们想要的当前拍摄的位姿
代码注释
代码部分,我暂且将无关的部分删除,对涉及的主要部分添加说明,我加入的注释都用// **xx 的形式
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];
CvScalar 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;
// **删除了一些判断和初始化
cvConvertPointsHomogeneous( objectPoints, matM );
cvConvertPointsHomogeneous( imagePoints, _m );
// **删除了一些判断和初始化
// normalize image points
// **去图像点畸变
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点的均值中心坐标
Mc = cvAvg(matM);
cvReshape( matM, matM, 1, count );
// **_MM = (matM-_Mc)^T * (matM-_Mc)协方差矩阵
cvMulTransposed( matM, &_MM, 1, &_Mc );
// SVD分解得到matV
cvSVD( &_MM, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );
// initialize extrinsic parameters
// **从前面的初始化可知matW与W是关联的,SVD得到的对角线矩阵的元素是特征值,若3D点在近似平面上,第3个特征值相对来说很小
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;
// **世界坐标系到标定板坐标系的R矩阵就是SVD分解的V,参见原理解析第3步
// **R_transform = matV
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;
if( V[2]*V[2] + V[5]*V[5] < 1e-10 )
cvSetIdentity( R_transform );
if( cvDet(R_transform) < 0 )
cvScale( R_transform, R_transform, -1 );
// **世界坐标系到标定板坐标系的T矩阵求解,参见原理解析第3步
// **T_transform=-matV*Mc
cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T );
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];
}
// **解算标定板平面与像平面的单应性变换
cvFindHomography( _Mxy, _mn, &matH );
if( cvCheckArr(&matH, CV_CHECK_QUIET) )
{
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]);
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));
cvCrossProduct( &_h1, &_h2, &_h3 );
// **单应性矩阵求解变换RT的过程(本文不做解析)
cvRodrigues2( &matH, &_r );
cvRodrigues2( &_r, &matH );
// **综合第1步和第2步的变换矩阵,得到拍摄位姿
cvMatMulAdd( &matH, &T_transform, &_t, &_t );
cvMatMul( &matH, R_transform, &matR );
}
else
{
cvSetIdentity( &matR );
cvZero( &_t );
}
// **罗德里格斯转换,矩阵用向量角表达
cvRodrigues2( &matR, &_r );
}
else
{
// non-planar structure. Use DLT method
}
}
// 删除了迭代计算
cvConvert( &_r, rvec );
cvConvert( &_t, tvec );
}