OpenCV内部函数cvFindExtrinsicCameraParams2解析（一）

背景介绍

在opencv相机标定函数calibrateCamera中，根据标定板上特征点的3D坐标，以及对应的图像2D坐标，计算每个拍摄位置的初始位姿，以便后续的优化求解最终的内、外参数。cvFindExtrinsicCameraParams2函数中包含了两种外参数的估计方法（特征点在一个面上、特征点不在一个面上），以及迭代计算。该函数也是solvePnP函数选用CV_ITERATIVE时的主要执行方法。本文对cvFindExtrinsicCameraParams2函数中特征点在一个平面上时的外参估计方法进行解析，这个方法是平面标定板会执行的路线。

原理解析

因为是标定板上的特征点，3D的点近似在一个平面内，而图像2D点也是在平面上，很容易想到利用平面单应性的原理来解算3D-2D之间的关系。因此，第一步将是把3D点的坐标进行降维，变为2D点；然后第二步2D-2D点的单应性计算；最后第三步把降维矩阵和单应性矩阵汇总，得到我们想要的拍摄位姿。下面使用和代码中对应的符号来表达数据或矩阵，只是用于公式表达，和代码实际结构不一定相同，如代码中用齐次坐标。

倘若标定板上的点坐标一开始定义就是z=0z=0z=0，当然第一步就没有太多意义。而对于通用的情况，世界坐标系不在标定板上时，对于点集{matM}={xi,yi,zi}T\lbrace{matM\rbrace}=\lbrace{x_i,y_i,z_i\rbrace}^T{matM}={xi,yi,zi}T去中心化坐标{matM−Mc}={xi−xˉ,yi−yˉ,zi−zˉ}T\lbrace{matM-Mc\rbrace}=\lbrace{x_i-\bar{x},y_i-\bar{y},z_i-\bar{z}\rbrace}^T{matM−Mc}={xi−xˉ,yi−yˉ,zi−zˉ}T每个点都减去均值。协方差矩阵为MM=(matM−Mc)T(matM−Mc)MM =(matM-Mc)^T(matM-Mc)MM=(matM−Mc)T(matM−Mc)那么对协方差矩阵MMMMMM进行SVD分解，matVmatVmatV的列就是特征向量，具体理论可以参考这个博文PCA，SVD以及协方差矩阵的关系。我们想把三维的{matM−Mc}\lbrace{matM-Mc\rbrace}{matM−Mc}压缩为二维，就是取特征值最大的前两个特征向量，也就是取matVmatVmatV矩阵的前两列（V11,V21,V31)和(V12,V22,V32)（V_{11},V_{21},V_{31})和(V_{12},V_{22},V_{32})（V11,V21,V31)和(V12,V22,V32)。
ui=V11(xi−xˉ)+V21(yi−yˉ)+V31(zi−zˉ)vi=V12(xi−xˉ)+V22(yi−yˉ)+V32(zi−zˉ)u_i = V_{11}(x_i-\bar{x})+V_{21}(y_i-\bar{y})+V_{31}(z_i-\bar{z}) \\[2ex] v_i = V_{12}(x_i-\bar{x})+V_{22}(y_i-\bar{y})+V_{32}(z_i-\bar{z}) ui=V11(xi−xˉ)+V21(yi−yˉ)+V31(zi−zˉ)vi=V12(xi−xˉ)+V22(yi−yˉ)+V32(zi−zˉ)由此，将三维坐标matM={xi,yi,zi}matM=\lbrace{x_i,y_i,z_i\rbrace}matM={xi,yi,zi}转为二维Mxy={ui,vi}Mxy=\lbrace{u_i,v_i\rbrace}Mxy={ui,vi}。三维点如果近似在一个平面内，SVD分解得到的对角线矩阵matWmatWmatW上的元素就是特征值，最后一个特征值相对来说很小，对应的matVmatVmatV的第三列特征向量有
wi=V13(xi−xˉ)+V23(yi−yˉ)+V33(zi−zˉ)w_i =V_{13}(x_i-\bar{x})+V_{23}(y_i-\bar{y})+V_{33}(z_i-\bar{z}) wi=V13(xi−xˉ)+V23(yi−yˉ)+V33(zi−zˉ)得到的wiw_iwi接近于0，就相当于把世界坐标系挪到了标定板上面来，并且zzz轴垂直于标定板。
根据二维Mxy={ui,vi}Mxy=\lbrace{u_i,v_i\rbrace}Mxy={ui,vi}和图像上的特征点去除畸变的坐标mn={ai,bi}mn=\lbrace{a_i,b_i\rbrace}mn={ai,bi}，求解单应性矩阵HHH，可参考我之前的博文平面单应性变换
第2步得到了标定板坐标系与像平面的单应性变化HHH，进一步地能得到标定板坐标系与相机坐标系的变换RTCameraCalibPlaneRT^{CalibPlane}_{Camera}RTCameraCalibPlane(这部分的转换不在这里解析)。第1步虽然名义上的数据降维，其实也可以认为是世界坐标系到标定板坐标系的变换，
[uiviwi]=[V11V21V31V12V22V32V13V23V33][xiyizi]−[V11V21V31V12V22V32V13V23V33][xˉyˉzˉ]\begin{bmatrix} u_i \\ v_i \\ w_i \\ \end{bmatrix}=\begin{bmatrix} V_{11} & V_{21} & V_{31} \\ V_{12} & V_{22} & V_{32} \\ V_{13} & V_{23} & V_{33} \\ \end{bmatrix}\begin{bmatrix} x_i \\ y_i \\ z_i \\ \end{bmatrix} - \begin{bmatrix} V_{11} & V_{21} & V_{31} \\ V_{12} & V_{22} & V_{32} \\ V_{13} & V_{23} & V_{33} \\ \end{bmatrix}\begin{bmatrix} \bar{x} \\ \bar{y} \\ \bar{z} \\ \end{bmatrix} ⎣⎡uiviwi⎦⎤=⎣⎡V11V12V13V21V22V23V31V32V33⎦⎤⎣⎡xiyizi⎦⎤−⎣⎡V11V12V13V21V22V23V31V32V33⎦⎤⎣⎡xˉyˉzˉ⎦⎤用上面的符号来改写成矩阵形式就是
Mxy=matV∗matM−matV∗McMxy=matV*matM-matV*Mc Mxy=matV∗matM−matV∗Mc写成Mxy=R∗matM+TMxy=R*matM+TMxy=R∗matM+T的形式的话，其中Rtransform=matVRtransform=matVRtransform=matV, Ttransform=−matV∗McTtransform=-matV*McTtransform=−matV∗Mc。由第1步可得到世界坐标系到标定板坐标系的转换RTCalibPlaneWorldRT^{World}_{CalibPlane}RTCalibPlaneWorld，结合前面的矩阵得到世界坐标系到相机坐标系的转换，也就是得到了我们想要的当前拍摄的位姿
RTCameraWorld=RTCalibPlaneWorld∗RTCameraCalibPlaneRT^{World}_{Camera}=RT^{World}_{CalibPlane}*RT^{CalibPlane}_{Camera} RTCameraWorld=RTCalibPlaneWorld∗RTCameraCalibPlane

代码注释

代码部分，我暂且将无关的部分删除，对涉及的主要部分添加说明，我加入的注释都用// **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分解得到matVcvSVD( &_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 = matVCvMat* 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*MccvGEMM( 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 );
}