Opencv外参估计cvFindExtrinsicCameraParams2原理解析（三）

背景介绍

在外参估计函数第一篇中cvFindExtrinsicCameraParams2解析（一），介绍了特征点在同一个平面上时opencv走的解算路线，但未对解算出来的单应性矩阵如何分解出旋转平移矩阵做解析，本文补充该部分的原理解析，以及注释对应的代码段。

原理解析

标定板坐标系与像平面之间的单应性矩阵$H$，其实质是如下形式
$$\left[\begin{array}{c}{x}_c\\ y_c\\ 1\end{array}\right]={\lambda }_1\left[\begin{array}{cccc}{H}_{11}& H_{12}& M_{13}& H_{13}\\ H_{21}& H_{22}& M_{23}& H_{23}\\ H_{31}& H_{32}& M_{33}& H_{33}\end{array}\right]\left[\begin{array}{c}{x}_b\\ y_b\\ 0\\ 1\end{array}\right]\text{\hspace{1em}(1)}$$相当于解算出来了摄影机矩阵中的9个元素$H_{11},H_{12},..,H_{33}$，如同在解析二中的DLT的结果一样，计算出来的结果不能直接得到旋转矩阵，因为不满足单位正交的性质。为了估计出旋转和平移矩阵，大体思路是：式（1）中3x4的矩阵，前三列是旋转矩阵R的向量，后一列是平移向量，第一列和第二列已知，那么可以先进行单位化，叉乘得到第三列，而平移向量根据单位化过程中的缩放量得到最终结果。
我们要获得的旋转矩阵和平移向量与当前计算出单应性矩阵的关系可表示为$$\left[\begin{array}{cccc}{H}_{11}& H_{12}& M_{13}& H_{13}\\ H_{21}& H_{22}& M_{23}& H_{23}\\ H_{31}& H_{32}& M_{33}& H_{33}\end{array}\right]={\lambda }_2\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\end{array}\right]\text{\hspace{1em}(2)}$$其中$r_{11},r_{12},...,r_{33}$组成为旋转矩阵，$t_1,t_2,t_3$组成平移向量。为了满足单位正交性质，$$(r_{11},r_{21},r_{31})=\frac{(H_{11},H_{21},H_{31})}{\sqrt{H_{11}^2+H_{21}^2+H_{31}^2}}\text{\hspace{1em}(3)}$$用opencv代码中符号表示就是$h{1}_{norm}=\sqrt{H_{11}^2+H_{21}^2+H_{31}^2}$，同理，第二列的$h{2}_{norm}=\sqrt{H_{12}^2+H_{22}^2+H_{32}^2}$，旋转矩阵的第三列与其他两列都是正交的，那么可以用两个向量叉乘的办法得到第三列，即$$(r_{13},r_{23},r_{33})=(r_{11},r_{21},r_{31})\times (r_{12},r_{22},r_{32})\text{\hspace{1em}(4)}$$可以看到，向量单位化的过程中，对于每个向量来说清楚的知道式（2）中${\lambda }_2$的取值，但是两个向量的模毕竟是不太可能相同的，因此，只能近似的取$${\lambda }_2\approx 0.5\ast (h{1}_{norm}+h{2}_{norm})\text{\hspace{1em}(5)}$$那么平移向量为$$(t_1,t_2,t_3)=(H_{13},H_{23},H_{33})\ast \frac{1}{{\lambda }_2}\text{\hspace{1em}(6)}$$此时的旋转矩阵仅是满足了列向量单位且正交，在行方向不一定满足。如同在解析二中一样，为了寻找一个接近与该矩阵并且完全满足旋转矩阵性质的矩阵，将该矩阵进行SVD分解，$R=U{V}^T$得到我们最终的估计结果。opencv中这一步是在 cvRodrigues2( &matH, &_r );中实现的，这个函数若输入3x3的旋转矩阵，那么输出就是根据罗德里格斯变换得到的3x1旋转向量；若输入是3x1的旋转向量，那么输出就是3x3的旋转矩阵。
根据罗德里格斯公式$$R=\cos \theta \cdot I+(1-\cos \theta )n{n}^T+\sin \theta \cdot [n_x]\text{\hspace{1em}(7)}$$其中，$n=(r_x,r_y,r_z)$的单位向量，表示旋转轴，$\theta$表示旋转角度，旋转向量为$\mathbf{r}=\theta (r_x,r_y,r_z)$. 式（7）两边取迹得到$$tr(R)=3\cos \theta +(1-\cos \theta )\text{\hspace{1em}(8)}$$根据式（8）在已知R矩阵的情况下，可以计算出旋转角度$$\theta =\arccos (\frac{tr(R)-1}{2}\text{\hspace{1em}(9)}$$. 而$(r_x,r_y,r_z)$可以用下式求解$$\sin \theta \left[\begin{array}{ccc}0& -r_z& r_y\\ r_z& 0& -r_x\\ -r_y& r_x& 0\end{array}\right]=\frac{R-R^T}{2}\text{\hspace{1em}(10)}$$那么$$(r_x,r_y,r_z)=\frac{[R(2,1)-R(1,2),R(0,2)-R(2,0),R(1,0)-R(0,1)]}{2\sin \theta }\text{\hspace{1em}(11)}$$旋转向量为$$\mathbf{r}=\theta (r_x,r_y,r_z)\text{\hspace{1em}(12)}$$

代码注释

本文的内容在下列删减的cvFindExtrinsicCameraParams2函数中第91行到111行。

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 );              
				// **单位化后的旋转矩阵不是完全满足单位正交性质，进一步SVD分解
                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 );
}

