OpenCV内部函数cvFindExtrinsicCameraParams2解析(二)

背景介绍

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

原理解析

根据我们在opencv重建解析中的推导,该文中,投影矩阵得到的公式(7)是已知所有外参数,求解三维点坐标 ( X w , Y w , Z w ) (X_w,Y_w,Z_w) (Xw,Yw,Zw),而在本文中,就是已知三维点坐标 ( X w , Y w , Z w ) (X_w,Y_w,Z_w) (Xw,Yw,Zw)和归一化平面并去畸变的 ( x c ′ , y c ′ ) (x'_{c},y'_{c}) (xc,yc),求解RT参数。重新写一下这个式子
{ ( x c ′ R 31 − R 11 ) x w + ( x c ′ R 32 − R 12 ) y w + ( x c ′ R 33 − R 13 ) z w + ( x c ′ T z − T x ) = 0 ( y c ′ R 31 − R 21 ) x w + ( y c ′ R 32 − R 22 ) y w + ( y c ′ R 33 − R 23 ) z w + ( y c ′ T z − T y ) = 0 (1) \left\{ \begin{array}{c} (x'_{c}R_{31}-R_{11})x_w+(x'_{c}R_{32}-R_{12})y_w+(x'_{c}R_{33}-R_{13})z_w +(x'_{c}T_z-T_x)= 0 \\ (y'_{c}R_{31}-R_{21})x_w+(y'_{c}R_{32}-R_{22})y_w+(y'_{c}R_{33}-R_{23})z_w +(y'_{c}T_z-T_y)= 0\\ \end{array} \right.\tag{1} {(xcR31R11)xw+(xcR32R12)yw+(xcR33R13)zw+(xcTzTx)=0(ycR31R21)xw+(ycR32R22)yw+(ycR33R23)zw+(ycTzTy)=0(1)
opencv源代码中的正负号不同,是如下形式(后面也都用和代码中相对应的符号表示矩阵元素,这里用 R R t RRt RRt表示3行4列的RT矩阵)
{ ( R R t 11 − x c ′ R R t 31 ) x w + ( R R t 12 − x c ′ R R t 32 ) y w + ( R R t 13 − x c ′ R R t 33 ) z w + ( R R t 14 − x c ′ R R t 34 ) = 0 ( R R t 21 − y c ′ R R t 31 ) x w + ( R R t 22 − y c ′ R R t 32 ) y w + ( R R t 23 − y c ′ R R t 33 ) z w + ( R R t 24 − y c ′ R R t 34 ) = 0 (2) \left\{ \begin{array}{c} (RRt_{11}-x'_{c}RRt_{31})x_w+(RRt_{12}-x'_{c}RRt_{32})y_w+(RRt_{13}-x'_{c}RRt_{33})z_w +(RRt_{14}-x'_{c}RRt_{34})= 0 \\ (RRt_{21}-y'_{c}RRt_{31})x_w+(RRt_{22}-y'_{c}RRt_{32})y_w+(RRt_{23}-y'_{c}RRt_{33})z_w +(RRt_{24}-y'_{c}RRt_{34})= 0 \\ \end{array} \right. \tag{2} {(RRt11xcRRt31)xw+(RRt12xcRRt32)yw+(RRt13xcRRt33)zw+(RRt14xcRRt34)=0(RRt21ycRRt31)xw+(RRt22ycRRt32)yw+(RRt23ycRRt33)zw+(RRt24ycRRt34)=0(2)进一步地, 可写成矩阵形式
[ x w y w z w 1.0 0.0 0.0 0.0 0.0 − x c ′ x w − x c ′ y w − x c ′ z w − x c ′ 0.0 0.0 0.0 0.0 x w y w z w 1.0 − y c ′ x w − y c ′ y w − y c ′ z w − y c ′ . . . . . . ] [ R R t 11 R R t 12 R R t 13 R R t 14 R R t 21 R R t 22 R R t 23 R R t 24 R R t 31 R R t 32 R R t 33 R R t 34 ] = [ 0 ] (3) \begin{bmatrix} x_w & y_w & z_w & 1.0 & 0.0 & 0.0 & 0.0 & 0.0 &-x'_cx_w& -x'_cy_w&-x'_cz_w&-x'_c\\ 0.0 & 0.0 & 0.0 & 0.0&x_w &y_w & z_w & 1.0 &-y'_cx_w& -y'_cy_w&-y'_cz_w&-y'_c\\ ...\\... \end{bmatrix} \begin{bmatrix} RRt_{11} \\ RRt_{12} \\ RRt_{13} \\ RRt_{14} \\ RRt_{21} \\ RRt_{22} \\ RRt_{23}\\RRt_{24}\\ RRt_{31}\\ RRt_{32}\\RRt_{33}\\RRt_{34}\\\end{bmatrix} = \begin{bmatrix} 0 \\ \end{bmatrix} \tag{3} xw0.0......yw0.0zw0.01.00.00.0xw0.0yw0.0zw0.01.0xcxwycxwxcywycywxczwyczwxcyc RRt11RRt12RRt13RRt14RRt21RRt22RRt23RRt24RRt31RRt32RRt33RRt34 =[0](3)也就是说每一个3D-2D点对,能够提供两个等式,而未知数有12个,至少需要6组数据。式(3)是形如 A X = 0 AX=0 AX=0的方程组,系数矩阵A有2*n行,12列。Opencv中用 L ( 2 ∗ n ) ∗ 12 L_{(2*n)*12} L(2n)12矩阵表示该系数矩阵,因为 L X = 0 LX=0 LX=0 L T L X = 0 L^TLX=0 LTLX=0是同解的,Opencv对 L T L L^TL LTL做SVD分解为 ( L U ) 12 ∗ 12 W 对角阵 ( L V ) 12 ∗ 12 (LU)_{12*12}W_{对角阵}(LV)_{12*12} (LU)1212W对角阵(LV)1212 L V 12 ∗ 12 LV_{12*12} LV1212的最后一列 R R t 12 ∗ 1 RRt_{12*1} RRt121是方程的解。
但是,直接线性变换求解的结果,也就是这个12行1列的向量,因为该列向量本身就是单位向量,所以转换得到的 R R t 3 ∗ 4 RRt_{3*4} RRt34并不是我们需要的外参矩阵,外参矩阵的分解 R R t = [ R R ∣ t t ] (4) RRt=[RR|tt] \tag{4} RRt=[RRtt](4)它的旋转矩阵元素 R R RR RR不满足单位正交的特性。形如’Ax=0’的解,可以看出来等式两边乘一个系数还是相等,所以式(3)SVD分解结果并不是我们想要的正解,设正解为 m a t R t = [ m a t R ∣ t ] matRt=[matR|t] matRt=[matRt],正解和 R R t = [ R R ∣ t t ] RRt=[RR|tt] RRt=[RRtt]相差一个尺度 β \beta β, m a t R t = β ∗ R R t m a t R = β ∗ R R t = β ∗ t t (5) matRt= \beta*RRt \tag{5} \\ matR=\beta*RR\\ t=\beta*tt matRt=βRRtmatR=βRRt=βtt(5)为了求解matR, 可以认为是寻找无限接近 β ∗ R R \beta*RR βRR的近似解 m a t R = a r g m i n ∣ ∣ m a t R − β ∗ R R ∣ ∣ F matR=arg\rm{min}||matR-\beta*RR||_F matR=argmin∣∣matRβRR∣F这一部分的推导可以借鉴在ICP中解R矩阵的过程,可参考SVD解算旋转矩阵和PnP解析,推导过程如下, ∣ ∣ m a t R − β ∗ R R ∣ ∣ F = t r ( m a t R − β ∗ R R ) T ( m a t R − β ∗ R R ) (6) ||matR-\beta*RR||_F=\sqrt{tr(matR-\beta*RR)^T(matR-\beta*RR)} \tag{6} ∣∣matRβRRF=tr(matRβRR)T(matRβRR) (6)F范数就是矩阵所有元素的平方之和开根号,可等价的写成式(6)形式。进一步地,对RR做SVD分解 ∣ ∣ m a t R − β ∗ R R ∣ ∣ F = t r ( m a t R − β ∗ U W V T ) T ( m a t R − β ∗ U W V T ) = t r ( m a t R T m a t R − 2 ∗ m a t R T ∗ β ∗ U W V T + β 2 ( U W V T ) T ( U W V T ) ) = t r ( I − 2 ∗ m a t R T ∗ β ∗ U W V T + β 2 ( U W V T ) T ( U W V T ) ) (7) ||matR-\beta*RR||_F=\sqrt{tr(matR-\beta*UWV^T)^T(matR-\beta*UWV^T)}\\ =\sqrt{tr(matR^TmatR-2*matR^T*\beta*UWV^T+\beta^2(UWV^T)^T(UWV^T))} \\ =\sqrt{tr(I-2*matR^T*\beta*UWV^T+\beta^2(UWV^T)^T(UWV^T))} \tag{7} ∣∣matRβRRF=tr(matRβUWVT)T(matRβUWVT) =tr(matRTmatR2matRTβUWVT+β2(UWVT)T(UWVT)) =tr(I2matRTβUWVT+β2(UWVT)T(UWVT)) (7)最后的式子根号下只有一个分量与matR相关,其他的都是常数,因此 min ∣ ∣ m a t R − β ∗ R R ∣ ∣ = min ∣ ∣ − m a t R T ∗ U W V T ∣ ∣ = max ∣ ∣ m a t R T ∗ U W V T ∣ ∣ (8) \text{min}||matR-\beta*RR||=\text{min}||-matR^T*UWV^T||=\text{max}||matR^T*UWV^T||\tag{8} min∣∣matRβRR∣∣=min∣∣matRTUWVT∣∣=max∣∣matRTUWVT∣∣(8)最后, m a t R = U V T (9) matR=UV^T\tag{9} matR=UVT(9)根据式(5)可解算尺度为 β = ∣ ∣ m a t R ∣ ∣ F ∣ ∣ R R ∣ ∣ F (10) \beta=\frac{||matR||_F}{||RR||_F}\tag{10} β=∣∣RRF∣∣matRF(10), 平移向量可计算 t = β ∗ t t (11) t=\beta*tt\tag{11} t=βtt(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 );
}

你可能感兴趣的:(结构光三维重建,opencv,计算机视觉,机器学习)