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} {(xc′R31−R11)xw+(xc′R32−R12)yw+(xc′R33−R13)zw+(xc′Tz−Tx)=0(yc′R31−R21)xw+(yc′R32−R22)yw+(yc′R33−R23)zw+(yc′Tz−Ty)=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} {(RRt11−xc′RRt31)xw+(RRt12−xc′RRt32)yw+(RRt13−xc′RRt33)zw+(RRt14−xc′RRt34)=0(RRt21−yc′RRt31)xw+(RRt22−yc′RRt32)yw+(RRt23−yc′RRt33)zw+(RRt24−yc′RRt34)=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.0−xc′xw−yc′xw−xc′yw−yc′yw−xc′zw−yc′zw−xc′−yc′⎦ ⎤⎣ ⎡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(2∗n)∗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)12∗12W对角阵(LV)12∗12， L V 12 ∗ 12 LV_{12*12} LV12∗12的最后一列 R R t 12 ∗ 1 RRt_{12*1} RRt12∗1是方程的解。
但是，直接线性变换求解的结果，也就是这个12行1列的向量，因为该列向量本身就是单位向量，所以转换得到的 R R t 3 ∗ 4 RRt_{3*4} RRt3∗4并不是我们需要的外参矩阵，外参矩阵的分解 R R t = [ R R ∣ t t ] (4) RRt=[RR|tt] \tag{4} RRt=[RR∣tt](4)它的旋转矩阵元素 R R RR RR不满足单位正交的特性。形如’Ax=0’的解，可以看出来等式两边乘一个系数还是相等，所以式（3）SVD分解结果并不是我们想要的正解，设正解为 m a t R t = [ m a t R ∣ t ] matRt=[matR|t] matRt=[matR∣t]，正解和 R R t = [ R R ∣ t t ] RRt=[RR|tt] RRt=[RR∣tt]相差一个尺度 β \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−β∗RR∣∣F=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−β∗RR∣∣F=tr(matR−β∗UWVT)T(matR−β∗UWVT)=tr(matRTmatR−2∗matRT∗β∗UWVT+β2(UWVT)T(UWVT))=tr(I−2∗matRT∗β∗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∣∣−matRT∗UWVT∣∣=max∣∣matRT∗UWVT∣∣(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} β=∣∣RR∣∣F∣∣matR∣∣F(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 );
}