相机标定(Camera Calibration)原理、实现流程及代码
定义
相机标定:计算相机内参 (Intrinsic, K)、外参 (Extrinsic, t)、畸变参数 (Distortion Paramters, D) 的过程。
相机标定的目的
确定空间中物体上点的三维几何位置与该点在图像中对应点的二维几何位置的对应关系。
相机标定方法
- 传统相机标定法
- 优点:可用于任意相机模型、精度高
- 缺点:需要标定物、算法复杂
- 方法:Tsai 两步法、张氏标定法
- 主动视觉相机标定法
- 优点:不需要标定物、算法简单、稳定
- 缺点:成本高、设备贵
- 方法:主动系统控制相机运动完成标定
- 相机自标定法
- 优点:灵活、支持在线标定
- 缺点:精度低、不稳定
- 方法:分层逐步标定、基于 Kruppa 方程
方法详细介绍
这里只介绍传统相机标定中的 Tsai 两步法和张氏标定法。
Tsai 两步法
首先,线性求得相机参数,之后考虑畸变因素,从而得到初始的参数值,最后通过非线性优化得到最终的相机参数。
- 优点:速度快
- 缺点:仅考虑径向畸变 (Radial Distortion)
张氏标定法
使用二维方格组成的标定板进行标定,采集标定板不同位姿的图像,提取图像中角点像素坐标,然后,通过单应矩阵计算出相机的内外参数初始值。畸变系数通过利用非线性最小二乘法估计,最后使用极大似然估计法优化参数。
- 优点:操作简单,精度较高
- 缺点:只考虑了径向畸变,没有考虑切向畸变
涉及的坐标系
本章节主要涉及到世界坐标系、相机坐标系、像素坐标系(图像坐标系)及坐标系之间的转换。
世界坐标系 World Coordinate System
世界坐标系描述相机位置,单位:米,常用坐标表示: x w , y w , z w x_w, y_w, z_w xw,yw,zw.
相机坐标 Camera Coordinate System
相机坐标系的原点位于镜头的光点,x, y 轴分别与相面的两边平行,z轴为镜头光轴,与像平面垂直。单位:米,常用坐标表示:
x
c
,
y
c
,
z
c
x_c, y_c, z_c
xc,yc,zc.
图中,
O
O
O 点是镜头的光点;
∠
A
O
B
\angle AOB
∠AOB 是视角;平面
E
F
EF
EF 是焦平面,即成像平面,相机 CMOS 位于这个平面上;
O
M
OM
OM 和
O
N
ON
ON 为物距,一般用
u
u
u 表示;
O
P
OP
OP 为像距,一般用
v
v
v 表示。
O
P
,
O
M
,
O
N
OP, OM, ON
OP,OM,ON 所在的轴上即为镜头的光轴,也即相机中轴线。
图像坐标系 Image Coordinate System
图像坐标系的原点为成像平面的中点,单位:毫米,常用坐标表示: x , y x,y x,y.
像素坐标系 Pixel Coordinate System
像素坐标系的原点为图像左上角,单位:像素,常用坐标表示:
u
,
v
u,v
u,v. 其分别表示该像素在图像中的列数和行数。
坐标系转换
世界坐标系 → \rightarrow →相机坐标系
[ x c y c z c 1 ] = [ R t 0 1 ] [ x w y w z w 1 ] \begin{bmatrix} x_c\\ y_c\\ z_c\\ 1\\ \end{bmatrix}=\begin{bmatrix} R&t\\ 0&1\\ \end{bmatrix} \begin{bmatrix} x_w\\ y_w\\ z_w\\ 1 \end{bmatrix} xcyczc1 =[R0t1] xwywzw1
其中,旋转矩阵 R ∈ R 3 × 3 R\in \mathbb{R}^{3\times 3} R∈R3×3,平移矩阵 t ∈ R 3 × 1 t\in \mathbb{R}^{3\times 1} t∈R3×1.
二维旋转矩阵为:(主动旋转,向量逆时针旋转)
R = [ cos θ − sin θ sin θ cos θ ] R=\begin{bmatrix} \cos{\theta}&-\sin{\theta} \\ \sin{\theta}&\cos{\theta} \\ \end{bmatrix} R=[cosθsinθ−sinθcosθ](被动旋转,坐标轴逆时针旋转,相当于向量顺时针旋转)
R = [ cos θ sin θ − sin θ cos θ ] R=\begin{bmatrix} \cos{\theta}&\sin{\theta} \\ -\sin{\theta}&\cos{\theta} \\ \end{bmatrix} R=[cosθ−sinθsinθcosθ]
三维旋转矩阵就在二维旋转矩阵上增加了一个旋转轴。
绕x轴逆时针旋转:(x坐标值不变)
R = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] R=\begin{bmatrix} 1&0&0\\ 0&\cos{\theta}&-\sin{\theta} \\ 0&\sin{\theta}&\cos{\theta} \\ \end{bmatrix} R= 1000cosθsinθ0−sinθcosθ 绕y轴逆时针旋转:(y坐标值不变)
R = [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R=\begin{bmatrix} \cos{\theta}&0&\sin{\theta} \\ 0&1&0 \\ -\sin{\theta}&0&\cos{\theta} \\ \end{bmatrix} R= cosθ0−sinθ010sinθ0cosθ
绕z轴逆时针旋转:(z坐标值不变)
R = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] R=\begin{bmatrix} \cos{\theta}&-\sin{\theta}&0 \\ \sin{\theta}&\cos{\theta}&0 \\ 0&0&1 \end{bmatrix} R= cosθsinθ0−sinθcosθ0001
R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] , t = [ t x t y t z ] R= \begin{bmatrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\ r_{31}&r_{32}&r_{33}\\ \end{bmatrix}, t=\begin{bmatrix} t_x\\ t_y\\ t_z\\ \end{bmatrix} R= r11r21r31r12r22r32r13r23r33 ,t= txtytz
物体坐标从世界坐标系转换为像素坐标系是刚体转换,与相机无关,故 [ R t 0 1 ] \begin{bmatrix} R&t\\ 0&1\\ \end{bmatrix} [R0t1]称为外参矩阵。
相机坐标系 → \rightarrow →图像坐标系
相机坐标中的点(
x
c
,
y
c
,
z
c
x_c,y_c,z_c
xc,yc,zc)投影到图像坐标中的点为(
x
,
y
x,y
x,y)。根据相似三角形原理可以得到两个点之间的比例关系:
x
c
x
=
z
c
f
,
y
c
y
=
z
c
f
\frac{x_c}{x}=\frac{z_c}{f},\frac{y_c}{y}=\frac{z_c}{f}
xxc=fzc,yyc=fzc
故图像坐标系中的点(
x
,
y
x,y
x,y)可表示为:
x
=
f
x
c
z
c
,
y
=
f
y
c
z
c
x=\frac{fx_c}{z_c},y=\frac{fy_c}{z_c}
x=zcfxc,y=zcfyc
从而转换为矩阵表示为:
z
c
[
x
y
1
]
=
[
f
0
0
0
0
f
0
0
0
0
1
0
]
[
x
c
y
c
z
c
1
]
z_c\begin{bmatrix} x\\ y\\ 1\\ \end{bmatrix}= \begin{bmatrix} f&0&0&0\\ 0&f&0&0\\ 0&0&1&0\\ \end{bmatrix} \begin{bmatrix} x_c\\ y_c\\ z_c\\ 1\\ \end{bmatrix}
zc
xy1
=
f000f0001000
xcyczc1
其中, f f f 是焦距。
图像坐标系 → \rightarrow →像素坐标系
像素坐标系与图像坐标系为平移关系,它们之间的转换关系可表示为:
[
u
v
1
]
=
[
1
/
d
x
0
u
0
0
1
/
d
y
v
0
0
0
1
]
[
x
y
1
]
\begin{bmatrix} u\\ v\\ 1\\ \end{bmatrix}= \begin{bmatrix} 1/dx&0&u_0\\ 0&1/dy&v_0\\ 0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ 1\\ \end{bmatrix}
uv1
=
1/dx0001/dy0u0v01
xy1
其中, d x , d y dx,dy dx,dy 表示单一像素的物理尺寸,单位:毫米; u 0 , v 0 u_0,v_0 u0,v0 为图像原点坐标。
世界坐标系 → \rightarrow →像素坐标系
将上面的转换矩阵组合起来,即得到是世界坐标系转换为像素坐标ix
z
c
[
u
v
1
]
=
[
1
/
d
x
0
u
0
0
1
/
d
y
v
0
0
0
1
]
[
f
0
0
0
0
f
0
0
0
0
1
0
]
[
R
t
0
1
]
[
x
w
y
w
z
w
1
]
z_c\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=\begin{bmatrix} 1/dx&0&u_0\\ 0&1/dy&v_0\\ 0&0&1\\ \end{bmatrix} \begin{bmatrix} f&0&0&0\\ 0&f&0&0\\ 0&0&1&0\\ \end{bmatrix} \begin{bmatrix} R&t\\ 0&1\\ \end{bmatrix} \begin{bmatrix} x_w\\ y_w\\ z_w\\ 1 \end{bmatrix}
zc
uv1
=
1/dx0001/dy0u0v01
f000f0001000
[R0t1]
xwywzw1
= [ α x 0 u 0 0 0 α y v 0 0 0 0 1 0 ] [ R t 0 1 ] [ x w y w z w 1 ] = M 1 M 2 X w = M X w =\begin{bmatrix} \alpha_x&0&u_0&0\\ 0&\alpha_y&v_0&0\\ 0&0&1&0 \end{bmatrix} \begin{bmatrix} R&t\\ 0&1\\ \end{bmatrix} \begin{bmatrix} x_w\\ y_w\\ z_w\\ 1 \end{bmatrix}=M_1 M_2 X_w =MX_w = αx000αy0u0v01000 [R0t1] xwywzw1 =M1M2Xw=MXw
其中, α x = f / d x , α y = f / d y \alpha_x=f/dx,\alpha_y=f/dy αx=f/dx,αy=f/dy 称为 u , v u, v u,v 轴的尺度因子; M 1 M_1 M1 为相机内参矩阵; M 2 M_2 M2 为相机外参矩阵; M M M 为投影矩阵。这是无畸变情况。
考虑畸变的情况下,内参矩阵
M
1
M_1
M1 为:
[
f
/
d
x
−
(
f
c
o
t
θ
)
/
d
x
u
0
0
0
f
/
(
d
y
s
i
n
θ
)
v
0
0
0
0
1
0
]
\begin{bmatrix} f/dx&-(fcot\theta)/dx&u_0&0\\ 0&f/(dysin\theta)&v_0&0\\ 0&0&1&0\\ \end{bmatrix}
f/dx00−(fcotθ)/dxf/(dysinθ)0u0v01000
其中, θ \theta θ 为 CMOS 的横边和纵边之间的角度( 90 ° 90\degree 90° 表示无误差)。
内参矩阵也有如下表示:
[
f
x
γ
u
0
0
f
y
v
0
0
0
1
]
\begin{bmatrix} f_x&\gamma&u_0\\ 0&f_y&v_0\\ 0&0&1\\ \end{bmatrix}
fx00γfy0u0v01
相机畸变参数 Distortion Parameters
定义
畸变主要分为径向畸变和切向畸变。
径向畸变:沿着半径方向(远离或靠近中心)的变化。
切向畸变:沿着切线方向发生变化。
径向畸变
使用一个多项式函数描述畸变前后的坐标变化:
x
^
=
x
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
y
^
=
y
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
\hat{x}=x(1+k_1 r^2 + k_2 r^4 + k_3 r^6) \\ \hat{y}=y(1+k_1 r^2 + k_2 r^4 + k_3 r^6)
x^=x(1+k1r2+k2r4+k3r6)y^=y(1+k1r2+k2r4+k3r6)
其中,
[
x
,
y
]
T
[x,y]^{T}
[x,y]T是理想无畸变归一化后的图像坐标坐标;
[
x
^
,
y
^
]
T
[\hat{x},\hat{y}]^{T}
[x^,y^]T是畸变后归一化后的图像坐标;
r
r
r 表示点离坐标原点的距离。
畸变较小,矫正主要是
k
1
k_1
k1 作用;畸变大,主要是
k
3
k_3
k3 起作用。
切向畸变
对于切向畸变使用参数
p
1
,
p
2
p_1,p_2
p1,p2 进行纠正:
x
^
=
x
+
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
y
^
=
y
+
p
1
(
r
2
+
2
y
2
)
+
2
p
2
x
y
\hat{x}=x+2p_1 xy+p_2(r^2+2x^2) \\ \hat{y}=y+p_1(r^2+2y^2)+2p_2xy
x^=x+2p1xy+p2(r2+2x2)y^=y+p1(r2+2y2)+2p2xy
张氏标定法
张正友标定法标定相机的内外参的思路:
- 求解内参矩阵与外参矩阵的积
- 求解内参
- 求解外参
- 求解畸变参数
- L-M 算法参数优化
求解内参矩阵与外参矩阵的积
内参矩阵
M
1
M_1
M1(考虑畸变):
M
1
=
[
f
/
d
x
−
(
f
c
o
t
θ
)
/
d
x
u
0
0
f
/
(
d
y
s
i
n
θ
)
v
0
0
0
1
]
M_1=\begin{bmatrix} f/dx&-(fcot\theta)/dx&u_0\\ 0&f/(dysin\theta)&v_0\\ 0&0&1\\ \end{bmatrix}
M1=
f/dx00−(fcotθ)/dxf/(dysinθ)0u0v01
外参矩阵
M
2
M_2
M2:
M
2
=
[
R
1
R
2
t
]
M_2=\begin{bmatrix} R_1&R_2&t \\ \end{bmatrix}
M2=[R1R2t]
其中, R 1 , R 2 R_1, R_2 R1,R2为旋转矩阵 R R R 的第一二列。
Q:为什么这里的旋转矩阵只取第一二列?
A:张氏标定板将世界坐标系的原点选在棋盘格上,故棋盘格上的任一点的物理坐标 z w = 0 z_w=0 zw=0,从而将旋转矩阵的第三列 R 3 R_3 R3消掉。可通过旋转矩阵性质(列与列之间单位正交), R 1 , R 2 R_1,R_2 R1,R2叉乘即可得到 R 3 R_3 R3, R 3 = R 1 × R 2 R_3=R_1\times R_2 R3=R1×R2。
内参矩阵和外参矩阵相乘得到矩阵
H
H
H:
H
=
M
1
M
2
=
[
f
/
d
x
−
(
f
c
o
t
θ
)
/
d
x
u
0
0
f
/
(
d
y
s
i
n
θ
)
v
0
0
0
1
]
[
R
1
R
2
t
]
H=M_1M_2=\begin{bmatrix} f/dx&-(fcot\theta)/dx&u_0\\ 0&f/(dysin\theta)&v_0\\ 0&0&1\\ \end{bmatrix}\begin{bmatrix} R_1&R_2&t \\ \end{bmatrix}
H=M1M2=
f/dx00−(fcotθ)/dxf/(dysinθ)0u0v01
[R1R2t]
该矩阵 H H H也被叫做单应性矩阵。
单应性 (Homography):从一个平面到另一个平面的投影映射关系。
Homography: one of two or more words spelled alike but different in meaning or derivation or pronunciation (such as the bow of a ship, a bow and arrow)
单应性矩阵也可以这样写,根据世界坐标系到相机坐标系的转换,从而定义相机标定的单应性矩阵(从物体平面到成像平面)为:
H
=
1
z
c
[
α
x
γ
u
0
0
0
α
y
v
0
0
0
0
1
0
]
[
R
t
0
1
]
H=\frac{1}{z_c}\begin{bmatrix} \alpha_x&\gamma&u_0&0\\ 0&\alpha_y&v_0&0\\ 0&0&1&0 \end{bmatrix} \begin{bmatrix} R&t\\ 0&1\\ \end{bmatrix}
H=zc1
αx00γαy0u0v01000
[R0t1]
也即:
[
u
v
1
]
=
1
z
c
H
[
U
V
1
]
\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=\frac{1}{z_c}H \begin{bmatrix} U\\ V\\ 1 \end{bmatrix}
uv1
=zc1H
UV1
从而得到:
u
=
H
11
U
+
H
12
V
+
H
13
H
31
U
+
H
32
V
+
H
33
,
v
=
H
21
U
+
H
22
V
+
H
23
H
31
U
+
H
32
V
+
H
33
u=\frac{H_{11}U+H_{12}V+H_{13}}{H_{31}U+H_{32}V+H_{33}},v=\frac{H_{21}U+H_{22}V+H_{23}}{H_{31}U+H_{32}V+H_{33}}
u=H31U+H32V+H33H11U+H12V+H13,v=H31U+H32V+H33H21U+H22V+H23
其中, ( u , v ) (u,v) (u,v) 是像素坐标(通过图像识别算法得到), ( U , V ) (U,V) (U,V)是世界坐标系下的标定板的角点坐标(格子大小是人为规定的)。
求解内参 M 1 M_1 M1
一些性质:
R
1
,
R
2
R_1,R_2
R1,R2是旋转矩阵
R
R
R的两列,存在单位正交关系:
R
1
T
R
2
=
0
,
R
1
T
R
1
=
R
2
T
R
2
=
1
R_1^TR_2=0,R_1^TR_1=R_2^TR_2=1
R1TR2=0,R1TR1=R2TR2=1
则通过
H
=
M
1
[
R
1
R
2
t
]
H=M_1\begin{bmatrix} R_1&R_2&t \\ \end{bmatrix}
H=M1[R1R2t]可以计算得到:
R
1
=
M
1
−
1
H
1
,
R
2
=
M
1
−
1
H
2
R_1=M_1^{-1}H_1,R_2=M_1^{-1}H_2
R1=M1−1H1,R2=M1−1H2
其中,
H
1
,
H
2
H_1,H_2
H1,H2为矩阵
H
H
H的第一二列。
代入上式:
(
M
1
−
1
H
1
)
T
M
1
−
1
H
2
=
0
,
(
M
1
−
1
H
1
)
T
M
1
−
1
H
1
=
(
M
1
−
1
H
2
)
T
M
1
−
1
H
2
=
1
(M_1^{-1}H_1)^TM_1^{-1}H_2=0,\\ (M_1^{-1}H_1)^TM_1^{-1}H_1=(M_1^{-1}H_2)^TM_1^{-1}H_2=1
(M1−1H1)TM1−1H2=0,(M1−1H1)TM1−1H1=(M1−1H2)TM1−1H2=1
记上式中出现的 M 1 − T M 1 − 1 M_1^{-T}M_1^{-1} M1−TM1−1为 B B B,故 B B B为对称阵。
求解 B B B:
根据之前提到的内参矩阵
M
1
M_1
M1,计算得到
B
B
B。
M
1
=
[
f
/
d
x
−
(
f
c
o
t
θ
)
/
d
x
u
0
0
f
/
(
d
y
s
i
n
θ
)
v
0
0
0
1
]
M_1=\begin{bmatrix} f/dx&-(fcot\theta)/dx&u_0\\ 0&f/(dysin\theta)&v_0\\ 0&0&1\\ \end{bmatrix}
M1=
f/dx00−(fcotθ)/dxf/(dysinθ)0u0v01
B
=
M
1
−
T
M
1
−
1
=
[
B
11
B
12
B
13
B
12
B
22
B
23
B
13
B
23
B
33
]
B=M_1^{-T}M_1^{-1}= \begin{bmatrix} B_{11}&B_{12}&B_{13}\\ B_{12}&B_{22}&B_{23}\\ B_{13}&B_{23}&B_{33}\\ \end{bmatrix}
B=M1−TM1−1=
B11B12B13B12B22B23B13B23B33
另外,使用
B
B
B重新表示
R
1
,
R
2
R_1,R_2
R1,R2的正交约束矩阵为:
H
1
T
B
H
2
=
0
,
H
2
T
B
H
2
=
1
H_1^TBH_2=0,H_2^TBH_2=1
H1TBH2=0,H2TBH2=1
故,计算 H i T B H j = [ H 1 i H 2 i H 3 i ] [ B 11 B 12 B 13 B 12 B 22 B 23 B 13 B 23 B 33 ] [ H 1 j H 2 j H 3 j ] H_i^TBH_j= \begin{bmatrix} H_{1i}&H_{2i}&H_{3i} \end{bmatrix} \begin{bmatrix} B_{11}&B_{12}&B_{13}\\ B_{12}&B_{22}&B_{23}\\ B_{13}&B_{23}&B_{33}\\ \end{bmatrix} \begin{bmatrix} H_{1j} \\ H_{2j} \\ H_{3j} \end{bmatrix} HiTBHj=[H1iH2iH3i] B11B12B13B12B22B23B13B23B33 H1jH2jH3j
化简得:
H
i
T
B
H
j
=
v
i
j
b
H_i^TBH_j=v_{ij}b
HiTBHj=vijb,再通过上面的
R
1
,
R
2
R_1,R_2
R1,R2正交约束方程可继续化简为:
v
12
T
b
=
0
,
v
11
T
b
=
v
22
T
b
=
1
v_{12}^Tb=0,v_{11}^{T}b=v_{22}^Tb=1
v12Tb=0,v11Tb=v22Tb=1
即:
[
v
12
T
v
11
T
−
v
22
T
]
b
=
v
b
=
0
\begin{bmatrix} v_{12}^T \\ v_{11}^T-v_{22}^T \end{bmatrix}b=vb=0
[v12Tv11T−v22T]b=vb=0
因为矩阵 H H H已知,矩阵 v v v由矩阵 H H H元素构成,故 v v v已知。
故,接下来只需要求解
b
b
b:
标定板可以提供一个
v
b
=
0
vb=0
vb=0的约束关系,该约束关系含两个约束方程。但
b
b
b有6个未知元素,单张图片提供的两个约束方程不足以解出
b
b
b。故取3张标定板图像,即3个
v
b
=
0
vb=0
vb=0的约束关系,也即6个约束方程,从而可求解
b
b
b。
当标定板图像大于3时,可采用最小二乘法拟合最佳 b b b,最终得到矩阵 B B B。
因为, B B B是由内参矩阵 M 1 M_1 M1表示,故最终可求得内参矩阵。
详细求解过程请看这篇文章。
求解外参 M 2 M_2 M2
因为相机内参矩阵不受相机和标定板位置关系影响,故相机内参矩阵不变。每张标定板图像对应的外参矩阵也是不同的。
在关系 M 1 [ R 1 R 2 t ] = H M_1\begin{bmatrix}R_1&R_2&t\end{bmatrix}=H M1[R1R2t]=H中,已知 H , M 1 H,M_1 H,M1。可通过 [ R 1 R 2 t ] = M 1 − 1 H \begin{bmatrix}R_1&R_2&t\end{bmatrix}=M_1^{-1}H [R1R2t]=M1−1H求得每张图像的外参矩阵 M 2 = [ R 1 R 2 t ] M_2=\begin{bmatrix}R_1&R_2&t\end{bmatrix} M2=[R1R2t]。
此时,得到相机外参。
求解畸变参数
需要知道,张氏标定法仅考虑了影响较大的径向畸变。
径向畸变公式(2阶)为:
x
^
=
x
(
1
+
k
1
r
2
+
k
2
r
4
)
y
^
=
y
(
1
+
k
1
r
2
+
k
2
r
4
)
\hat{x}=x(1+k_1 r^2 + k_2 r^4) \\ \hat{y}=y(1+k_1 r^2 + k_2 r^4)
x^=x(1+k1r2+k2r4)y^=y(1+k1r2+k2r4)
其中, [ x , y ] T [x,y]^{T} [x,y]T是未矫正的点坐标,为归一化后的点; [ x ^ y ^ ] T \begin{bmatrix} \hat{x}&\hat{y}\end{bmatrix}^T [x^y^]T为矫正后无畸变的归一化后的图像坐标; r r r 表示图像像素点到图像中心点的距离,即 r 2 = x 2 + y 2 r^2=x^2+y^2 r2=x2+y2。
图像坐标和像素坐标的转化关系:
[
u
v
1
]
=
[
1
d
x
−
cot
θ
d
x
u
0
0
1
d
y
sin
θ
v
0
0
0
1
]
[
x
y
1
]
=
M
1
[
x
y
1
]
\begin{bmatrix} u\\ v\\ 1\\ \end{bmatrix}= \begin{bmatrix} \frac{1}{dx}&-\frac{\cot{\theta}}{dx}&u_0\\ 0&\frac{1}{dy\sin{\theta}}&v_0\\ 0&0&1\\ \end{bmatrix} \begin{bmatrix} x\\ y\\ 1\\ \end{bmatrix}=M_1\begin{bmatrix} x\\ y\\ 1\\ \end{bmatrix}
uv1
=
dx100−dxcotθdysinθ10u0v01
xy1
=M1
xy1
其中,
(
u
,
v
)
(u,v)
(u,v)为理想无畸变的像素坐标。由于
θ
\theta
θ接近于
90
°
90\degree
90°,则上式的仿射变换矩阵可近似为:
[
1
/
d
x
0
u
0
0
1
/
d
y
v
0
0
0
1
]
\begin{bmatrix} 1/dx&0&u_0\\ 0&1/dy&v_0\\ 0&0&1\\ \end{bmatrix}
1/dx0001/dy0u0v01
故有:
u
=
x
d
x
+
u
0
,
v
=
y
d
y
+
v
0
u=\frac{x}{dx}+u_0,v=\frac{y}{dy}+v_0
u=dxx+u0,v=dyy+v0
同理,畸变后的像素坐标
(
u
^
,
v
^
)
(\hat{u},\hat{v})
(u^,v^)为:
u
^
=
x
^
d
x
+
u
0
,
v
^
=
y
^
d
y
+
v
0
\hat{u}=\frac{\hat{x}}{dx}+u_0,\hat{v}=\frac{\hat{y}}{dy}+v_0
u^=dxx^+u0,v^=dyy^+v0
代入径向畸变公式:
u
^
−
u
0
=
(
u
−
u
0
)
(
1
+
k
1
r
2
+
k
2
r
4
)
v
^
−
v
0
=
(
v
−
v
0
)
(
1
+
k
1
r
2
+
k
2
r
4
)
\hat{u}-u_0=(u-u_0)(1+k_1 r^2 + k_2 r^4) \\ \hat{v}-v_0=(v-v_0)(1+k_1 r^2 + k_2 r^4)
u^−u0=(u−u0)(1+k1r2+k2r4)v^−v0=(v−v0)(1+k1r2+k2r4)
化简得:
u
^
=
u
+
(
u
−
u
0
)
(
k
1
r
2
+
k
2
r
4
)
v
^
=
v
+
(
v
−
v
0
)
(
k
1
r
2
+
k
2
r
4
)
\hat{u}=u+(u-u_0)(k_1 r^2 + k_2 r^4) \\ \hat{v}=v+(v-v_0)(k_1 r^2 + k_2 r^4)
u^=u+(u−u0)(k1r2+k2r4)v^=v+(v−v0)(k1r2+k2r4)
即为:
[
(
u
−
u
0
)
r
2
(
u
−
u
0
)
r
4
(
v
−
v
0
)
r
2
(
v
−
v
0
)
r
4
]
[
k
1
k
2
]
=
[
u
^
−
u
v
^
−
v
]
\begin{bmatrix} (u-u_0)r^2&(u-u_0)r^4 \\ (v-v_0)r^2&(v-v_0)r^4 \\ \end{bmatrix}\begin{bmatrix} k_1 \\ k_2 \\ \end{bmatrix}=\begin{bmatrix} \hat{u}-u \\ \hat{v}-v \\ \end{bmatrix}
[(u−u0)r2(v−v0)r2(u−u0)r4(v−v0)r4][k1k2]=[u^−uv^−v]
其中,
u
^
,
u
,
v
^
,
v
\hat{u},u,\hat{v},v
u^,u,v^,v可通过识别标定板角点获得,每个角点可提供两个上述等式。若m张图像,每张图像有n个标定板角点,则有mn个未知数为
k
=
[
k
1
k
2
]
T
k=\begin{bmatrix} k_1&k_2 \end{bmatrix}^T
k=[k1k2]T的约束方程,将约束方程系数矩阵记为
D
D
D,等式右边非齐次项记为
d
d
d,则有:
D
k
=
d
Dk=d
Dk=d
使用最小二乘法可得:
k
=
[
k
1
k
2
]
=
(
D
T
D
)
−
1
D
T
d
k=\begin{bmatrix} k_1 \\ k_2 \end{bmatrix}=(D^TD)^{-1}D^Td
k=[k1k2]=(DTD)−1DTd
此时,畸变矫正参数已标定。
L-M 算法参数优化
由于张氏标定法用了很多近似,需要用到L-M(Levenberg-Marquardt,列文伯格-马夸尔特方法)算法或者高斯牛顿法对参数进行迭代优化。
定义
L-M方法是非线性回归中回归参数最小二乘法的一种方法。其同时使用最速下降法和线性优化方法(泰勒级数/高斯牛顿法)。这么用的原因在于开始阶段参数估计值离最优值远,采用最速下降法;而线性优化方法适用于迭代后期,参数估计值接近最优值。结合这两种能个更快拟合。
L-M算法公式:
X
k
+
1
=
X
k
−
(
G
+
u
×
I
)
−
1
×
▽
f
(
X
k
)
×
f
(
X
k
)
=
X
k
−
J
−
1
×
▽
f
(
X
k
)
×
f
(
X
k
)
X_{k+1}= X_{k}-(G+u\times I)^{-1}\times \bigtriangledown f(X_{k})\times f(X_k) \\ =X_k - J^{-1} \times \bigtriangledown f(X_k) \times f(X_k)
Xk+1=Xk−(G+u×I)−1×▽f(Xk)×f(Xk)=Xk−J−1×▽f(Xk)×f(Xk)
其中,当 u u u很小时,矩阵 J J J接近矩阵 G G G,即为高斯-牛顿法;当 u u u很大时,相当于梯度下降法; f ( X k ) f(X_k) f(Xk)为第 k k k次迭代的函数值。
重投影误差 Reprojection Error
在计算平面单应性矩阵和投影矩阵的时候,往往会使用投影误差来构造代价函数,然后最小化该代价函数,以优化单应性矩阵或投影矩阵。越接近0越好。
重投影误差考虑了单应性矩阵的计算误差,也考虑了图像点的测量误差
定义
重投影误差指的真实三维空间点在图像平面上的投影(也就是图像上的像素点)和重投影(其实是用我们的计算值得到的虚拟的像素点)的差值。
重投影的定义:
- 第一次投影:指相机在拍照的时候,三维空间中的点投影到图像上。
- 随后,利用这些图像对一些特征点进行三角定位,利用几何信息构建三角形来确定三维空间点的位置。
- 最后,利用计算得到的三维点的坐标和计算得到的相机位姿进行第二次投影,再次投影到图像上。
计算
三维空间中点 P P P,在不同像平面上的投影为 p p p,求旋转矩阵 R R R和平移矩阵 t t t,两个矩阵用李代数表示为 ξ \xi ξ。李代数详解
空间点
P
i
=
[
X
i
,
Y
i
,
Z
i
]
T
P_i=[X_i,Y_i,Z_i]^T
Pi=[Xi,Yi,Zi]T,其像素坐标为
p
i
=
[
u
i
,
v
i
]
T
p_i=[u_i,v_i]^T
pi=[ui,vi]T。像素坐标和空间点位置间关系,可通过上面世界坐标和像素坐标转换关系得到:
s
i
[
u
i
v
i
1
]
=
M
1
exp
(
ξ
∧
)
[
X
i
Y
i
Z
i
1
]
s_i\begin{bmatrix} u_i\\ v_i\\ 1 \end{bmatrix}=M_1\exp{(\xi^\land)} \begin{bmatrix} X_i\\ Y_i\\ Z_i\\ 1 \end{bmatrix}
si
uivi1
=M1exp(ξ∧)
XiYiZi1
可写为:
s
i
p
i
=
M
1
exp
(
ξ
∧
)
P
i
s_i p_i=M_1\exp{(\xi^\land)}P_i
sipi=M1exp(ξ∧)Pi
其中,
s
i
s_i
si是深度值,在世界坐标到像素坐标的转换关系中,
s
i
s_i
si对应
z
c
z_c
zc,即相机坐标系中的深度值。
对于任意的反对称矩阵,可以找到一个与之对应的向量,这个关系可以用 ∨ ^\lor ∨表示:
a ∧ = A , A ∨ = a a^{\land}=A,A^\lor=a a∧=A,A∨=a反对称矩阵:对于 n 维矩阵 B B B,满足 B T = − B B^T=-B BT=−B,则称矩阵 B B B为反对称矩阵。
由于,相机位姿未知且存在观测误差,要最小化这个误差,即重投影误差,通过:
- 将这个误差求和;
- 构建最小二乘法问题;
- 寻找相机位姿 ξ \xi ξ;
- 使用Bundle adjustment优化最小化误差。
ξ ∗ = arg min ξ 1 2 ∑ i = 1 n ∣ ∣ p i − 1 s i M 1 exp ( ξ ∧ ) P i ∣ ∣ 2 2 \xi^*=\arg\min_{\xi} \frac{1}{2} \sum^{n}_{i=1}\vert\vert p_i-\frac{1}{s_i}M_1 \exp{(\xi^\land)} P_i \vert\vert^2_2 ξ∗=argξmin21i=1∑n∣∣pi−si1M1exp(ξ∧)Pi∣∣22
Bundle adjustment 优化
Bundle adjustment 优化使得重投影误差的和达到最小。
bundle, 来源于 bundle of light (光束),指三维空间中的点投影到像平面上的光束, 重投影利用这些光束来构建的,故称为光束法。
代码实现
- 首先,采集标定板图像(一般20-40张,角度不宜过大)
- 使用OpenCV生成标定图像,其中
# create the conner in world space
w, h = shape_inner_corner
# cp_int: corner point in int form, save the coordinate of corner points in world sapce in 'int' form
# like (0,0,0), (1,0,0), (2,0,0) ...., (10,7,0)
cp_int = np.zeros((w * h, 3), np.float32)
cp_int[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)
# cp_world: corner point in world space, save the coordinate of corner points in world space
cp_world = cp_int * size_grid
- 读取图像
- 然后使用OpenCV检测标定板角点
# find the corners, cp_img: corner points in pixel space
ret, cp_img = cv2.findChessboardCorners(gray_img, (w, h), None)
- 标定相机
ret, mat_intri, coff_dis, v_rot, v_trans = cv2.calibrateCamera(points_world, points_pixel, gray_img.shape[::-1], None, None)
- 计算重投影误差
# calculate the error of reproject
total_error = 0
for i in range(len(points_world)):
points_pixel_repro, _ = cv2.projectPoints(points_world[i], v_rot[i], v_trans[i], mat_intri, coff_dis)
error = cv2.norm(points_pixel[i], points_pixel_repro, cv2.NORM_L2) / len(points_pixel_repro)
total_error += error
print("Average error of reproject: {}".format(total_error / len(points_world)))
- 矫正畸变
img = cv2.imread(img_path)
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(self.mat_intri, self.coff_dis, (w,h), 0, (w,h))
dst = cv2.undistort(img, self.mat_intri, self.coff_dis, None, newcameramtx)
# clip the image
# x, y, w, h = roi
# dst = dst[y:y+h, x:x+w]
cv2.imwrite(os.path.join(save_dir, img_name), dst)