一个坐标系可以通过一个原点 O O O和三个基来 e 1 , e 2 , e 3 \boldsymbol{e_1},\boldsymbol{e_2},\boldsymbol{e_3} e1,e2,e3确定。
那么对于世界坐标系,其原点
O
w
O_w
Ow为:
O
w
=
(
0
0
0
)
O_w = \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ \end{array} \right)
Ow=
000
其基为:
e
1
=
(
1
0
0
)
e
2
=
(
0
1
0
)
e
3
=
(
0
0
1
)
\boldsymbol{e_1} = \left( \begin{array}{c} 1 \\ 0 \\ 0 \\ \end{array} \right) \ \boldsymbol {e_2} = \left( \begin{array}{c} 0 \\ 1 \\ 0 \\ \end{array} \right) \ \boldsymbol {e_3} = \left( \begin{array}{c} 0 \\ 0 \\ 1 \\ \end{array} \right)
e1=
100
e2=
010
e3=
001
现在设另一个坐标系
O
c
O_c
Oc ,其原点坐标
O
c
O_c
Oc在世界坐标系内的坐标为
B
=
(
t
1
,
t
2
,
t
3
)
T
B = (t_1,t_2,t_3)^{T}
B=(t1,t2,t3)T,所以有:
O
c
=
O
w
+
(
e
1
e
2
e
3
)
(
t
1
t
2
t
3
)
O_c = O_w +\left( \begin{array}{c} \boldsymbol{e_{1}} \ \boldsymbol{e_{2}} \ \boldsymbol{e_{3}} \end{array} \right) \left( \begin{array}{c} t_1 \\ t_2 \\ t_3 \\ \end{array} \right)
Oc=Ow+(e1 e2 e3)
t1t2t3
其基
e
c
1
,
e
c
2
,
e
c
3
\boldsymbol{e_{c1}},\boldsymbol{e_{c2}},\boldsymbol{e_{c3}}
ec1,ec2,ec3与
O
w
O_w
Ow坐标系的基
e
1
,
e
2
,
e
3
\boldsymbol{e_1},\boldsymbol{e_2},\boldsymbol{e_3}
e1,e2,e3的变化矩阵为:
(
e
c
1
e
c
2
e
c
3
)
=
(
e
1
e
2
e
3
)
M
\left( \begin{array}{c} \boldsymbol{e_{c1}} \ \boldsymbol{e_{c2}} \ \boldsymbol{e_{c3}} \end{array} \right)= \left( \begin{array}{c} \boldsymbol{e_{1}} \ \boldsymbol{e_{2}} \ \boldsymbol{e_{3}} \end{array} \right) \boldsymbol{M}
(ec1 ec2 ec3)=(e1 e2 e3)M
设一点
P
P
P在
O
w
O_w
Ow的坐标为
(
X
w
,
Y
w
,
Z
w
)
T
(X_w,Y_w,Z_w)^T
(Xw,Yw,Zw)T:
P
=
O
w
+
(
e
c
1
e
c
2
e
c
2
)
(
X
w
Y
w
Z
w
)
P= O_w + \left( \begin{array}{c} \boldsymbol{e_{c1}} \ \boldsymbol{e_{c2}} \ \boldsymbol{e_{c2}} \end{array} \right) \left( \begin{array}{c} X_w \\ Y_w \\ Z_w \\ \end{array} \right)
P=Ow+(ec1 ec2 ec2)
XwYwZw
由(3)和(4)式得:
O
w
=
O
c
−
(
e
c
1
e
c
2
e
c
3
)
M
−
1
(
t
1
t
2
t
3
)
O_w = O_c - \left( \begin{array}{c} \boldsymbol{e_{c1}} \ \boldsymbol{e_{c2}} \ \boldsymbol{e_{c3}} \end{array} \right) \boldsymbol{M^{-1}} \left( \begin{array}{c} t_1 \\ t_2 \\ t_3 \\ \end{array} \right)
Ow=Oc−(ec1 ec2 ec3)M−1
t1t2t3
将(4)和(6)式代入(5)得:
P
=
O
w
+
(
e
c
1
e
c
2
e
c
3
)
M
−
1
(
X
w
Y
w
Z
w
)
=
O
c
+
(
e
c
1
e
c
2
e
c
3
)
M
−
1
(
(
X
w
Y
w
Z
w
)
−
(
t
1
t
2
t
3
)
)
\begin{align*} P &= O_w + \left( \begin{array}{c} \boldsymbol{e_{c1}} \ \boldsymbol{e_{c2}} \ \boldsymbol{e_{c3}} \end{array} \right) \boldsymbol{M^{-1}} \left( \begin{array}{c} X_w \\ Y_w \\ Z_w \\ \end{array} \right)\\ & = O_c + \left( \begin{array}{c} \boldsymbol{e_{c1}} \ \boldsymbol{e_{c2}} \ \boldsymbol{e_{c3}} \end{array} \right) \boldsymbol{M^{-1}}(\left( \begin{array}{c} X_w \\ Y_w \\ Z_w \\ \end{array} \right)-\left( \begin{array}{c} t_1 \\ t_2 \\ t_3 \\ \end{array} \right)) \end{align*}
P=Ow+(ec1 ec2 ec3)M−1
XwYwZw
=Oc+(ec1 ec2 ec3)M−1(
XwYwZw
−
t1t2t3
)
所以
P
P
P在
O
c
O_c
Oc系的坐标为:
M
−
1
(
(
X
w
Y
w
Z
w
)
−
(
t
1
t
2
t
3
)
)
\boldsymbol{M^{-1}} ( \left( \begin{array}{c} X_w \\ Y_w \\ Z_w \\ \end{array} \right)- \left( \begin{array}{c} t_1 \\ t_2 \\ t_3 \\ \end{array} \right) )
M−1(
XwYwZw
−
t1t2t3
)
可以将其写成齐次坐标形式:
(
X
c
Y
c
Z
c
1
)
=
(
M
−
1
−
M
−
1
B
0
1
)
(
X
w
Y
w
Z
w
1
)
\left( \begin{array}{c} X_c \\ Y_c \\ Z_c \\ 1 \\ \end{array} \right) =\left( \begin{array}{cccc} \boldsymbol{M^{-1}} & -\boldsymbol{M^{-1}B}\\ \boldsymbol{0} & \boldsymbol{1} \end{array} \right) \left( \begin{array}{c} X_w \\ Y_w \\ Z_w \\ 1 \\ \end{array} \right)
XcYcZc1
=(M−10−M−1B1)
XwYwZw1
现在来求矩阵
M
M
M。因为我们的相机一般只会绕
y
y
y轴旋转,所以我们设其绕
y
y
y轴旋转的角度为
a
a
a:
(
e
c
1
e
c
2
e
c
3
)
=
(
e
1
e
2
e
3
)
(
cos
a
0
sin
a
0
1
0
−
sin
a
0
cos
a
)
\left( \begin{array}{c} \boldsymbol{e_{c1}} \ \boldsymbol{e_{c2}} \ \boldsymbol{e_{c3}} \end{array} \right)= \left( \begin{array}{c} \boldsymbol{e_{1}} \ \boldsymbol{e_{2}} \ \boldsymbol{e_{3}} \end{array} \right) \left( \begin{array}{cccc} \cos a & 0 & \sin a \\ 0 & 1 & 0 \\ -\sin a & 0 & \cos a \\ \end{array} \right)
(ec1 ec2 ec3)=(e1 e2 e3)
cosa0−sina010sina0cosa
将该变换矩阵称为相机的外参矩阵:
T
=
(
M
−
1
−
M
−
1
B
0
1
)
=
(
cos
(
a
)
0
−
sin
(
a
)
t
3
sin
(
a
)
−
t
1
cos
(
a
)
0
1
0
−
t
2
sin
(
a
)
0
cos
(
a
)
t
1
(
−
sin
(
a
)
)
−
t
3
cos
(
a
)
0
0
0
1
)
T= \left( \begin{array}{cccc} \boldsymbol{M^{-1}} & -\boldsymbol{M^{-1}B}\\ \boldsymbol{0} & \boldsymbol{1} \end{array} \right)= \left( \begin{array}{cccc} \cos (a) & 0 & -\sin (a) & t_3 \sin (a)-t_1 \cos (a) \\ 0 & 1 & 0 & -t_2 \\ \sin (a) & 0 & \cos (a) & t_1 (-\sin (a))-t_3 \cos (a) \\ 0 & 0 & 0 & 1 \\ \end{array} \right)
T=(M−10−M−1B1)=
cos(a)0sin(a)00100−sin(a)0cos(a)0t3sin(a)−t1cos(a)−t2t1(−sin(a))−t3cos(a)1
二、相机坐标到图片坐标
设相机的正方向为X轴方向,其焦距为
f
f
f,
P
c
P_c
Pc点在相机成像平面的坐标为
P
n
(
X
n
,
Y
n
,
Z
n
)
P_n\ (X_n,Y_n,Z_n)
Pn (Xn,Yn,Zn),则有:
X
c
f
=
Z
c
Z
n
=
Y
c
Y
n
\frac{X_c}{f} = \frac{Z_c}{Z_n} = \frac{Y_c}{Y_n}
fXc=ZnZc=YnYc
于是有:
(
1
Y
n
Z
n
)
=
1
X
c
(
1
0
0
0
f
0
0
0
f
)
(
X
c
Y
c
Z
c
)
\left( \begin{array}{c} 1 \\ Y_n\\ Z_n\\ \end{array} \right)= \frac{1}{X_c} \left( \begin{array}{cccc} 1 & 0 & 0 \\ 0 & f & 0 \\ 0 & 0 & f \\ \end{array} \right) \left( \begin{array}{c} X_c \\ Y_c\\ Z_c\\ \end{array} \right)
1YnZn
=Xc1
1000f000f
XcYcZc
设相机的horizontal FOV 为
ϕ
\phi
ϕ,其水平分辨率为
w
w
w,纵向分辨率为
h
h
h,则相机成像平面的宽度
W
W
W和高度
H
H
H为:
W
=
2
f
tan
ϕ
2
H
=
2
f
h
w
tan
ϕ
2
W =2f \tan{\frac{\phi}{2}} \\ H =2\frac{fh}{w} \tan{\frac{\phi}{2}}
W=2ftan2ϕH=2wfhtan2ϕ
因此,相机成像平面的
Y
Y
Y轴坐标
Y
n
Y_n
Yn范围为
[
−
W
2
,
W
2
]
[-\frac{W}{2},\frac{W}{2}]
[−2W,2W],
Z
Z
Z轴坐标范围
Z
n
Z_n
Zn为
[
−
H
2
,
H
2
]
[-\frac{H}{2},\frac{H}{2}]
[−2H,2H]。
设点
P
n
P_n
Pn在像素坐标系中的坐标为
(
u
,
v
)
(u,v)
(u,v),则有:
u
=
w
(
−
Y
n
+
W
/
2
)
W
v
=
h
(
−
Z
n
+
H
/
2
)
H
u = \frac{w(-Y_n +W/2)}{W} \\ v = \frac{h(-Z_n +H/2)}{H} \\
u=Ww(−Yn+W/2)v=Hh(−Zn+H/2)
将其写成矩阵形式:
(
1
u
v
)
=
(
1
0
0
w
2
−
w
W
0
h
2
0
−
h
H
)
(
1
Y
n
Z
n
)
\left( \begin{array}{c} 1 \\ u \\ v \\ \end{array} \right)= \left( \begin{array}{cccc} 1 & 0 & 0 \\ \frac{w}{2} & -\frac{w}{W} & 0 \\ \frac{h}{2} & 0 & -\frac{h}{H} \\ \end{array} \right) \left( \begin{array}{c} 1 \\ Y_n \\ Z_n \\ \end{array} \right)
1uv
=
12w2h0−Ww000−Hh
1YnZn
所以,相机的内参矩阵
K
K
K为:
K
=
(
1
0
0
w
2
−
f
w
W
0
h
2
0
−
f
h
H
)
K= \left( \begin{array}{ccc} 1 & 0 & 0 \\ \frac{w}{2} & -\frac{f w}{W} & 0 \\ \frac{h}{2} & 0 & -\frac{f h}{H} \\ \end{array} \right)
K=
12w2h0−Wfw000−Hfh
因此,整个变换过程为:
(
1
u
v
)
=
1
X
c
K
T
′
(
X
w
Y
w
Z
w
1
)
\left( \begin{array}{c} 1 \\ u \\ v \\ \end{array} \right)= \frac{1}{X_c}KT' \left( \begin{array}{c} X_w \\ Y_w \\ Z_w \\ 1 \\ \end{array} \right)
1uv
=Xc1KT′
XwYwZw1
注意到这里有一个齐次坐标到非齐次坐标的变换,所以矩阵
T
′
T'
T′为:
T
′
=
(
M
−
1
−
M
−
1
B
)
T'= \left( \begin{array}{cccc} \boldsymbol{M^{-1}} & -\boldsymbol{M^{-1}B}\\ \end{array} \right)
T′=(M−1−M−1B)
它是一个三行四列的矩阵。
令
H
=
K
T
′
H=KT'
H=KT′,于是整个变换可以写为:
(
1
u
v
)
=
1
X
c
H
(
X
w
Y
w
Z
w
1
)
\left( \begin{array}{c} 1 \\ u \\ v \\ \end{array} \right)= \frac{1}{X_c}H \left( \begin{array}{c} X_w \\ Y_w \\ Z_w \\ 1 \\ \end{array} \right)
1uv
=Xc1H
XwYwZw1
可以知道H是一个三行四列矩阵,所以
H
H
H不可逆。因此,我们不能根据像素坐标反向求出世界坐标。这说明了深度信息在成像过程中丢失了。
现在还剩下一个问题,我们似乎还不知道
X
c
X_c
Xc,作如下变换:
(
X
c
X
c
u
X
c
v
)
=
H
(
X
w
Y
w
Z
w
1
)
\left( \begin{array}{c} X_c \\ X_cu \\ X_cv \\ \end{array} \right)= H \left( \begin{array}{c} X_w \\ Y_w \\ Z_w \\ 1 \\ \end{array} \right)
XcXcuXcv
=H
XwYwZw1
三、特殊情况的像素坐标到世界坐标变换
在特殊情况下, 可以求得
H
H
H的逆。比如对于汽车上的摄像头,我们要获取地面上的车道信息,可以认为
Z
w
Z_w
Zw恒等于0:
(
1
u
v
)
=
1
X
c
(
h
11
h
12
h
13
h
14
h
21
h
22
h
23
h
24
h
31
h
32
h
33
h
34
)
(
X
w
Y
w
0
1
)
\left( \begin{array}{c} 1 \\ u \\ v \\ \end{array} \right)= \frac{1}{X_c} \left( \begin{array}{cccc} h_{11} & h_{12} & h_{13} & h_{14} \\ h_{21} & h_{22} & h_{23} & h_{24} \\ h_{31} & h_{32} & h_{33} & h_{34} \\ \end{array} \right) \left( \begin{array}{c} X_w \\ Y_w \\ 0 \\ 1 \\ \end{array} \right)
1uv
=Xc1
h11h21h31h12h22h32h13h23h33h14h24h34
XwYw01
可以发现
H
H
H的第三列其实是没有意义的,于是可以将
H
H
H矩阵变为一个
3
×
3
3\times3
3×3的方阵,而
∣
H
∣
≠
0
|H|\neq0
∣H∣=0,可以求逆:
H
=
(
h
11
h
12
h
14
h
21
h
22
h
24
h
31
h
32
h
34
)
H= \left( \begin{array}{cccc} h_{11} & h_{12} & h_{14} \\ h_{21} & h_{22} & h_{24} \\ h_{31} & h_{32} & h_{34} \\ \end{array} \right)
H=
h11h21h31h12h22h32h14h24h34
所以该逆变换为:
(
X
w
X
c
Y
w
X
c
1
X
c
)
=
H
−
1
(
1
u
v
)
\left( \begin{array}{c} \frac{X_w}{X_c} \\ \frac{Y_w}{X_c} \\ \frac{1}{X_c} \\ \end{array} \right)= H^{-1} \left( \begin{array}{c} 1 \\ u \\ v \\ \end{array} \right)
XcXwXcYwXc1
=H−1
1uv