上一篇介绍了相机模型和畸变模型,有了这个基础就可以对相机标定进行讲解了。
张正友标定法,其实就是拍摄10几张不同姿态的棋盘格,就可以估算出相机的内参和棋盘格的姿态。利用了针孔相机模型和棋盘格三维平面的世界坐标与棋盘格图像的坐标之间的透视关系(单应性)进行计算的。
具体推导过程:
有2D点m 和 3D点M
m
=
[
u
v
]
M
=
[
X
Y
Z
]
m = \left[\begin{matrix} u \\ v \end{matrix}\right] M= \left[\begin{matrix} X \\ Y \\ Z \end{matrix}\right]
m=[uv]M=⎣⎡XYZ⎦⎤
对应的齐次坐标分别是
m
^
=
[
u
v
1
]
M
^
=
[
X
Y
Z
1
]
\widehat{m}=\left[\begin{matrix} u \\ v \\ 1 \end{matrix}\right]\widehat{M}=\left[\begin{matrix} X \\ Y \\ Z \\1\end{matrix}\right]
m
=⎣⎡uv1⎦⎤M
=⎣⎢⎢⎡XYZ1⎦⎥⎥⎤
所以有
s
m
^
=
A
[
R
t
]
M
^
s\widehat{m} = A\left[\begin{matrix}R&t\end{matrix}\right]\widehat{M}
sm
=A[Rt]M
A 就是相机的内参矩阵
A
=
[
a
γ
u
0
0
b
v
0
0
0
1
]
A=\left[\begin{matrix} a & \gamma & u_0\\ 0 & b & v_0\\ 0 & 0 & 1 \end{matrix}\right]
A=⎣⎡a00γb0u0v01⎦⎤
其中
a
,
b
a, b
a,b分别表示x,y方向的焦距,
u
0
,
v
0
u_0, v_0
u0,v0表示光心点的坐标,
γ
\gamma
γ表示x和y方向之间的倾斜系数,一般来说默认等于0.
我们把R写成3个列向量的表示,并令
Z
=
0
Z=0
Z=0
s [ u v 1 ] = A [ r 1 r 2 r 3 t ] [ X Y 0 1 ] = A [ r 1 r 2 t ] [ X Y 1 ] s\left[\begin{matrix} u \\ v \\ 1 \end{matrix}\right]=A\left[\begin{matrix} r_1 & r_2 & r_3 & t\end{matrix}\right]\left[\begin{matrix} X \\ Y \\ 0 \\1\end{matrix}\right] \\ =A\left[\begin{matrix} r_1 & r_2 & t\end{matrix}\right]\left[\begin{matrix} X \\ Y \\1\end{matrix}\right] s⎣⎡uv1⎦⎤=A[r1r2r3t]⎣⎢⎢⎡XY01⎦⎥⎥⎤=A[r1r2t]⎣⎡XY1⎦⎤
进一步简化
s
m
^
=
H
M
^
H
=
A
[
r
1
r
2
t
]
\begin{aligned} &s\widehat{m} = H \widehat{M} \\ &H=A\left[\begin{matrix} r_1 & r_2 & t\end{matrix}\right] \end{aligned}
sm
=HM
H=A[r1r2t]
H 就是单应矩阵。
根据R的性质可以设置两个约束条件, r1, r2 是单位正交的向量
r
1
T
r
2
=
0
∣
r
1
∣
=
∣
r
2
∣
=
1
即
r
1
T
r
1
=
r
2
T
r
2
\begin{aligned} & r_1^Tr_2=0 \\ & |r_1|=|r_2| = 1即r_1^Tr_1=r_2^Tr_2 \end{aligned}
r1Tr2=0∣r1∣=∣r2∣=1即r1Tr1=r2Tr2
把H矩阵也写成3个列向量的形式
H
=
[
h
1
h
2
h
3
]
H=\left[\begin{matrix} h_1 & h_2 & h_3\end{matrix}\right]
H=[h1h2h3]
那么约束条件就可以写成
A
−
1
[
h
1
h
2
h
3
]
=
[
r
1
r
2
t
]
A^{-1}\left[\begin{matrix} h_1 & h_2 & h_3\end{matrix}\right]=\left[\begin{matrix} r_1 & r_2 & t\end{matrix}\right]
A−1[h1h2h3]=[r1r2t]
(
A
−
1
h
1
)
T
A
−
1
h
2
=
0
(
A
−
1
h
1
)
T
A
−
1
h
1
=
(
A
−
1
h
2
)
T
A
−
1
h
2
\begin{aligned} &(A^{-1}h_1)^TA^{-1}h_2=0\\ &(A^{-1}h_1)^TA^{-1}h_1=({A^{-1}h_2})^TA^{-1}h_2 \end{aligned}
(A−1h1)TA−1h2=0(A−1h1)TA−1h1=(A−1h2)TA−1h2
展开
h
1
T
A
−
T
A
−
1
h
2
=
0
h
1
T
A
−
T
A
−
1
h
1
=
h
2
T
A
−
T
A
−
1
h
2
\begin{aligned} &h_1^TA^{-T}A^{-1}h_2=0\\ &h_1^TA^{-T}A^{-1}h_1=h_2^TA^{-T}A^{-1}h_2 \end{aligned}
h1TA−TA−1h2=0h1TA−TA−1h1=h2TA−TA−1h2
可以看出这两个等式是关于
A
−
T
A
−
1
A^{-T}A^{-1}
A−TA−1的方程。两个平面之间的单应性矩阵H 可以利用DLT计算求得,那么计算出A来,然后就可以计算其他的参数。
B 是一个对称矩阵,有6个参数需要求解,一个棋盘格有两个方程,所以至少需要拍摄3张棋盘格图像。
b
=
[
B
11
B
12
B
22
B
13
B
23
B
33
]
T
b = \left[\begin{matrix} B_{11}& B_{12}&B_{22}&B_{13}&B_{23}&B_{33}\end{matrix}\right]^T
b=[B11B12B22B13B23B33]T
令
h
i
h_i
hi 表示H矩阵第i列的向量 , 那么
h
i
=
[
h
i
1
h
i
2
h
i
3
]
T
h
i
T
B
h
j
=
v
i
j
T
b
h_i = \left[\begin{matrix} h_{i1}&h_{i2}&h_{i3}\end{matrix}\right]^T \\ h_i^TBh_j = v_{ij}^Tb
hi=[hi1hi2hi3]ThiTBhj=vijTb
把方程组化简成
V
b
=
0
Vb=0
Vb=0的形式
[
v
12
T
(
v
11
−
v
22
)
T
]
b
=
0
其
中
v
i
j
=
[
h
i
1
h
j
1
h
i
1
h
j
2
+
h
i
2
h
j
1
h
i
2
h
j
2
h
i
3
h
j
1
+
h
i
1
h
j
3
h
i
3
h
j
2
+
h
i
2
h
j
3
h
i
3
h
j
3
]
T
\left[\begin{matrix}v_{12}^T \\ (v_{11}-v_{22})^T\end{matrix}\right]b=0 \\ 其中 v_{ij}=\left[\begin{matrix} h_{i1}h_{j1}& h_{i1}h_{j2}+h_{i2}h_{j1}&h_{i2}h_{j2}&h_{i3}h_{j1}+h_{i1}h_{j3}&h_{i3}h_{j2}+h_{i2}h_{j3}&h_{i3}h_{j3}\end{matrix}\right]^T
[v12T(v11−v22)T]b=0其中vij=[hi1hj1hi1hj2+hi2hj1hi2hj2hi3hj1+hi1hj3hi3hj2+hi2hj3hi3hj3]T
计算出B之后,就可以对B进行分解,求得相机内参A
v
0
=
(
B
12
B
13
−
B
11
B
23
)
/
(
B
11
B
22
−
B
12
2
)
λ
=
B
33
−
[
B
13
2
+
v
0
(
B
12
B
13
−
B
11
B
23
)
]
/
B
11
a
=
λ
/
B
11
b
=
λ
B
11
/
(
B
11
B
22
−
B
12
2
)
γ
=
−
B
12
a
2
b
/
λ
u
0
=
γ
v
0
/
b
−
B
13
a
2
/
λ
\begin{aligned} &v_0 = (B_{12}B_{13}-B_{11}B_{23})/(B_{11}B_{22}-B_{12}^2)\\ &\lambda=B_{33}-[B_{13}^2+v_0(B_{12}B_{13}-B_{11}B_{23})]/B_{11}\\ &a = \sqrt{\lambda/B_{11}}\\ &b=\sqrt{\lambda{B_{11}/(B_{11}B_{22}-B_{12}^2)}} \\ &\gamma=-B_{12}a^2b/\lambda\\ &u_0 = \gamma{v_0}/b-B_{13}a^2/\lambda \end{aligned}
v0=(B12B13−B11B23)/(B11B22−B122)λ=B33−[B132+v0(B12B13−B11B23)]/B11a=λ/B11b=λB11/(B11B22−B122)γ=−B12a2b/λu0=γv0/b−B13a2/λ
求解出A矩阵之后,相机的外参就可以求解
r
1
=
λ
A
−
1
h
1
r
2
=
λ
A
−
1
h
2
r
3
=
r
1
×
r
2
t
=
λ
A
−
1
h
3
λ
=
1
/
∣
∣
A
−
1
h
1
∣
∣
=
1
/
∣
∣
A
−
1
h
2
∣
∣
\begin{aligned} &r_1 = \lambda{A^{-1}h_1} \\ &r_2 = \lambda{A^{-1}h_2} \\ &r_3 = r_1\times{r_2}\\ &t = \lambda{A^{-1}h_3}\\ &\lambda = 1/||A^{-1}h_1||=1/||A^{-1}h_2|| \end{aligned}
r1=λA−1h1r2=λA−1h2r3=r1×r2t=λA−1h3λ=1/∣∣A−1h1∣∣=1/∣∣A−1h2∣∣
以上都是通过求解方程组,得到的初值,而做相机标定,往往需要拍摄10几张以上,那么求解的结果就不够准确,所以需要对初值进行迭代优化。
构建最大似然估计,使得目标函数求和最小
∑
i
=
1
n
∑
j
=
1
m
∣
∣
m
i
j
−
m
^
(
A
,
R
i
,
t
i
,
M
j
)
∣
∣
2
\sum_{i=1}^{n}\sum_{j=1}^{m}||m_{ij} - \widehat{m}(A, R_i, t_i, M_j)||^2
i=1∑nj=1∑m∣∣mij−m
(A,Ri,ti,Mj)∣∣2
如果把畸变系数也考虑进去
∑ i = 1 n ∑ j = 1 m ∣ ∣ m i j − m ^ ( A , k 1 , k 2 , R i , t i , M j ) ∣ ∣ 2 \sum_{i=1}^{n}\sum_{j=1}^{m}||m_{ij} - \widehat{m}(A, k_1, k_2, R_i, t_i, M_j)||^2 i=1∑nj=1∑m∣∣mij−m (A,k1,k2,Ri,ti,Mj)∣∣2
好了原理就讲这么多,那么开始实现吧。
下一讲,重点用C++代码实现…