背景介绍
在之前的博文中，已经在外参估计函数cvFindExtrinsicCameraParams2分别进行了单应性方法、DLT方法，单应性矩阵进一步估计旋转平移量做了原理解析。本文对opencv在估计得到初始的外参之后，进一步迭代计算的推导详细分析，这一部分其实是相机标定迭代计算中对R,t的求导。
原理解析
在该函数第一篇解析中提到过，该函数是solvePnP函数选用CV_ITERATIVE时的主要执行方法，突显的特点就在于迭代这一步，这一篇博文将推导对构建最小化重投影误差方程后，对旋转分量(3个自由度，以旋转向量形式来表达
R
=
(
n
1
,
n
2
,
n
3
)
R=(n_1,n_2,n_3)
R=(n1,n2,n3))和平移分量(3个变量，即
T
=
(
t
1
,
t
2
,
t
3
T=(t_1,t_2,t_3
T=(t1,t2,t3)迭代优化的过程。有了这一步之后，后续相机标定求解内、外参的原理解析就更容易，只需要再对内参数迭代推导就行。
前面三篇的解析过程多次提到，内参数除了焦距，畸变量都初始化为零，在没有畸变量的情况下，投影仪方程为
z
c
[
u
v
1
]
=
[
f
x
0
u
0
0
f
y
v
0
0
0
1
]
[
R
11
R
12
R
13
t
1
R
21
R
22
R
23
t
2
R
31
R
32
R
33
t
3
]
[
X
Y
Z
1
]
(1)
z_{c} \begin{bmatrix} u \\ v\\ 1\end{bmatrix}=\begin{bmatrix} f_x & 0 & u_0 \\ 0 & f_y & v_0 \\ 0 & 0 & 1\end{bmatrix} \begin{bmatrix} 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{bmatrix} \begin{bmatrix} X \\ Y\\Z\\ 1\end{bmatrix}\tag{1}
zc⎣
⎡uv1⎦
⎤=⎣
⎡fx000fy0u0v01⎦
⎤⎣
⎡R11R21R31R12R22R32R13R23R33t1t2t3⎦
⎤⎣
⎡XYZ1⎦
⎤(1)进一步得到两个等式
u
=
f
x
R
11
X
+
R
12
Y
+
R
13
Z
+
t
1
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
+
u
0
v
=
f
y
R
21
X
+
R
22
Y
+
R
23
Z
+
t
2
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
+
v
0
(2)
\begin{array}{c} u=f_x\cfrac{R_{11}X+R_{12}Y+R_{13}Z+t_1}{R_{31}X+R_{32}Y+R_{33}Z+t_3} +u_0\\ v=f_y\cfrac{R_{21}X+R_{22}Y+R_{23}Z+t_2}{R_{31}X+R_{32}Y+R_{33}Z+t_3} +v_0 \end{array}\tag{2}
u=fxR31X+R32Y+R33Z+t3R11X+R12Y+R13Z+t1+u0v=fyR31X+R32Y+R33Z+t3R21X+R22Y+R23Z+t2+v0(2)我们的目标是根据这些参数投影的点与图像识别的点偏差最小，也就是
(
R
,
t
)
=
arg min
E
=
arg min
∑
∣
∣
(
u
−
u
k
)
2
+
(
v
−
v
k
)
2
∣
∣
2
(3)
(R,t) = \argmin{E}=\argmin{\sum||(u-u_k)^2+(v-v_k)^2||_2}\tag{3}
(R,t)=argminE=argmin∑∣∣(u−uk)2+(v−vk)2∣∣2(3)opencv采用LM算法来解算式（3），LM算法是梯度下降法和高斯牛顿法的结合，在我之前博文高斯牛顿法去畸变中对高斯牛顿法引用做了介绍，令残差为
F
(
n
1
,
n
2
,
n
3
,
t
1
,
t
2
,
t
3
)
=
f
x
R
11
X
+
R
12
Y
+
R
13
Z
+
t
1
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
+
u
0
−
u
k
G
(
n
1
,
n
2
,
n
3
,
t
1
,
t
2
,
t
3
)
=
f
y
R
21
X
+
R
22
Y
+
R
23
Z
+
t
2
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
+
v
0
−
v
k
(4)
\begin{array}{c} F(n_1,n_2,n_3,t_1,t_2,t_3)=f_x\cfrac{R_{11}X+R_{12}Y+R_{13}Z+t_1}{R_{31}X+R_{32}Y+R_{33}Z+t_3} +u_0-u_k\\ G(n_1,n_2,n_3,t_1,t_2,t_3)=f_y\cfrac{R_{21}X+R_{22}Y+R_{23}Z+t_2}{R_{31}X+R_{32}Y+R_{33}Z+t_3} +v_0-v_k \end{array}\tag{4}
F(n1,n2,n3,t1,t2,t3)=fxR31X+R32Y+R33Z+t3R11X+R12Y+R13Z+t1+u0−ukG(n1,n2,n3,t1,t2,t3)=fyR31X+R32Y+R33Z+t3R21X+R22Y+R23Z+t2+v0−vk(4)LM求解增量为
[
Δ
n
1
Δ
n
2
Δ
n
3
Δ
t
1
Δ
t
2
Δ
t
3
]
=
(
J
T
J
+
λ
I
)
−
1
(
−
J
T
E
r
r
)
(5)
\begin{bmatrix} \Delta{n_1}\\ \Delta{n_2}\\ \Delta{n_3} \\\Delta{t_1} \\\Delta{t_2}\\\Delta{t_3} \end{bmatrix} =(J^TJ+\lambda I)^{-1}(-J^TErr)\tag{5}
⎣
⎡Δn1Δn2Δn3Δt1Δt2Δt3⎦
⎤=(JTJ+λI)−1(−JTErr)(5)当有N个对应点时，雅可比矩阵为
J
=
[
∂
F
∂
n
1
∂
F
∂
n
2
∂
F
∂
n
3
∂
F
∂
t
1
∂
F
∂
t
2
∂
F
∂
t
3
∂
G
∂
n
1
∂
G
∂
n
2
∂
G
∂
n
3
∂
G
∂
t
1
∂
G
∂
t
2
∂
G
∂
t
3
.
.
.
.
.
.
]
[
2
N
,
6
]
(6)
J=\begin{bmatrix} \cfrac{\partial{F}}{\partial{n_1}} & \cfrac{\partial{F}}{\partial{n_2}} & \cfrac{\partial{F}}{\partial{n_3}} &\cfrac{\partial{F}}{\partial{t_1}} & \cfrac{\partial{F}}{\partial{t_2}} & \cfrac{\partial{F}}{\partial{t_3}} \\ \\ \cfrac{\partial{G}}{\partial{n_1}} & \cfrac{\partial{G}}{\partial{n_2}} & \cfrac{\partial{G}}{\partial{n_3}} &\cfrac{\partial{G}}{\partial{t_1}} & \cfrac{\partial{G}}{\partial{t_2}} & \cfrac{\partial{G}}{\partial{t_3}} \\...\\... \end{bmatrix}_{[2N,6]}\tag{6}
J=⎣
⎡∂n1∂F∂n1∂G......∂n2∂F∂n2∂G∂n3∂F∂n3∂G∂t1∂F∂t1∂G∂t2∂F∂t2∂G∂t3∂F∂t3∂G⎦
⎤[2N,6](6)每两行对应带入一对点数据，同样的残差矩阵为
E
r
r
=
[
F
1
G
1
F
2
G
2
.
.
.
.
.
.
]
[
2
N
,
1
]
(7)
Err=\begin{bmatrix} F_1\\G_1\\F_2\\G_2\\...\\... \end{bmatrix}_{[2N,1]}\tag{7}
Err=⎣
⎡F1G1F2G2......⎦
⎤[2N,1](7)可以看到，在内参数估计值、外参数初始值都有的情况下，且三维点和图像点数据都有，根据式（4）能计算出每个点的残差向量，得到式(7)的矩阵。为了解算出LM迭代中的增量式（6），还需要得到雅可比矩阵的值。根据式（6）可以看出，雅可比矩阵求解分为两部分，一是对旋转向量求导，二是对平移向量求导。
对旋转向量求导
根据式（4）可知，
∂
F
∂
n
k
=
f
x
(
∂
R
11
∂
n
k
X
+
∂
R
12
∂
n
k
Y
+
∂
R
13
∂
n
k
Z
)
(
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
)
−
(
R
11
X
+
R
12
Y
+
R
13
Z
+
t
1
)
(
∂
R
31
∂
n
k
X
+
∂
R
32
∂
n
k
Y
+
∂
R
33
∂
n
k
Z
)
(
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
)
2
k
=
1
,
2
,
3
(8)
\cfrac{\partial{F}}{\partial{n_k}}=f_x\cfrac{(\cfrac{\partial{R_{11}}}{\partial{n_k}}X+\cfrac{\partial{R_{12}}}{\partial{n_k}}Y+\cfrac{\partial{R_{13}}}{\partial{n_k}}Z)(R_{31}X+R_{32}Y+R_{33}Z+t_3)-(R_{11}X+R_{12}Y+R_{13}Z+t_1)(\cfrac{\partial{R_{31}}}{\partial{n_k}}X+\cfrac{\partial{R_{32}}}{\partial{n_k}}Y+\cfrac{\partial{R_{33}}}{\partial{n_k}}Z)}{(R_{31}X+R_{32}Y+R_{33}Z+t_3)^2} \\ k=1,2,3\tag{8}
∂nk∂F=fx(R31X+R32Y+R33Z+t3)2(∂nk∂R11X+∂nk∂R12Y+∂nk∂R13Z)(R31X+R32Y+R33Z+t3)−(R11X+R12Y+R13Z+t1)(∂nk∂R31X+∂nk∂R32Y+∂nk∂R33Z)k=1,2,3(8)
∂
G
∂
n
k
=
f
y
(
∂
R
21
∂
n
k
X
+
∂
R
22
∂
n
k
Y
+
∂
R
23
∂
n
k
Z
)
(
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
)
−
(
R
21
X
+
R
22
Y
+
R
23
Z
+
t
2
)
(
∂
R
31
∂
n
k
X
+
∂
R
32
∂
n
k
Y
+
∂
R
33
∂
n
k
Z
)
(
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
)
2
k
=
1
,
2
,
3
(9)
\cfrac{\partial{G}}{\partial{n_k}}=f_y\cfrac{(\cfrac{\partial{R_{21}}}{\partial{n_k}}X+\cfrac{\partial{R_{22}}}{\partial{n_k}}Y+\cfrac{\partial{R_{23}}}{\partial{n_k}}Z)(R_{31}X+R_{32}Y+R_{33}Z+t_3)-(R_{21}X+R_{22}Y+R_{23}Z+t_2)(\cfrac{\partial{R_{31}}}{\partial{n_k}}X+\cfrac{\partial{R_{32}}}{\partial{n_k}}Y+\cfrac{\partial{R_{33}}}{\partial{n_k}}Z)}{(R_{31}X+R_{32}Y+R_{33}Z+t_3)^2} \\ k=1,2,3\tag{9}
∂nk∂G=fy(R31X+R32Y+R33Z+t3)2(∂nk∂R21X+∂nk∂R22Y+∂nk∂R23Z)(R31X+R32Y+R33Z+t3)−(R21X+R22Y+R23Z+t2)(∂nk∂R31X+∂nk∂R32Y+∂nk∂R33Z)k=1,2,3(9)可以看出来问题的关键就是求出
∂
R
i
j
∂
n
k
，
i
=
1
,
2
,
3
,
j
=
1
,
2
,
3
。
\cfrac{\partial{R_{ij}}}{\partial{n_k}}，i=1,2,3,j=1,2,3。
∂nk∂Rij，i=1,2,3,j=1,2,3。这27个导数
根据罗德里格斯公式
R
=
I
cos
θ
+
(
1
−
cos
θ
)
[
r
x
r
y
r
z
]
[
r
x
r
y
r
z
]
+
sin
θ
[
0
−
r
z
r
y
r
z
0
−
r
x
−
r
y
r
x
0
]
(10)
R=I\cos\theta+(1-\cos\theta)\begin{bmatrix} r_x\\r_y\\r_z \end{bmatrix}\begin{bmatrix} r_x&r_y&r_z \end{bmatrix}+\sin\theta \begin{bmatrix} 0 & -r_z& r_y\\ r_z & 0 & -r_x \\ -r_y & r_x & 0\end{bmatrix}\tag{10}
R=Icosθ+(1−cosθ)⎣
⎡rxryrz⎦
⎤[rxryrz]+sinθ⎣
⎡0rz−ry−rz0rxry−rx0⎦
⎤(10)旋转向量
R
=
(
n
1
,
n
2
,
n
3
)
=
θ
(
r
x
,
r
y
,
r
z
)
R=(n_1,n_2,n_3)=\theta(r_x,r_y,r_z)
R=(n1,n2,n3)=θ(rx,ry,rz)，
r
x
,
r
y
,
r
z
r_x,r_y,r_z
rx,ry,rz为单位向量，对应着旋转的轴
θ
=
n
1
2
+
n
2
2
+
n
3
2
(11)
\theta=\sqrt{n_1^2+n_2^2+n_3^2}\tag{11}
θ=n12+n22+n32(11)为旋转的角度，那么旋转矩阵可以表达为
R
=
[
R
11
R
12
R
13
R
21
R
22
R
23
R
31
R
32
R
33
]
=
[
r
x
2
(
1
−
cos
θ
)
+
cos
θ
r
x
r
y
(
1
−
cos
θ
)
−
r
z
sin
θ
r
x
r
z
(
1
−
cos
θ
)
+
r
y
sin
θ
r
x
r
y
(
1
−
cos
θ
)
+
r
z
sin
θ
r
y
2
(
1
−
cos
θ
)
+
cos
θ
r
y
r
z
(
1
−
cos
θ
)
−
r
x
sin
θ
r
x
r
z
(
1
−
cos
θ
)
−
r
y
sin
θ
r
y
r
z
(
1
−
cos
θ
)
+
r
x
sin
θ
r
z
2
(
1
−
cos
θ
)
+
cos
θ
]
(12)
R= \begin{bmatrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \\ \end{bmatrix} = \begin{bmatrix} r_x^2(1-\cos\theta)+\cos\theta & r_xr_y(1-\cos\theta)-r_z\sin\theta & r_xr_z(1-\cos\theta)+r_y\sin\theta \\ r_xr_y(1-\cos\theta)+r_z\sin\theta & r_y^2(1-\cos\theta)+\cos\theta & r_yr_z(1-\cos\theta)-r_x\sin\theta \\ r_xr_z(1-\cos\theta)-r_y\sin\theta & r_yr_z(1-\cos\theta)+r_x\sin\theta & r_z^2(1-\cos\theta)+\cos\theta \end{bmatrix} \tag{12}
R=⎣
⎡R11R21R31R12R22R32R13R23R33⎦
⎤=⎣
⎡rx2(1−cosθ)+cosθrxry(1−cosθ)+rzsinθrxrz(1−cosθ)−rysinθrxry(1−cosθ)−rzsinθry2(1−cosθ)+cosθryrz(1−cosθ)+rxsinθrxrz(1−cosθ)+rysinθryrz(1−cosθ)−rxsinθrz2(1−cosθ)+cosθ⎦
⎤(12)利用复合函数的求导性质可知
∂
R
11
∂
n
1
=
∂
R
11
∂
r
x
∂
r
x
∂
n
1
+
∂
R
11
∂
r
y
∂
r
y
∂
n
1
+
∂
R
11
∂
r
z
∂
r
z
∂
n
1
+
∂
R
11
∂
θ
∂
θ
∂
n
1
(13)
\cfrac{\partial{R_{11}}}{\partial{n_1}}=\cfrac{\partial{R_{11}}}{\partial{r_x}}\cfrac{\partial{r_x}}{\partial{n_1}}+\cfrac{\partial{R_{11}}}{\partial{r_y}}\cfrac{\partial{r_y}}{\partial{n_1}}+\cfrac{\partial{R_{11}}}{\partial{r_z}}\cfrac{\partial{r_z}}{\partial{n_1}}+\cfrac{\partial{R_{11}}}{\partial{\theta}}\cfrac{\partial{\theta}}{\partial{n_1}}\tag{13}
∂n1∂R11=∂rx∂R11∂n1∂rx+∂ry∂R11∂n1∂ry+∂rz∂R11∂n1∂rz+∂θ∂R11∂n1∂θ(13)写成通用形式就是
∂
R
i
j
∂
n
k
=
∂
R
i
j
∂
r
x
∂
r
x
∂
n
k
+
∂
R
i
j
∂
r
y
∂
r
y
∂
n
k
+
∂
R
i
j
∂
r
z
∂
r
z
∂
n
k
+
∂
R
i
j
∂
θ
∂
θ
∂
n
k
i
=
1
,
2
,
3
,
j
=
1
,
2
,
3
,
k
=
1
,
2
,
3
(14)
\cfrac{\partial{R_{ij}}}{\partial{n_k}}=\cfrac{\partial{R_{ij}}}{\partial{r_x}}\cfrac{\partial{r_x}}{\partial{n_k}}+\cfrac{\partial{R_{ij}}}{\partial{r_y}}\cfrac{\partial{r_y}}{\partial{n_k}}+\cfrac{\partial{R_{ij}}}{\partial{r_z}}\cfrac{\partial{r_z}}{\partial{n_k}}+\cfrac{\partial{R_{ij}}}{\partial{\theta}}\cfrac{\partial{\theta}}{\partial{n_k}} \\i=1,2,3,j=1,2,3,k=1,2,3\tag{14}
∂nk∂Rij=∂rx∂Rij∂nk∂rx+∂ry∂Rij∂nk∂ry+∂rz∂Rij∂nk∂rz+∂θ∂Rij∂nk∂θi=1,2,3,j=1,2,3,k=1,2,3(14)根据式（12）每个
R
i
j
R_{ij}
Rij对应分量已知，求导为
∂
R
i
j
∂
r
x
=
[
2
r
x
(
1
−
cos
θ
)
r
y
(
1
−
cos
θ
)
r
z
(
1
−
cos
θ
)
r
y
(
1
−
cos
θ
)
0
−
sin
θ
r
z
(
1
−
cos
θ
)
sin
θ
0
]
(15)
\cfrac{\partial{R_{ij}}}{\partial{r_x}}= \begin{bmatrix} 2r_x(1-\cos\theta) & r_y(1-\cos\theta)& r_z(1-\cos\theta) \\ r_y(1-\cos\theta) & 0 & -\sin\theta \\ r_z(1-\cos\theta) & \sin\theta & 0 \end{bmatrix}\tag{15}
∂rx∂Rij=⎣
⎡2rx(1−cosθ)ry(1−cosθ)rz(1−cosθ)ry(1−cosθ)0sinθrz(1−cosθ)−sinθ0⎦
⎤(15)
∂
R
i
j
∂
r
y
=
[
0
r
x
(
1
−
cos
θ
)
sin
θ
r
x
(
1
−
cos
θ
)
2
r
y
(
1
−
cos
θ
)
r
z
(
1
−
cos
θ
)
−
sin
θ
r
z
(
1
−
cos
θ
)
0
]
(16)
\cfrac{\partial{R_{ij}}}{\partial{r_y}}= \begin{bmatrix} 0 & r_x(1-\cos\theta)& \sin\theta \\ r_x(1-\cos\theta) &2r_y(1-\cos\theta) & r_z(1-\cos\theta) \\ -\sin\theta & r_z(1-\cos\theta)& 0 \end{bmatrix}\tag{16}
∂ry∂Rij=⎣
⎡0rx(1−cosθ)−sinθrx(1−cosθ)2ry(1−cosθ)rz(1−cosθ)sinθrz(1−cosθ)0⎦
⎤(16)
∂
R
i
j
∂
r
z
=
[
0
−
sin
θ
r
x
(
1
−
cos
θ
)
sin
θ
0
r
y
(
1
−
cos
θ
)
r
x
(
1
−
cos
θ
)
r
y
(
1
−
cos
θ
)
2
r
z
(
1
−
cos
θ
)
]
(17)
\cfrac{\partial{R_{ij}}}{\partial{r_z}}= \begin{bmatrix} 0 &- \sin\theta & r_x(1-\cos\theta) \\ \sin\theta &0 &r_y(1-\cos\theta) \\ r_x(1-\cos\theta)& r_y(1-\cos\theta)& 2r_z(1-\cos\theta) \end{bmatrix}\tag{17}
∂rz∂Rij=⎣
⎡0sinθrx(1−cosθ)−sinθ0ry(1−cosθ)rx(1−cosθ)ry(1−cosθ)2rz(1−cosθ)⎦
⎤(17)
∂
r
i
∂
n
k
=
1
θ
[
(
1
−
r
x
2
)
−
r
x
r
y
−
r
x
r
z
−
r
x
r
y
(
1
−
r
y
2
)
−
r
y
r
z
−
r
x
r
z
−
r
y
r
z
(
1
−
r
z
2
)
]
(18)
\cfrac{\partial{r_i}}{\partial{n_k}}=\cfrac{1}{\theta} \begin{bmatrix} (1-r_x^2) & -r_xr_y& -r_xr_z \\ -r_xr_y & (1-r_y^2) & -r_yr_z \\ -r_xr_z& -r_yr_z &(1-r_z^2) \end{bmatrix}\tag{18}
∂nk∂ri=θ1⎣
⎡(1−rx2)−rxry−rxrz−rxry(1−ry2)−ryrz−rxrz−ryrz(1−rz2)⎦
⎤(18)
∂
R
i
j
∂
θ
=
[
sin
θ
(
r
x
2
−
1
)
r
x
r
y
sin
θ
−
r
z
cos
θ
r
x
r
z
sin
θ
+
r
y
cos
θ
r
x
r
y
sin
θ
+
r
z
cos
θ
sin
θ
(
r
y
2
−
1
)
r
y
r
z
sin
θ
−
r
x
cos
θ
r
x
r
z
sin
θ
−
r
y
cos
θ
r
y
r
z
sin
θ
+
r
x
cos
θ
sin
θ
(
r
z
2
−
1
)
]
(19)
\cfrac{\partial{R_{ij}}}{\partial{\theta}}= \begin{bmatrix} \sin\theta(r_x^2-1)& r_xr_y\sin\theta-r_z\cos\theta& r_xr_z\sin\theta+r_y\cos\theta \\ r_xr_y\sin\theta+r_z\cos\theta & \sin\theta(r_y^2-1)&r_yr_z\sin\theta-r_x\cos\theta \\ r_xr_z\sin\theta-r_y\cos\theta& r_yr_z\sin\theta+r_x\cos\theta& \sin\theta(r_z^2-1) \end{bmatrix}\tag{19}
∂θ∂Rij=⎣
⎡sinθ(rx2−1)rxrysinθ+rzcosθrxrzsinθ−rycosθrxrysinθ−rzcosθsinθ(ry2−1)ryrzsinθ+rxcosθrxrzsinθ+rycosθryrzsinθ−rxcosθsinθ(rz2−1)⎦
⎤(19)
∂
θ
∂
n
k
=
[
r
x
r
y
r
z
]
(20)
\cfrac{\partial{\theta}}{\partial{n_k}}= \begin{bmatrix} r_x & r_y &r_z \end{bmatrix} \tag{20}
∂nk∂θ=[rxryrz](20)结合式（15）~（20）带入式（14）中得到我们需要的
∂
R
i
j
∂
n
k
\cfrac{\partial{R_{ij}}}{\partial{n_k}}
∂nk∂Rij，进一步的带入式（8）和（9）中完成对旋转分量的求解。
对平移向量求导
对平移向量的求导要简单很多，因为不涉及符合函数，根据式（4）可知，
∂
F
∂
t
1
=
f
x
1
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
(21)
\cfrac{\partial{F}}{\partial{t_1}}=f_x\cfrac{1}{R_{31}X+R_{32}Y+R_{33}Z+t_3}\tag{21}
∂t1∂F=fxR31X+R32Y+R33Z+t31(21)
∂
F
∂
t
2
=
0
(22)
\cfrac{\partial{F}}{\partial{t_2}}=0\tag{22}
∂t2∂F=0(22)
∂
F
∂
t
3
=
f
x
−
(
R
11
X
+
R
12
Y
+
R
13
Z
+
t
1
)
(
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
)
2
(23)
\cfrac{\partial{F}}{\partial{t_3}}=f_x\cfrac{-(R_{11}X+R_{12}Y+R_{13}Z+t_1)}{(R_{31}X+R_{32}Y+R_{33}Z+t_3)^2}\tag{23}
∂t3∂F=fx(R31X+R32Y+R33Z+t3)2−(R11X+R12Y+R13Z+t1)(23)同理
∂
G
∂
t
1
=
0
(24)
\cfrac{\partial{G}}{\partial{t_1}}=0\tag{24}
∂t1∂G=0(24)
∂
G
∂
t
2
=
f
y
1
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
(25)
\cfrac{\partial{G}}{\partial{t_2}}=f_y\cfrac{1}{R_{31}X+R_{32}Y+R_{33}Z+t_3}\tag{25}
∂t2∂G=fyR31X+R32Y+R33Z+t31(25)
∂
G
∂
t
3
=
f
y
−
(
R
21
X
+
R
22
Y
+
R
23
Z
+
t
2
)
(
R
31
X
+
R
32
Y
+
R
33
Z
+
t
3
)
2
(26)
\cfrac{\partial{G}}{\partial{t_3}}=f_y\cfrac{-(R_{21}X+R_{22}Y+R_{23}Z+t_2)}{(R_{31}X+R_{32}Y+R_{33}Z+t_3)^2}\tag{26}
∂t3∂G=fy(R31X+R32Y+R33Z+t3)2−(R21X+R22Y+R23Z+t2)(26)综上对旋转向量和平移向量的求导，带入式（6）中，即可进行LM迭代算法，代码中LM计算在
bool proceed = solver.update( __param, matJ, _err );
该函数中子函数void CvLevMarq::step()是求解的核心部分，得到式（5）的变量增量，以进行逐步迭代优化得到最终的6个外参数量。至此，四个篇章就完成cvFindExtrinsicCameraParams2对外参数估计原理解析的全部过程。中间的求导如有错误请在评论区纠正，毕竟我没有把所有的数和opencv代码进行核对，这个有时间会进行逐个元素核对。
代码注释
本文对应的主要代码段是
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;
//** 省略初始值估计的代码
// refine extrinsic parameters using iterative algorithm
CvLevMarq solver( 6, count*2, cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,max_iter,FLT_EPSILON), true);
cvCopy( &_param, solver.param );
//** 开始迭代计算
for(;;)
{
CvMat *matJ = 0, *_err = 0;
const CvMat *__param = 0;
//** LM算法求解迭代的增量，见式（5）
bool proceed = solver.update( __param, matJ, _err );
cvCopy( __param, &_param );
if( !proceed || !_err )
break;
cvReshape( _err, _err, 2, 1 );
if( matJ )
{
cvGetCols( matJ, &_dpdr, 0, 3 );
cvGetCols( matJ, &_dpdt, 3, 6 );
//** 求解LM中的雅可比矩阵J
cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
_err, &_dpdr, &_dpdt, 0, 0, 0 );
}
else
{
cvProjectPoints2( matM, &_r, &_t, &matA, distCoeffs,
_err, 0, 0, 0, 0, 0 );
}
//** 在此处将投影点与图像识别点做偏差，构成err矩阵
cvSub(_err, _m, _err);
cvReshape( _err, _err, 1, 2*count );
}
cvCopy( solver.param, &_param );
_r = cvMat( rvec->rows, rvec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param );
_t = cvMat( tvec->rows, tvec->cols,
CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 );
cvConvert( &_r, rvec );
cvConvert( &_t, tvec );
}
对cvProjectPoints2函数求解雅可比矩阵相关部分的注解
CV_IMPL void cvProjectPoints2( const CvMat* objectPoints,
const CvMat* r_vec,
const CvMat* t_vec,
const CvMat* A,
const CvMat* distCoeffs,
CvMat* imagePoints, CvMat* dpdr,
CvMat* dpdt, CvMat* dpdf,
CvMat* dpdc, CvMat* dpdk,
double aspectRatio )
{
Ptr<CvMat> matM, _m;
Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
int i, j, count;
int calc_derivatives;
const CvPoint3D64f* M;
CvPoint2D64f* m;
double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
Matx33d matTilt = Matx33d::eye();
Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
//**省略
if( r_vec->rows == 3 && r_vec->cols == 3 )
{
_r = cvMat( 3, 1, CV_64FC1, r );
cvRodrigues2( r_vec, &_r );
cvRodrigues2( &_r, &matR, &_dRdr );
cvCopy( r_vec, &matR );
}
else
{
_r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
cvConvert( r_vec, &_r );
// **旋转矩阵的每一个分量对旋转向量求导，见式（15~20）
cvRodrigues2( &_r, &matR, &_dRdr );
}
//** 省略
calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk;
for( i = 0; i < count; i++ )
{
double X = M[i].x, Y = M[i].y, Z = M[i].z;
double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
double r2, r4, r6, a1, a2, a3, cdist, icdist2;
double xd, yd, xd0, yd0, invProj;
Vec3d vecTilt;
Vec3d dVecTilt;
Matx22d dMatTilt;
Vec2d dXdYd;
z = z ? 1./z : 1;
x *= z; y *= z;
r2 = x*x + y*y;
r4 = r2*r2;
r6 = r4*r2;
a1 = 2*x*y;
a2 = r2 + 2*x*x;
a3 = r2 + 2*y*y;
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
// additional distortion by projecting onto a tilt plane
vecTilt = matTilt*Vec3d(xd0, yd0, 1);
invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
xd = invProj * vecTilt(0);
yd = invProj * vecTilt(1);
//**根据当前参数在图像上的投影点，在其他地方会和图像识别的点求偏差，构成err矩阵
m[i].x = xd*fx + cx;
m[i].y = yd*fy + cy;
if( calc_derivatives )
{
//**省略内参求导
//**对平移分量求导
if( dpdt_p )
{
double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
for( j = 0; j < 3; j++ )
{
double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
dpdt_p[j] = fx*dXdYd(0);
dpdt_p[dpdt_step+j] = fy*dXdYd(1);
}
dpdt_p += dpdt_step*2;
}
//**对旋转分量求导
if( dpdr_p )
{
double dx0dr[] =
{
X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
};
double dy0dr[] =
{
X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
};
double dz0dr[] =
{
X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
};
for( j = 0; j < 3; j++ )
{
double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
double dydr = z*(dy0dr[j] - y*dz0dr[j]);
double dr2dr = 2*x*dxdr + 2*y*dydr;
double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
double da1dr = 2*(x*dydr + y*dxdr);
double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
dpdr_p[j] = fx*dXdYd(0);
dpdr_p[dpdr_step+j] = fy*dXdYd(1);
}
dpdr_p += dpdr_step*2;
}
}
}
if( _m != imagePoints )
cvConvert( _m, imagePoints );
if( _dpdr != dpdr )
cvConvert( _dpdr, dpdr );
if( _dpdt != dpdt )
cvConvert( _dpdt, dpdt );
if( _dpdf != dpdf )
cvConvert( _dpdf, dpdf );
if( _dpdc != dpdc )
cvConvert( _dpdc, dpdc );
if( _dpdk != dpdk )
cvConvert( _dpdk, dpdk );
}
6076
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