进一步地，在cvRodrigues2函数得到最接近的单位正交的旋转矩阵，并且转为旋转向量

CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
{
    //**省略一些无关代码
    if( src->cols == 1 || src->rows == 1 )
    {
       //** 这部分是旋转向量转为旋转矩阵
    }
    //** 旋转矩阵转为旋转向量
    else if( src->cols == 3 && src->rows == 3 )
    {
        Matx33d U, Vt;
        Vec3d W;
        double theta, s, c;
        int step = dst->rows > 1 ? dst->step / elem_size : 1;

        if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) &&
            (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1))
            CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );

        Matx33d R = cvarrToMat(src);

        if( !checkRange(R, true, NULL, -100, 100) )
        {
            cvZero(dst);
            if( jacobian )
                cvZero(jacobian);
            return 0;
        }
		//** SVD分解输入的矩阵
        SVD::compute(R, W, U, Vt);
        //** 得到满足单位正交，并且接近输入矩阵的结果
        R = U*Vt;
 		//** 式（11）的分子
        Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
		//** s表示的就是sin(theta)，
		//** 这里的r.x是式(11)的分子中第一个分量，为2*sin*rx, 而(rx^2+ry^2+rz^2)=1是单位向量
		//** sqrt{[(2*sin*rx)^2+(2*sin*ry)^2+(2*sin*rz)^2]*0.25}=sin, 所以s表示是sin(theta)
        s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25);
        //** 式(9)
        c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5;
        c = c > 1. ? 1. : c < -1. ? -1. : c;
        theta = acos(c);

        if( s < 1e-5 )
        {
           
        }
        else
        {
          	//** 式(11)中的分母
            double vth = 1/(2*s);

            if( jacobian )
            {
                //** 省略无关代码
            }
			//** 式(12)中的分母
            vth *= theta;
            r *= vth;
        }

        if( depth == CV_32F )
        {
            dst->data.fl[0] = (float)r.x;
            dst->data.fl[step] = (float)r.y;
            dst->data.fl[step*2] = (float)r.z;
        }
        else
        {
            dst->data.db[0] = r.x;
            dst->data.db[step] = r.y;
            dst->data.db[step*2] = r.z;
        }
    }
    //**省略无关代码
    return 1;
}