一、世界坐标到图像坐标
1. 相机外参
[ X c Y c Z c 1 ] = [ R t 0 1 ] [ X w Y w Z w 1 ] (1.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} \tag{1.1} XcYcZc1 =[R0t1] XwYwZw1 (1.1)
E = [ R T 0 1 ] E − 1 = [ R ⊤ − R ⊤ T 0 ⊤ 1 ] (1.2) \begin{align*} E =& \begin{bmatrix} R & T \\ 0 & 1 \end{bmatrix} \\ E^{-1} =& \begin{bmatrix} R^{\top} & -R^{\top}T \\ 0^{ \top} & 1 \end{bmatrix} \end{align*} \tag{1.2} E=E−1=[R0T1][R⊤0⊤−R⊤T1](1.2)
2. 小孔成像模型
相机可以抽象为最简单的形式:一个小孔和一个成像平面,小孔位于成像平面和真实的三维场景之间,任何来自真实世界的光只有通过小孔才能到达成像平面。因此在成像平面和通过小孔看到的真实三维场景存在着一种对应关系,也就是图像中的二维像素点和真实三维世界的三维点存在某种变换关系。找到这种变换关系,就可以利用图像中的二维信息点来回复三维信息。
小孔成像实际就是将相机中的三维点变换到成像平面中图像坐标系中的二维点。假设,三维空间中点P,其在相机坐标系下坐标为:
P
c
=
[
X
,
Y
,
Z
]
⊤
P_c = [X,Y,Z]^{\top}
Pc=[X,Y,Z]⊤其像点
p
=
[
x
,
y
]
⊤
p = [x, y]^{\top}
p=[x,y]⊤由相似三角形原理得到:
Z
f
=
X
x
=
Y
y
(1.3)
\frac{Z}{f}=\frac{X}{x}=\frac{Y}{y} \tag{1.3}
fZ=xX=yY(1.3)
x
=
f
X
Z
y
=
f
Y
Z
(1.4)
\begin{align*} x = f\frac{X}{Z} \\y = f\frac{Y}{Z} \end{align*} \tag{1.4}
x=fZXy=fZY(1.4)
3. 相机内参
机内参由下面的两部分组成:
- 射影变换本身的参数,相机的焦点到成像平面的距离,也就是焦距 f f f
- 从成像平面坐标系到像素坐标系的变换。上面推导中使用的像点
p
=
[
x
,
y
]
⊤
p=[x,y]^{\top}
p=[x,y]⊤是成像平面坐标系下,以成像平面的中心为原点。而实际像素点的表示方法是以像素来描述的,坐标原点通常是图像的左上角,X轴水平向左,Y轴竖直向下。像素是一个矩形块,这里假设水平和数值方向的长度为:
α
\alpha
α和
β
\beta
β.所以像素坐标和成像平面坐标之间,相差了一个缩放和原点的平移。其公式如下:
u = α x + c x v = β x + c y (1.5) \begin{align*} u &= \alpha x + c_x \\ v &= \beta x + c_y \end{align*} \tag{1.5} uv=αx+cx=βx+cy(1.5)
f
x
=
α
f
f
y
=
β
f
(1.6)
\begin{align*} f_x =& \alpha f \\ f_y =& \beta f \end{align*} \tag{1.6}
fx=fy=αfβf(1.6)
结合(3)~(5)从相机坐标到像素坐标的转换关系为:
[
u
v
1
]
=
1
Z
c
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
X
c
Y
c
Z
c
]
(1.7)
\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z_c} \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ \end{bmatrix} \tag{1.7}
uv1
=Zc1
fx000fy0cxcy1
XcYcZc
(1.7)
通过上面的推导就得到了相机的内参矩阵(Cammera Intrinsics) K:
K
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
K
−
1
=
[
1
f
x
0
c
x
f
x
0
1
f
y
c
y
f
y
0
0
1
]
(1.8)
\begin{align*} K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \\ K^{-1} = \begin{bmatrix} \frac{1}{f_x} & 0 & \frac{c_x}{f_x} \\ \\ 0 & \frac{1}{f_y} & \frac{c_y}{f_y} \\ \\ 0 & 0 & 1 \end{bmatrix} \end{align*} \tag{1.8}
K=
fx000fy0cxcy1
K−1=
fx1000fy10fxcxfycy1
(1.8)
- fx,fy 和相机的焦距,像素当量大小有关
- cx,cy 是平移距离,和相机成像平面的大小有关
4. 从世界坐标系到图像坐标系
Z
c
[
u
v
1
]
=
[
α
0
c
x
0
β
c
y
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.9)
Z_c\begin{bmatrix} u \\ v \\ 1 \end{bmatrix}= \begin{bmatrix} \alpha&0&c_x\\ 0&\beta&c_y\\ 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 \\ \end{bmatrix} \tag{1.9}
Zc
uv1
=
α000β0cxcy1
f000f0001000
[R0t1]
XwYwZw
(1.9)
p
=
1
Z
c
[
K
∣
0
]
[
R
∣
T
]
P
w
(1.10)
p = \frac{1}{Z_c}[K|0][R|T]P_w \tag{1.10}
p=Zc1[K∣0][R∣T]Pw(1.10)
5. 多相机的相对位姿
p
a
=
1
Z
c
a
[
K
∣
0
]
[
R
a
∣
T
a
]
P
w
p
b
=
1
Z
c
b
[
K
∣
0
]
[
R
b
∣
T
b
]
P
w
(1.11)
\begin{align*} p_a =& \frac{1}{Z_{ca}}[K|0][R_a|T_a]P_w \\ p_b =& \frac{1}{Z_{cb}}[K|0][R_b|T_b]P_w \end{align*} \tag{1.11}
pa=pb=Zca1[K∣0][Ra∣Ta]PwZcb1[K∣0][Rb∣Tb]Pw(1.11)
P
w
=
Z
c
b
[
R
b
∣
T
b
]
−
1
[
K
−
1
∣
0
]
p
b
(1.12)
P_w = Z_{cb}[R_b|T_b]^{-1}[K^{-1}|0]p_b \tag{1.12}
Pw=Zcb[Rb∣Tb]−1[K−1∣0]pb(1.12)
可以得到相机B的图像坐标到相机A的图像坐标:
p
a
=
[
R
a
∣
T
a
]
[
R
b
∣
T
b
]
−
1
[
K
11
−
1
K
12
−
1
K
13
−
1
K
21
−
1
K
22
−
1
K
23
−
1
K
31
−
1
K
32
−
1
K
33
−
1
0
0
1
Z
c
b
]
p
b
(1.13)
p_a = [R_a|T_a][R_b|T_b]^{-1} \begin{bmatrix} K_{11}^{-1} & K_{12}^{-1} & K_{13}^{-1} \\ \\ K_{21}^{-1} & K_{22}^{-1} & K_{23}^{-1} \\ \\ K_{31}^{-1} & K_{32}^{-1} & K_{33}^{-1} \\ \\ 0 & 0 & \frac{1}{Z_{cb}} \end{bmatrix} p_b \tag{1.13}
pa=[Ra∣Ta][Rb∣Tb]−1
K11−1K21−1K31−10K12−1K22−1K32−10K13−1K23−1K33−1Zcb1
pb(1.13)
令:
M
a
b
M_{ab}
Mab 为相机b 到 相机 a 的相机外参转换关系即:相机的相对位姿。则有:
M
a
b
=
[
R
a
∣
T
a
]
[
R
b
∣
T
b
]
−
1
(1.14)
\begin{align*} M_{ab} =& [R_a|T_a][R_b|T_b]^{-1} \\ \end{align*} \tag{1.14}
Mab=[Ra∣Ta][Rb∣Tb]−1(1.14)
则从相机B的像素坐标的点到相机A像素坐标系的点的映射关系为:
p
a
=
Z
c
b
Z
c
a
[
K
a
∣
0
]
M
a
b
[
K
11
−
1
K
12
−
1
K
13
−
1
K
21
−
1
K
22
−
1
K
23
−
1
K
31
−
1
K
32
−
1
K
33
−
1
0
0
1
Z
c
b
]
p
b
(1.15)
\begin{align*} p_a =& \frac{Z_{cb}}{Z_{ca}}[K_a|0]M_{ab} \begin{bmatrix} K_{11}^{-1} & K_{12}^{-1} & K_{13}^{-1} \\ \\ K_{21}^{-1} & K_{22}^{-1} & K_{23}^{-1} \\ \\ K_{31}^{-1} & K_{32}^{-1} & K_{33}^{-1} \\ \\ 0 & 0 & \frac{1}{Z_{cb}} \end{bmatrix}p_b \end{align*} \tag{1.15}
pa=ZcaZcb[Ka∣0]Mab
K11−1K21−1K31−10K12−1K22−1K32−10K13−1K23−1K33−1Zcb1
pb(1.15)
二 、相机畸变模型
1. 径向畸变
实际情况中我们使用
r
=
0
r=0
r=0 处的泰勒展开的前几项来近似描述径向畸变,径向畸变后的归一化坐标为:
{
x
distorted
=
x
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
y
distorted
=
y
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
(2.1)
\begin{align*} \begin{cases} x_{\text{distorted}} = x(1 + k_1r^2 + k_2r^4 + k_3r^6)\\ y_{\text{distorted}} = y(1 + k_1r^2 + k_2r^4 + k_3r^6) \end{cases} \end{align*} \tag{2.1}
{xdistorted=x(1+k1r2+k2r4+k3r6)ydistorted=y(1+k1r2+k2r4+k3r6)(2.1)
2. 切向畸变
切向畸变是由于透镜的安装位置误差即:相机的组装过程中由于不能使得透镜和成像平面严格平行,切向畸变需要两个额外的畸变参数来描述,切向畸变后的归一化坐标为:
{ x distored = x + 2 p 1 x y + p 2 ( r 2 + 2 x ) y distored = y + 2 p 2 x y + p 1 ( r 2 + 2 x ) (2.2) \begin{cases} x_{\text{distored} = x + 2p_1xy + p_2(r^2 +2x)} \\ y_{\text{distored} = y + 2p_2xy + p_1(r^2 +2x)} \end{cases} \tag{2.2} {xdistored=x+2p1xy+p2(r2+2x)ydistored=y+2p2xy+p1(r2+2x)(2.2)
3. 相机畸变模型
令
r
2
=
x
2
+
y
2
r^2 = x^2 + y^2
r2=x2+y2。对于相机坐标系中的一点
P
P
P,我们能够通过5个畸变系数找到这个点在像素平面上的正确位置:
{
x
distored
=
x
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
+
2
p
1
x
y
+
p
2
(
r
2
+
2
x
)
y
distored
=
y
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
+
2
p
2
x
y
+
p
1
(
r
2
+
2
x
)
(2.3)
\begin{cases} x_{\text{distored} = x(1+ k_1r^2+k_2r^4 + k_3r^6) + 2p_1xy + p_2(r^2 +2x)} \\ y_{\text{distored} = y(1+ k_1r^2+k_2r^4 + k_3r^6) + 2p_2xy + p_1(r^2 +2x)} \end{cases} \tag{2.3}
{xdistored=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x)ydistored=y(1+k1r2+k2r4+k3r6)+2p2xy+p1(r2+2x)(2.3)
三、实验结果
1. 图像拼接
拼接图片:
拼接结果: