OpenCV内部函数cvFindExtrinsicCameraParams2解析（一）

背景介绍

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

原理解析

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

1. 倘若标定板上的点坐标一开始定义就是$z=0$，当然第一步就没有太多意义。而对于通用的情况，世界坐标系不在标定板上时，对于点集$$\{matM\}=\{{x_i,y_i,z_i\}}^T$$去中心化坐标$$\{matM-Mc\}=\{{x_i-\bar{x},y_i-\bar{y},z_i-\bar{z}\}}^T$$每个点都减去均值。协方差矩阵为$$MM=(matM-Mc{)}^T(matM-Mc)$$那么对协方差矩阵$MM$进行SVD分解，$matV$的列就是特征向量，具体理论可以参考这个博文PCA，SVD以及协方差矩阵的关系。我们想把三维的$\{matM-Mc\}$压缩为二维，就是取特征值最大的前两个特征向量，也就是取$matV$矩阵的前两列$（V_{11},V_{21},V_{31})和(V_{12},V_{22},V_{32})$。
   $$\begin{gathered} u_i=V_{11}(x_i-\bar{x})+V_{21}(y_i-\bar{y})+V_{31}(z_i-\bar{z}) \\ {v}_i=V_{12}(x_i-\bar{x})+V_{22}(y_i-\bar{y})+V_{32}(z_i-\bar{z}) \end{gathered}$$由此，将三维坐标$matM=\{x_i,y_i,z_i\}$转为二维$Mxy=\{u_i,v_i\}$。三维点如果近似在一个平面内，SVD分解得到的对角线矩阵$matW$上的元素就是特征值，最后一个特征值相对来说很小，对应的$matV$的第三列特征向量有
   $$w_i=V_{13}(x_i-\bar{x})+V_{23}(y_i-\bar{y})+V_{33}(z_i-\bar{z})$$得到的$w_i$接近于0，就相当于把世界坐标系挪到了标定板上面来，并且$z$轴垂直于标定板。
2. 根据二维$Mxy=\{u_i,v_i\}$和图像上的特征点去除畸变的坐标$mn=\{a_i,b_i\}$，求解单应性矩阵$H$，可参考我之前的博文平面单应性变换
3. 第2步得到了标定板坐标系与像平面的单应性变化$H$，进一步地能得到标定板坐标系与相机坐标系的变换$R{T}_{Camera}^{CalibPlane}$(这部分的转换不在这里解析)。第1步虽然名义上的数据降维，其实也可以认为是世界坐标系到标定板坐标系的变换，
   $$\left[\begin{array}{c}{u}_i\\ v_i\\ w_i\end{array}\right]=\left[\begin{array}{ccc}{V}_{11}& V_{21}& V_{31}\\ V_{12}& V_{22}& V_{32}\\ V_{13}& V_{23}& V_{33}\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]-\left[\begin{array}{ccc}{V}_{11}& V_{21}& V_{31}\\ V_{12}& V_{22}& V_{32}\\ V_{13}& V_{23}& V_{33}\end{array}\right]\left[\begin{array}{c}\bar{x}\\ \bar{y}\\ \bar{z}\end{array}\right]$$用上面的符号来改写成矩阵形式就是
   $$Mxy=matV\ast matM-matV\ast Mc$$写成$Mxy=R\ast matM+T$的形式的话，其中$Rtransform=matV$, $Ttransform=-matV\ast Mc$。由第1步可得到世界坐标系到标定板坐标系的转换$R{T}_{CalibPlane}^{World}$，结合前面的矩阵得到世界坐标系到相机坐标系的转换，也就是得到了我们想要的当前拍摄的位姿
   $$R{T}_{Camera}^{World}=R{T}_{CalibPlane}^{World}\ast R{T}_{Camera}^{CalibPlane}$$

代码注释

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