OpenCV内部函数cvFindExtrinsicCameraParams2解析（二）

背景介绍

在上一篇cvFindExtrinsicCameraParams2解析（一）中，对cvFindExtrinsicCameraParams2函数中特征点在一个平面上时的外参估计方法进行解析，这个方法是平面标定板会执行的路线。本文对该函数中，当特征点不在同一个平面上时外参数估计的执行路线解析，使用直接线性变换DLT方法，值得注意的是，opencv这里是在有初始内参矩阵的情况下做外参数的估计，若此处内参数也未知，那么DLT方法的结果将要先RQ分解，将内参矩阵和外参矩阵分离出来，可参考我这一篇博文《摄像机矩阵的分解》。

原理解析

根据我们在opencv重建解析中的推导，该文中，投影矩阵得到的公式（7）是已知所有外参数，求解三维点坐标$(X_w,Y_w,Z_w)$，而在本文中，就是已知三维点坐标$(X_w,Y_w,Z_w)$和归一化平面并去畸变的$(x_c^{\prime },y_c^{\prime })$，求解RT参数。重新写一下这个式子
$$\{\begin{array}{c}(x_c^{\prime }{R}_{31}-R_{11})x_w+(x_c^{\prime }{R}_{32}-R_{12})y_w+(x_c^{\prime }{R}_{33}-R_{13})z_w+(x_c^{\prime }{T}_z-T_x)=0\\ (y_c^{\prime }{R}_{31}-R_{21})x_w+(y_c^{\prime }{R}_{32}-R_{22})y_w+(y_c^{\prime }{R}_{33}-R_{23})z_w+(y_c^{\prime }{T}_z-T_y)=0\end{array}\text{\hspace{1em}(1)}$$
opencv源代码中的正负号不同，是如下形式（后面也都用和代码中相对应的符号表示矩阵元素，这里用$RRt$表示3行4列的RT矩阵）
$$\{\begin{array}{c}(RR{t}_{11}-x_c^{\prime }RR{t}_{31})x_w+(RR{t}_{12}-x_c^{\prime }RR{t}_{32})y_w+(RR{t}_{13}-x_c^{\prime }RR{t}_{33})z_w+(RR{t}_{14}-x_c^{\prime }RR{t}_{34})=0\\ (RR{t}_{21}-y_c^{\prime }RR{t}_{31})x_w+(RR{t}_{22}-y_c^{\prime }RR{t}_{32})y_w+(RR{t}_{23}-y_c^{\prime }RR{t}_{33})z_w+(RR{t}_{24}-y_c^{\prime }RR{t}_{34})=0\end{array}\text{\hspace{1em}(2)}$$进一步地, 可写成矩阵形式
$$\left[\begin{array}{cccccccccccc}{x}_w& y_w& z_w& 1.0& 0.0& 0.0& 0.0& 0.0& -x_c^{\prime }{x}_w& -x_c^{\prime }{y}_w& -x_c^{\prime }{z}_w& -x_c^{\prime }\\ 0.0& 0.0& 0.0& 0.0& x_w& y_w& z_w& 1.0& -y_c^{\prime }{x}_w& -y_c^{\prime }{y}_w& -y_c^{\prime }{z}_w& -y_c^{\prime }\\ ...& & & & & & & & & & & \\ ...& & & & & & & & & & & \end{array}\right]\left[\begin{array}{c}RR{t}_{11}\\ RR{t}_{12}\\ RR{t}_{13}\\ RR{t}_{14}\\ RR{t}_{21}\\ RR{t}_{22}\\ RR{t}_{23}\\ RR{t}_{24}\\ RR{t}_{31}\\ RR{t}_{32}\\ RR{t}_{33}\\ RR{t}_{34}\end{array}\right]=\left[\begin{array}{c}0\end{array}\right]\text{\hspace{1em}(3)}$$也就是说每一个3D-2D点对，能够提供两个等式，而未知数有12个，至少需要6组数据。式（3）是形如$AX=0$的方程组，系数矩阵A有2*n行，12列。Opencv中用$L_{(2\ast n)\ast 12}$矩阵表示该系数矩阵，因为$LX=0$与$L^{T}LX=0$是同解的，Opencv对$L^{T}L$做SVD分解为$(LU{)}_{12\ast 12}{W}_{对角阵}(LV{)}_{12\ast 12}$，$L{V}_{12\ast 12}$的最后一列$RR{t}_{12\ast 1}$是方程的解。
但是，直接线性变换求解的结果，也就是这个12行1列的向量，因为该列向量本身就是单位向量，所以转换得到的$RR{t}_{3\ast 4}$并不是我们需要的外参矩阵，外参矩阵的分解$$RRt=[RR|tt]\text{\hspace{1em}(4)}$$它的旋转矩阵元素$RR$不满足单位正交的特性。形如’Ax=0’的解，可以看出来等式两边乘一个系数还是相等，所以式（3）SVD分解结果并不是我们想要的正解，设正解为$matRt=[matR|t]$，正解和$RRt=[RR|tt]$相差一个尺度$\beta$, $$\begin{gathered} matRt=\beta \ast RRt \\ matR=\beta \ast RR \\ t=\beta \ast tt\text{\hspace{1em}(5)} \end{gathered}$$为了求解matR, 可以认为是寻找无限接近$\beta \ast RR$的近似解$$matR=arg\min ||matR-\beta \ast RR|{|}_{\mathrm{F}}$$这一部分的推导可以借鉴在ICP中解R矩阵的过程，可参考SVD解算旋转矩阵和PnP解析，推导过程如下，$$||matR-\beta \ast RR|{|}_F=\sqrt{tr(matR-\beta \ast RR{)}^T(matR-\beta \ast RR)}\text{\hspace{1em}(6)}$$F范数就是矩阵所有元素的平方之和开根号，可等价的写成式（6）形式。进一步地，对RR做SVD分解$$\begin{gathered} ||matR-\beta \ast RR|{|}_F=\sqrt{tr(matR-\beta \ast UW{V}^T{)}^T(matR-\beta \ast UW{V}^T)} \\ =\sqrt{tr(mat{R}^{T}matR-2\ast mat{R}^T\ast \beta \ast UW{V}^T+{\beta }^2(UW{V}^T{)}^T(UW{V}^T))} \\ =\sqrt{tr(I-2\ast mat{R}^T\ast \beta \ast UW{V}^T+{\beta }^2(UW{V}^T{)}^T(UW{V}^T))}\text{\hspace{1em}(7)} \end{gathered}$$最后的式子根号下只有一个分量与matR相关，其他的都是常数，因此$$\text{min}||matR-\beta \ast RR||=\text{min}||-mat{R}^T\ast UW{V}^T||=\text{max}||mat{R}^T\ast UW{V}^T||\text{\hspace{1em}(8)}$$最后,$$matR=U{V}^T\text{\hspace{1em}(9)}$$根据式（5）可解算尺度为$$\beta =\frac{||matR|{|}_F}{||RR|{|}_F}\text{\hspace{1em}(10)}$$, 平移向量可计算$$t=\beta \ast tt\text{\hspace{1em}(11)}$$

代码注释

代码部分，我暂且将无关的部分删除，对涉及的主要部分添加说明，我加入的注释都用// **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)
        }
         else
        {
            // non-planar structure. Use DLT method
            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系数矩阵
                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;
            }
			// **LL = matL^T*matL
            cvMulTransposed( matL, &_LL, 1 );
            // **SVD分解LL矩阵
            cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T );
            // **取LL分解得到的LV的最后一列12个元素，赋值给3行4列_RRt矩阵
            _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 );
            // **取_RRt的前3列组成_RR矩阵
            cvGetCols( &_RRt, &_RR, 0, 3 );
            // **取_RRt的第4列组成_tt
            cvGetCol( &_RRt, &_tt, 3 );
            if( cvDet(&_RR) < 0 )
                cvScale( &_RRt, &_RRt, -1 );
            // **计算_RR矩阵的F范数，opencv中是CV_L2，所有元素平方和开根号    
            sc = cvNorm(&_RR);
            // **svd分解_RR矩阵    
            cvSVD( &_RR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T );
            // **正解matR=matU*matV^T
            cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T );
            // **正解_t=尺度*_tt，其中尺度就是cvNorm(&matR)/sc，matR的F范数/RR的F范数
            cvScale( &_tt, &_t, cvNorm(&matR)/sc );
            // **罗德里格斯转换，将旋转矩阵转为角轴表示
            cvRodrigues2( &matR, &_r );
        }
    }
    // 删除了迭代计算
    
    cvConvert( &_r, rvec );
    cvConvert( &_t, tvec );
}