OpenCV内部函数cvFindExtrinsicCameraParams2解析（一）

背景介绍

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

原理解析

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

1. 倘若标定板上的点坐标一开始定义就是z = 0 z=0 z =0，当然第一步就没有太多意义。而对于通用的情况，世界坐标系不在标定板上时，对于点集{ m a t M } = { x i , y i , z i } T \lbrace{matM\rbrace}=\lbrace{x_i,y_i,z_i\rbrace}^T {ma tM }={x i ​,y i ​,z i ​}T去中心化坐标{ m a t M − M c } = { x i − x ˉ , y i − y ˉ , z i − z ˉ } T \lbrace{matM-Mc\rbrace}=\lbrace{x_i-\bar{x},y_i-\bar{y},z_i-\bar{z}\rbrace}^T {ma tM −M c }={x i ​−x ˉ,y i ​−y ˉ​,z i ​−z ˉ}T每个点都减去均值。协方差矩阵为M M = ( m a t M − M c ) T ( m a t M − M c ) MM =(matM-Mc)^T(matM-Mc)MM =(ma tM −M c )T (ma tM −M c )那么对协方差矩阵M M MM MM进行SVD分解，m a t V matV ma t V的列就是特征向量，具体理论可以参考这个博文PCA，SVD以及协方差矩阵的关系。我们想把三维的{ m a t M − M c } \lbrace{matM-Mc\rbrace}{ma tM −M c }压缩为二维，就是取特征值最大的前两个特征向量，也就是取m a t V matV ma t V矩阵的前两列（ V 11 , V 21 , V 31 ) 和 ( V 12 , V 22 , V 32 ) （V_{11},V_{21},V_{31})和(V_{12},V_{22},V_{32})（V 11 ​,V 21 ​,V 31 ​)和(V 12 ​,V 22 ​,V 32 ​)。
   u i = V 11 ( x i − x ˉ ) + V 21 ( y i − y ˉ ) + V 31 ( z i − z ˉ ) v i = V 12 ( x i − x ˉ ) + V 22 ( y i − y ˉ ) + V 32 ( z i − 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})u i ​=V 11 ​(x i ​−x ˉ)+V 21 ​(y i ​−y ˉ​)+V 31 ​(z i ​−z ˉ)v i ​=V 12 ​(x i ​−x ˉ)+V 22 ​(y i ​−y ˉ​)+V 32 ​(z i ​−z ˉ)由此，将三维坐标m a t M = { x i , y i , z i } matM=\lbrace{x_i,y_i,z_i\rbrace}ma tM ={x i ​,y i ​,z i ​}转为二维M x y = { u i , v i } Mxy=\lbrace{u_i,v_i\rbrace}M x y ={u i ​,v i ​}。三维点如果近似在一个平面内，SVD分解得到的对角线矩阵m a t W matW ma t W上的元素就是特征值，最后一个特征值相对来说很小，对应的m a t V matV ma t V的第三列特征向量有
   w i = V 13 ( x i − x ˉ ) + V 23 ( y i − y ˉ ) + V 33 ( z i − z ˉ ) w_i =V_{13}(x_i-\bar{x})+V_{23}(y_i-\bar{y})+V_{33}(z_i-\bar{z})w i ​=V 13 ​(x i ​−x ˉ)+V 23 ​(y i ​−y ˉ​)+V 33 ​(z i ​−z ˉ)得到的w i w_i w i ​接近于0，就相当于把世界坐标系挪到了标定板上面来，并且z z z轴垂直于标定板。
2. 根据二维M x y = { u i , v i } Mxy=\lbrace{u_i,v_i\rbrace}M x y ={u i ​,v i ​}和图像上的特征点去除畸变的坐标m n = { a i , b i } mn=\lbrace{a_i,b_i\rbrace}mn ={a i ​,b i ​}，求解单应性矩阵H H H，可参考我之前的博文平面单应性变换
3. 第2步得到了标定板坐标系与像平面的单应性变化H H H，进一步地能得到标定板坐标系与相机坐标系的变换R T C a m e r a C a l i b P l a n e RT^{CalibPlane}{Camera}R T C am er a C a l ib Pl an e ​(这部分的转换不在这里解析)。第1步虽然名义上的数据降维，其实也可以认为是世界坐标系到标定板坐标系的变换，
   [ u i v i w i ] = [ V 11 V 21 V 31 V 12 V 22 V 32 V 13 V 23 V 33 ] [ x i y i z i ] − [ V 11 V 21 V 31 V 12 V 22 V 32 V 13 V 23 V 33 ] [ 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}⎣⎡​u i ​v i ​w i ​​⎦⎤​=⎣⎡​V 11 ​V 12 ​V 13 ​​V 21 ​V 22 ​V 23 ​​V 31 ​V 32 ​V 33 ​​⎦⎤​⎣⎡​x i ​y i ​z i ​​⎦⎤​−⎣⎡​V 11 ​V 12 ​V 13 ​​V 21 ​V 22 ​V 23 ​​V 31 ​V 32 ​V 33 ​​⎦⎤​⎣⎡​x ˉy ˉ​z ˉ​⎦⎤​用上面的符号来改写成矩阵形式就是
   M x y = m a t V ∗ m a t M − m a t V ∗ M c Mxy=matVmatM-matVMc M x y =ma t V ∗ma tM −ma t V ∗M c写成M x y = R ∗ m a t M + T Mxy=RmatM+T M x y =R ∗ma tM +T的形式的话，其中R t r a n s f o r m = m a t V Rtransform=matV Rt r an s f or m =ma t V,T t r a n s f o r m = − m a t V ∗ M c Ttransform=-matVMc Tt r an s f or m =−ma t V ∗M c。由第1步可得到世界坐标系到标定板坐标系的转换R T C a l i b P l a n e W o r l d RT^{World}{CalibPlane}R T C a l ib Pl an e W or l d ​，结合前面的矩阵得到世界坐标系到相机坐标系的转换，也就是得到了我们想要的当前拍摄的位姿
   R T C a m e r a W o r l d = R T C a l i b P l a n e W o r l d ∗ R T C a m e r a C a l i b P l a n e RT^{World}{Camera}=RT^{World}{CalibPlane}*RT^{CalibPlane}{Camera}R T C am er a W or l d ​=R T C a l ib Pl an e W or l d ​∗R T C am er a C a l ib Pl an e ​

代码注释

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

    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
    {

        Mc = cvAvg(matM);
        cvReshape( matM, matM, 1, count );

        cvMulTransposed( matM, &_MM, 1, &_Mc );

        cvSVD( &_MM, &matW, 0, &matV, CV_SVD_MODIFY_A + CV_SVD_V_T );

        if( W[2]/W[1] < 1e-3)
        {

            double tt[3], h[9], h1_norm, h2_norm;

            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 );

            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 );

                cvRodrigues2( &matH, &_r );
                cvRodrigues2( &_r, &matH );

                cvMatMulAdd( &matH, &T_transform, &_t, &_t );
                cvMatMul( &matH, R_transform, &matR );
            }
            else
            {
                cvSetIdentity( &matR );
                cvZero( &_t );
            }

            cvRodrigues2( &matR, &_r );
        }
        else
        {

        }
    }

    cvConvert( &_r, rvec );
    cvConvert( &_t, tvec );
}

Original: https://blog.csdn.net/weixin_43956164/article/details/126771627
Author: leaf_csdn
Title: OpenCV内部函数cvFindExtrinsicCameraParams2解析（一）