背景介绍
在上一篇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 );
}