IPM逆透视变换问题（2）：Image –> Ground
1. 如果前置条件如下：
- 坐标系坐标系定义为：世界（右-前-上），相机（右-下-前），像素（右-下）；
-
旋转角度表示为：绕
zz
z
轴为
ya
w
yaw
y
a
w
偏航角，绕
yy
y
轴为
ro
l
l
roll
r
o
l
l
滚转角，绕
xx
x
轴负方向为
pi
t
c
h
pitch
p
i
t
c
h
俯仰角，正负按照右手定则； -
实际上在表达
ya
w
,
p
i
t
c
h
,
r
o
l
l
yaw,pitch,roll
y
a
w
,
p
i
t
c
h
,
r
o
l
l
姿态角时默认在（前-左-上）坐标系上进行，因此在表达这些角度时也可以增加一个（前-左-上）坐标系，从乘一次旋转矩阵。
2. 则点从像素坐标系到世界坐标系逆透视变换如下：
世界坐标系（右-前-上）沿着自身
Z
Z
Z
轴平移
h
h
h
，得到
T
w
t
T_{wt}
T
w
t
:
T
w
t
=
[
1
0
0
0
0
1
0
0
0
0
1
h
0
0
0
1
]
(
1
)
\begin{aligned} T_{wt} &= \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&h\\ 0&0&0&1\end{bmatrix} \qquad &(1) \\ \end{aligned}
T
w
t
=
⎣
⎢
⎢
⎡
1
0
0
0
0
1
0
0
0
0
1
0
0
0
h
1
⎦
⎥
⎥
⎤
(
1
)
相机的姿态角度（
滚转,俯仰,偏航
）
(
r
o
l
l
,
p
i
t
c
h
,
y
a
w
)
(roll,pitch,yaw)
(
r
o
l
l
,
p
i
t
c
h
,
y
a
w
)
对应在世界坐标系(右前上)是
(
α
=
−
p
i
t
c
h
,
β
=
r
o
l
l
,
γ
=
y
a
w
)
(\alpha = -pitch, \beta = roll, \gamma = yaw)
(
α
=
−
p
i
t
c
h
,
β
=
r
o
l
l
,
γ
=
y
a
w
)
，计算消失点时滚转角无影响即
r
o
l
l
=
0
roll=0
r
o
l
l
=
0
，则绕着固定坐标系
X
,
Y
,
Z
X,Y,Z
X
,
Y
,
Z
三个坐标轴旋转后得到的变换矩阵
T
w
r
T_{wr}
T
w
r
:
T
w
r
=
T
w
z
⋅
T
w
y
⋅
T
w
x
=
[
c
o
s
(
γ
)
−
s
i
n
(
γ
)
0
0
s
i
n
(
γ
)
c
o
s
(
γ
)
0
0
0
0
1
0
0
0
0
1
]
[
c
o
s
(
β
)
0
sin
(
β
)
0
0
1
0
0
−
sin
(
β
)
0
c
o
s
(
β
)
0
0
0
0
1
]
[
1
0
0
0
0
cos
(
α
)
−
sin
(
α
)
0
0
sin
(
α
)
cos
(
α
)
0
0
0
0
1
]
=
[
c
o
s
(
γ
)
−
s
i
n
(
γ
)
0
0
s
i
n
(
γ
)
c
o
s
(
γ
)
0
0
0
0
1
0
0
0
0
1
]
[
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
]
[
1
0
0
0
0
cos
(
α
)
−
sin
(
α
)
0
0
sin
(
α
)
cos
(
α
)
0
0
0
0
1
]
=
[
cos
(
γ
)
−
sin
(
γ
)
cos
(
α
)
sin
(
γ
)
sin
(
α
)
0
sin
(
γ
)
cos
(
γ
)
cos
(
α
)
−
cos
(
γ
)
sin
(
α
)
0
0
sin
(
α
)
cos
(
α
)
0
0
0
0
1
]
(
2
)
\begin{aligned} T_{wr} &= T_{wz} \cdot T_{wy} \cdot T_{wx} \\ &= \begin{bmatrix}cos\left(\gamma \right)&-sin\left(\gamma \right)&0&0\\ sin\left(\gamma \right)&cos\left(\gamma \right)&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}cos\left(\beta \right)&0&\sin \left(\beta \right)&0\\ 0&1&0&0\\ -\sin \left(\beta \right)&0&cos\left(\beta \right)&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&\cos \left(α\right)&-\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}cos\left(\gamma \right)&-sin\left(\gamma \right)&0&0\\ sin\left(\gamma \right)&cos\left(\gamma \right)&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&\cos \left(α\right)&-\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\cos \left(α\right)&\sin \left(γ\right)\sin \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\cos \left(α\right)&-\cos \left(γ\right)\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&0\\ 0&0&0&1\end{bmatrix} \qquad &(2) \\ \end{aligned}
T
w
r
=
T
w
z
⋅
T
w
y
⋅
T
w
x
=
⎣
⎢
⎢
⎡
c
o
s
(
γ
)
s
i
n
(
γ
)
0
0
−
s
i
n
(
γ
)
c
o
s
(
γ
)
0
0
0
0
1
0
0
0
0
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
c
o
s
(
β
)
0
−
sin
(
β
)
0
0
1
0
0
sin
(
β
)
0
c
o
s
(
β
)
0
0
0
0
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
1
0
0
0
0
cos
(
α
)
sin
(
α
)
0
0
−
sin
(
α
)
cos
(
α
)
0
0
0
0
1
⎦
⎥
⎥
⎤
=
⎣
⎢
⎢
⎡
c
o
s
(
γ
)
s
i
n
(
γ
)
0
0
−
s
i
n
(
γ
)
c
o
s
(
γ
)
0
0
0
0
1
0
0
0
0
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
1
0
0
0
0
cos
(
α
)
sin
(
α
)
0
0
−
sin
(
α
)
cos
(
α
)
0
0
0
0
1
⎦
⎥
⎥
⎤
=
⎣
⎢
⎢
⎡
cos
(
γ
)
sin
(
γ
)
0
0
−
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
cos
(
α
)
sin
(
α
)
0
sin
(
γ
)
sin
(
α
)
−
cos
(
γ
)
sin
(
α
)
cos
(
α
)
0
0
0
0
1
⎦
⎥
⎥
⎤
(
2
)
世界坐标系（右前上）
到
相机自身的(右前上)坐标系
到相机的变换
T
w
o
T_{wo}
T
w
o
T
w
o
=
T
w
t
⋅
T
w
r
=
[
1
0
0
0
0
1
0
0
0
0
1
h
0
0
0
1
]
[
cos
(
γ
)
−
sin
(
γ
)
cos
(
α
)
sin
(
γ
)
sin
(
α
)
0
sin
(
γ
)
cos
(
γ
)
cos
(
α
)
−
cos
(
γ
)
sin
(
α
)
0
0
sin
(
α
)
cos
(
α
)
0
0
0
0
1
]
=
[
cos
(
γ
)
−
sin
(
γ
)
cos
(
α
)
sin
(
γ
)
sin
(
α
)
0
sin
(
γ
)
cos
(
γ
)
cos
(
α
)
−
cos
(
γ
)
sin
(
α
)
0
0
sin
(
α
)
cos
(
α
)
h
0
0
0
1
]
(
3
)
\begin{aligned} T_{wo} &=T_{wt} \cdot T_{wr}\\ &= \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&h\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\cos \left(α\right)&\sin \left(γ\right)\sin \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\cos \left(α\right)&-\cos \left(γ\right)\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\cos \left(α\right)&\sin \left(γ\right)\sin \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\cos \left(α\right)&-\cos \left(γ\right)\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&h\\ 0&0&0&1\end{bmatrix} \qquad &(3) \end{aligned}
T
w
o
=
T
w
t
⋅
T
w
r
=
⎣
⎢
⎢
⎡
1
0
0
0
0
1
0
0
0
0
1
0
0
0
h
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
cos
(
γ
)
sin
(
γ
)
0
0
−
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
cos
(
α
)
sin
(
α
)
0
sin
(
γ
)
sin
(
α
)
−
cos
(
γ
)
sin
(
α
)
cos
(
α
)
0
0
0
0
1
⎦
⎥
⎥
⎤
=
⎣
⎢
⎢
⎡
cos
(
γ
)
sin
(
γ
)
0
0
−
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
cos
(
α
)
sin
(
α
)
0
sin
(
γ
)
sin
(
α
)
−
cos
(
γ
)
sin
(
α
)
cos
(
α
)
0
0
0
h
1
⎦
⎥
⎥
⎤
(
3
)
相机自身的(右前上)坐标系
到
相机坐标系
的变换
T
c
o
T_{co}
T
c
o
T
o
c
=
[
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
]
[
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
]
[
1
0
0
0
0
0
1
0
0
−
1
1
0
0
0
0
1
]
=
[
1
0
0
0
0
0
1
0
0
−
1
1
0
0
0
0
1
]
(
4
)
\begin{aligned} T_{oc} &= \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&0&1&0\\ 0&-1&1&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}1&0&0&0\\ 0&0&1&0\\ 0&-1&1&0\\ 0&0&0&1\end{bmatrix} \qquad &(4) \end{aligned}
T
o
c
=
⎣
⎢
⎢
⎡
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
1
0
0
0
0
0
−
1
0
0
1
1
0
0
0
0
1
⎦
⎥
⎥
⎤
=
⎣
⎢
⎢
⎡
1
0
0
0
0
0
−
1
0
0
1
1
0
0
0
0
1
⎦
⎥
⎥
⎤
(
4
)
世界坐标系
到
相机坐标系
的变换为：
T
w
c
=
T
w
o
⋅
T
o
c
=
[
cos
(
γ
)
−
sin
(
γ
)
cos
(
α
)
sin
(
γ
)
sin
(
α
)
0
sin
(
γ
)
cos
(
γ
)
cos
(
α
)
−
cos
(
γ
)
sin
(
α
)
0
0
sin
(
α
)
cos
(
α
)
h
0
0
0
1
]
[
1
0
0
0
0
0
1
0
0
−
1
1
0
0
0
0
1
]
=
[
cos
(
γ
)
−
sin
(
γ
)
sin
(
α
)
−
sin
(
γ
)
cos
(
α
)
0
sin
(
γ
)
cos
(
γ
)
sin
(
α
)
cos
(
γ
)
cos
(
α
)
0
0
−
cos
(
α
)
sin
(
α
)
h
0
0
0
1
]
(
5
)
\begin{aligned} T_{wc} &= T_{wo} \cdot T_{oc} \\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\cos \left(α\right)&\sin \left(γ\right)\sin \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\cos \left(α\right)&-\cos \left(γ\right)\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&h\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&0&1&0\\ 0&-1&1&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\sin \left(α\right)&-\sin \left(γ\right)\cos \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\sin \left(α\right)&\cos \left(γ\right)\cos \left(α\right)&0\\ 0&-\cos \left(α\right)&\sin \left(α\right)&h\\ 0&0&0&1\end{bmatrix} \qquad &(5) \end{aligned}
T
w
c
=
T
w
o
⋅
T
o
c
=
⎣
⎢
⎢
⎡
cos
(
γ
)
sin
(
γ
)
0
0
−
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
cos
(
α
)
sin
(
α
)
0
sin
(
γ
)
sin
(
α
)
−
cos
(
γ
)
sin
(
α
)
cos
(
α
)
0
0
0
h
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
1
0
0
0
0
0
−
1
0
0
1
1
0
0
0
0
1
⎦
⎥
⎥
⎤
=
⎣
⎢
⎢
⎡
cos
(
γ
)
sin
(
γ
)
0
0
−
sin
(
γ
)
sin
(
α
)
cos
(
γ
)
sin
(
α
)
−
cos
(
α
)
0
−
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
cos
(
α
)
sin
(
α
)
0
0
0
h
1
⎦
⎥
⎥
⎤
(
5
)
相机坐标系
到
图像坐标系
的变换（对齐过程）为：
T
c
i
=
[
1
0
0
0
1
0
0
0
1
0
0
1
z
c
]
(
6
)
\begin{aligned} T_{ci} &= \begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&1\\ 0&0&\frac{1}{z_c}\end{bmatrix} \qquad &(6) \end{aligned}
T
c
i
=
⎣
⎢
⎢
⎡
1
0
0
0
0
1
0
0
0
0
1
z
c
1
⎦
⎥
⎥
⎤
(
6
)
图像坐标系
到
像素坐标系
的变换为：
T
i
p
=
[
1
f
x
0
−
c
x
f
x
0
1
f
y
−
c
y
f
y
0
0
1
]
(
7
)
\begin{aligned} T_{ip} &= \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} \qquad &(7) \end{aligned}
T
i
p
=
⎣
⎡
f
x
1
0
0
0
f
y
1
0
−
f
x
c
x
−
f
y
c
y
1
⎦
⎤
(
7
)
则
世界坐标系
到
图像像素坐标系
的变换为：
T
w
p
=
T
w
c
⋅
T
c
i
⋅
T
i
p
=
[
cos
(
γ
)
−
sin
(
γ
)
sin
(
α
)
−
sin
(
γ
)
cos
(
α
)
0
sin
(
γ
)
cos
(
γ
)
sin
(
α
)
cos
(
γ
)
cos
(
α
)
0
0
−
cos
(
α
)
sin
(
α
)
h
0
0
0
1
]
[
1
0
0
0
1
0
0
0
1
0
0
1
z
c
]
[
1
f
x
0
−
c
x
f
x
0
1
f
y
−
c
y
f
y
0
0
1
]
=
[
cos
(
γ
)
f
x
−
sin
(
γ
)
sin
(
α
)
f
y
−
cos
(
γ
)
c
x
f
y
+
sin
(
γ
)
sin
(
α
)
f
x
c
y
−
sin
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
sin
(
γ
)
f
x
cos
(
γ
)
sin
(
α
)
f
y
−
sin
(
γ
)
c
x
f
y
−
cos
(
γ
)
sin
(
α
)
f
x
c
y
+
cos
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
0
−
cos
(
α
)
f
y
cos
(
α
)
c
y
z
c
+
h
f
y
+
sin
(
α
)
f
y
z
c
z
c
f
y
0
0
1
z
c
]
(
8
)
\begin{aligned} T_{wp} &= T_{wc} \cdot T_{ci} \cdot T_{ip}\\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\sin \left(α\right)&-\sin \left(γ\right)\cos \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\sin \left(α\right)&\cos \left(γ\right)\cos \left(α\right)&0\\ 0&-\cos \left(α\right)&\sin \left(α\right)&h\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&1\\ 0&0&\frac{1}{z_c}\end{bmatrix} \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}\\ &= \begin{bmatrix}\frac{\cos \left(γ\right)}{f_x}&-\frac{\sin \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\cos \left(γ\right)c_xf_y+\sin \left(γ\right)\sin \left(α\right)f_xc_y-\sin \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\sin \left(γ\right)}{f_x}&\frac{\cos \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\sin \left(γ\right)c_xf_y-\cos \left(γ\right)\sin \left(α\right)f_xc_y+\cos \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ 0&-\frac{\cos \left(α\right)}{f_y}&\frac{\cos \left(α\right)c_yz_c+hf_y+\sin \left(α\right)f_yz_c}{z_cf_y}\\ 0&0&\frac{1}{z_c}\end{bmatrix} \qquad &(8) \end{aligned}
T
w
p
=
T
w
c
⋅
T
c
i
⋅
T
i
p
=
⎣
⎢
⎢
⎡
cos
(
γ
)
sin
(
γ
)
0
0
−
sin
(
γ
)
sin
(
α
)
cos
(
γ
)
sin
(
α
)
−
cos
(
α
)
0
−
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
cos
(
α
)
sin
(
α
)
0
0
0
h
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
1
0
0
0
0
1
0
0
0
0
1
z
c
1
⎦
⎥
⎥
⎤
⎣
⎡
f
x
1
0
0
0
f
y
1
0
−
f
x
c
x
−
f
y
c
y
1
⎦
⎤
=
⎣
⎢
⎢
⎢
⎢
⎡
f
x
cos
(
γ
)
f
x
sin
(
γ
)
0
0
−
f
y
sin
(
γ
)
sin
(
α
)
f
y
cos
(
γ
)
sin
(
α
)
−
f
y
cos
(
α
)
0
f
x
f
y
−
cos
(
γ
)
c
x
f
y
+
sin
(
γ
)
sin
(
α
)
f
x
c
y
−
sin
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
−
sin
(
γ
)
c
x
f
y
−
cos
(
γ
)
sin
(
α
)
f
x
c
y
+
cos
(
γ
)
cos
(
α
)
f
x
f
y
z
c
f
y
cos
(
α
)
c
y
z
c
+
h
f
y
+
sin
(
α
)
f
y
z
c
z
c
1
⎦
⎥
⎥
⎥
⎥
⎤
(
8
)
则
图像像素坐标系的点
变换到
世界坐标系的点
的过程为：
1
z
c
[
x
w
y
w
z
w
1
]
=
T
w
p
[
u
v
1
]
[
x
w
z
c
y
w
z
c
z
w
z
c
1
z
c
]
=
[
cos
(
γ
)
f
x
−
sin
(
γ
)
sin
(
α
)
f
y
−
cos
(
γ
)
c
x
f
y
+
sin
(
γ
)
sin
(
α
)
f
x
c
y
−
sin
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
sin
(
γ
)
f
x
cos
(
γ
)
sin
(
α
)
f
y
−
sin
(
γ
)
c
x
f
y
−
cos
(
γ
)
sin
(
α
)
f
x
c
y
+
cos
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
0
−
cos
(
α
)
f
y
cos
(
α
)
c
y
z
c
+
h
f
y
+
sin
(
α
)
f
y
z
c
z
c
f
y
0
0
1
z
c
]
[
u
v
1
]
=
[
cos
(
γ
)
u
f
y
−
sin
(
γ
)
sin
(
α
)
f
x
v
−
cos
(
γ
)
c
x
f
y
+
sin
(
γ
)
sin
(
α
)
f
x
c
y
−
sin
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
sin
(
γ
)
u
f
y
+
cos
(
γ
)
sin
(
α
)
f
x
v
−
sin
(
γ
)
c
x
f
y
−
cos
(
γ
)
sin
(
α
)
f
x
c
y
+
cos
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
cos
(
α
)
c
y
z
c
+
sin
(
α
)
z
c
f
y
−
cos
(
α
)
z
c
v
+
h
f
y
z
c
f
y
1
z
c
]
(
9
)
\begin{aligned} \frac{1}{z_c}\begin{bmatrix}x_w\\ y_w\\ z_w\\ 1\end{bmatrix} &= T_{wp} \begin{bmatrix}u\\ v\\ 1\end{bmatrix} \\ \begin{bmatrix}\frac{x_w}{z_c}\\ \frac{y_w}{z_c}\\ \frac{z_w}{z_c}\\ \frac{1}{z_c}\end{bmatrix} &= \begin{bmatrix}\frac{\cos \left(γ\right)}{f_x}&-\frac{\sin \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\cos \left(γ\right)c_xf_y+\sin \left(γ\right)\sin \left(α\right)f_xc_y-\sin \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\sin \left(γ\right)}{f_x}&\frac{\cos \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\sin \left(γ\right)c_xf_y-\cos \left(γ\right)\sin \left(α\right)f_xc_y+\cos \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ 0&-\frac{\cos \left(α\right)}{f_y}&\frac{\cos \left(α\right)c_yz_c+hf_y+\sin \left(α\right)f_yz_c}{z_cf_y}\\ 0&0&\frac{1}{z_c}\end{bmatrix} \begin{bmatrix}u\\ v\\ 1\end{bmatrix} \\ &= \begin{bmatrix}\frac{\cos \left(γ\right)uf_y-\sin \left(γ\right)\sin \left(α\right)f_xv-\cos \left(γ\right)c_xf_y+\sin \left(γ\right)\sin \left(α\right)f_xc_y-\sin \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\sin \left(γ\right)uf_y+\cos \left(γ\right)\sin \left(α\right)f_xv-\sin \left(γ\right)c_xf_y-\cos \left(γ\right)\sin \left(α\right)f_xc_y+\cos \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\cos \left(α\right)c_yz_c+\sin \left(α\right)z_cf_y-\cos \left(α\right)z_cv+hf_y}{z_cf_y}\\ \frac{1}{z_c}\end{bmatrix} \qquad &(9) \end{aligned}
z
c
1
⎣
⎢
⎢
⎡
x
w
y
w
z
w
1
⎦
⎥
⎥
⎤
⎣
⎢
⎢
⎡
z
c
x
w
z
c
y
w
z
c
z
w
z
c
1
⎦
⎥
⎥
⎤
=
T
w
p
⎣
⎡
u
v
1
⎦
⎤
=
⎣
⎢
⎢
⎢
⎢
⎡
f
x
cos
(
γ
)
f
x
sin
(
γ
)
0
0
−
f
y
sin
(
γ
)
sin
(
α
)
f
y
cos
(
γ
)
sin
(
α
)
−
f
y
cos
(
α
)
0
f
x
f
y
−
cos
(
γ
)
c
x
f
y
+
sin
(
γ
)
sin
(
α
)
f
x
c
y
−
sin
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
−
sin
(
γ
)
c
x
f
y
−
cos
(
γ
)
sin
(
α
)
f
x
c
y
+
cos
(
γ
)
cos
(
α
)
f
x
f
y
z
c
f
y
cos
(
α
)
c
y
z
c
+
h
f
y
+
sin
(
α
)
f
y
z
c
z
c
1
⎦
⎥
⎥
⎥
⎥
⎤
⎣
⎡
u
v
1
⎦
⎤
=
⎣
⎢
⎢
⎢
⎢
⎡
f
x
f
y
cos
(
γ
)
u
f
y
−
sin
(
γ
)
sin
(
α
)
f
x
v
−
cos
(
γ
)
c
x
f
y
+
sin
(
γ
)
sin
(
α
)
f
x
c
y
−
sin
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
sin
(
γ
)
u
f
y
+
cos
(
γ
)
sin
(
α
)
f
x
v
−
sin
(
γ
)
c
x
f
y
−
cos
(
γ
)
sin
(
α
)
f
x
c
y
+
cos
(
γ
)
cos
(
α
)
f
x
f
y
z
c
f
y
cos
(
α
)
c
y
z
c
+
sin
(
α
)
z
c
f
y
−
cos
(
α
)
z
c
v
+
h
f
y
z
c
1
⎦
⎥
⎥
⎥
⎥
⎤
(
9
)
由于消失点是处于地面上的点，其Z坐标为0，即
z
w
=
=
0
z_w == 0
z
w
=
=
0
，所以有：
cos
(
α
)
c
y
z
c
+
sin
(
α
)
z
c
f
y
−
cos
(
α
)
z
c
v
+
h
f
y
z
c
f
y
=
0
(
10
)
\frac{\cos \left(α\right)c_yz_c+\sin \left(α\right)z_cf_y-\cos \left(α\right)z_cv+hf_y}{z_cf_y}=0 \qquad (10)
z
c
f
y
cos
(
α
)
c
y
z
c
+
sin
(
α
)
z
c
f
y
−
cos
(
α
)
z
c
v
+
h
f
y
=
0
(
1
0
)
则可以得到：
z
c
=
h
f
y
cos
(
α
)
v
−
cos
(
α
)
c
y
−
sin
(
α
)
f
y
(
11
)
1
z
c
=
cos
(
α
)
v
−
cos
(
α
)
c
y
−
sin
(
α
)
f
y
h
f
y
=
v
cos
(
α
)
h
f
y
+
−
cos
(
α
)
c
y
−
sin
(
α
)
f
y
h
f
y
(
12
)
\begin{aligned} z_c&=\frac{hf_y}{\cos \left(α\right)v – \cos \left(α\right)c_y – \sin \left(α\right)f_y} \qquad &(11) \\ \frac{1}{z_c}&= \frac{\cos \left(α\right)v – \cos \left(α\right)c_y – \sin \left(α\right)f_y}{hf_y}\\ &=v\frac{\cos \left(α\right)}{hf_y}+ \frac{-\cos \left(α\right)c_y -\sin \left(α\right)f_y}{hf_y} \qquad &(12) \end{aligned}
z
c
z
c
1
=
cos
(
α
)
v
−
cos
(
α
)
c
y
−
sin
(
α
)
f
y
h
f
y
=
h
f
y
cos
(
α
)
v
−
cos
(
α
)
c
y
−
sin
(
α
)
f
y
=
v
h
f
y
cos
(
α
)
+
h
f
y
−
cos
(
α
)
c
y
−
sin
(
α
)
f
y
(
1
1
)
(
1
2
)
将
z
c
z_c
z
c
带入上式即有：
T
w
p
=
[
cos
(
γ
)
f
x
−
sin
(
γ
)
sin
(
α
)
f
y
−
cos
(
γ
)
c
x
f
y
+
sin
(
γ
)
sin
(
α
)
f
x
c
y
−
sin
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
sin
(
γ
)
f
x
cos
(
γ
)
sin
(
α
)
f
y
−
sin
(
γ
)
c
x
f
y
−
cos
(
γ
)
sin
(
α
)
f
x
c
y
+
cos
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
0
−
cos
(
α
)
f
y
v
cos
(
α
)
f
y
0
0
v
cos
(
α
)
h
f
y
+
−
cos
(
α
)
c
y
−
sin
(
α
)
f
y
h
f
y
]
(
13
)
\begin{aligned} T_{wp}&= \begin{bmatrix}\frac{\cos \left(γ\right)}{f_x}&-\frac{\sin \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\cos \left(γ\right)c_xf_y+\sin \left(γ\right)\sin \left(α\right)f_xc_y-\sin \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\sin \left(γ\right)}{f_x}&\frac{\cos \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\sin \left(γ\right)c_xf_y-\cos \left(γ\right)\sin \left(α\right)f_xc_y+\cos \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ 0&-\frac{\cos \left(α\right)}{f_y}&v\frac{\cos \left(α\right)}{f_y}\\ 0&0&v\frac{\cos \left(α\right)}{hf_y}+ \frac{-\cos \left(α\right)c_y -\sin \left(α\right)f_y}{hf_y}\end{bmatrix} \qquad &(13) \end{aligned}
T
w
p
=
⎣
⎢
⎢
⎢
⎢
⎡
f
x
cos
(
γ
)
f
x
sin
(
γ
)
0
0
−
f
y
sin
(
γ
)
sin
(
α
)
f
y
cos
(
γ
)
sin
(
α
)
−
f
y
cos
(
α
)
0
f
x
f
y
−
cos
(
γ
)
c
x
f
y
+
sin
(
γ
)
sin
(
α
)
f
x
c
y
−
sin
(
γ
)
cos
(
α
)
f
x
f
y
f
x
f
y
−
sin
(
γ
)
c
x
f
y
−
cos
(
γ
)
sin
(
α
)
f
x
c
y
+
cos
(
γ
)
cos
(
α
)
f
x
f
y
v
f
y
cos
(
α
)
v
h
f
y
cos
(
α
)
+
h
f
y
−
cos
(
α
)
c
y
−
sin
(
α
)
f
y
⎦
⎥
⎥
⎥
⎥
⎤
(
1
3
)
具体计算时要分离（u, v）的 影响，
T
w
p
T_{wp}
T
w
p
第三行中的
v
v
v
难以分离，但是第三行用于计算
z
w
z_w
z
w
，且地面上的
z
w
z_w
z
w
恒为零，所以可以全部设为0；
T
w
p
T_{wp}
T
w
p
的第四行则比较容易分离。最后得到计算IPM时从像素到世界的变换
T
4
×
3
T_{4\times 3}
T
4
×
3
：
T
4
×
3
=
[
cos
(
γ
)
f
x
−
sin
(
γ
)
sin
(
α
)
f
y
−
c
x
f
y
cos
(
γ
)
+
c
y
f
x
sin
(
γ
)
sin
(
α
)
−
f
x
f
y
sin
(
γ
)
cos
(
α
)
f
x
f
y
sin
(
γ
)
f
x
cos
(
γ
)
sin
(
α
)
f
y
−
c
x
f
y
sin
(
γ
)
−
c
y
f
x
cos
(
γ
)
sin
(
α
)
+
f
x
f
y
cos
(
γ
)
cos
(
α
)
f
x
f
y
0
0
0
0
cos
(
α
)
h
f
y
−
c
y
cos
(
α
)
−
f
y
sin
(
α
)
h
f
y
]
=
[
cos
(
γ
)
f
x
−
sin
(
γ
)
sin
(
α
)
f
y
−
c
x
cos
(
γ
)
f
x
+
c
y
sin
(
γ
)
sin
(
α
)
f
y
−
sin
(
γ
)
cos
(
α
)
sin
(
γ
)
f
x
cos
(
γ
)
sin
(
α
)
f
y
−
c
x
sin
(
γ
)
f
x
−
c
y
cos
(
γ
)
sin
(
α
)
f
y
+
cos
(
γ
)
cos
(
α
)
0
0
0
0
cos
(
α
)
h
f
y
−
c
y
cos
(
α
)
h
f
y
−
sin
(
α
)
h
]
(
14
)
\begin{aligned}T_{4\times 3} &= \begin{bmatrix} \frac{\cos \left(\gamma \right)}{f_x} &\frac{-\sin \left(\gamma \right)\sin \left(\alpha \right)}{f_y} &\frac{-c_x f_y \cos \left(\gamma \right)+c_y f_x\sin \left(\gamma \right)\sin \left(\alpha \right)-f_x f_y \sin \left(\gamma \right)\cos \left(\alpha \right)}{f_x f_y}\\ \frac{\sin \left(\gamma \right)}{f_x} &\frac{\cos \left(\gamma \right)\sin \left(\alpha \right)}{f_y} &\frac{-c_x f_y\sin \left(\gamma \right)-c_y f_x\cos \left(\gamma \right)\sin \left(\alpha \right)+f_x f_y\cos \left(\gamma \right)\cos \left(\alpha \right)}{f_x f_y}\\ 0&0&0\\ 0&\frac{\cos \left(\alpha \right)}{h f_y} &\frac{-c_y\cos \left(\alpha \right)-f_y \sin \left(\alpha \right)}{h f_y} \end{bmatrix} \\ &= \begin{bmatrix} \frac{\cos(\gamma)}{f_x} &-\frac{\sin(\gamma) \sin(\alpha)}{f_y} &-\frac{c_x \cos(\gamma)}{f_x} + \frac{c_y \sin(\gamma) \sin(\alpha)}{f_y} – \sin(\gamma) \cos(\alpha)\\ \frac{\sin(\gamma)}{f_x} &\frac{\cos(\gamma) \sin(\alpha)}{f_y} &-\frac{c_x \sin(\gamma)}{f_x} – \frac{c_y \cos(\gamma) \sin(\alpha)}{f_y} + \cos(\gamma) \cos(\alpha)\\ 0&0&0\\ 0 &\frac{\cos(\alpha)}{h f_y} &-\frac{c_y \cos(\alpha)}{h f_y} – \frac{\sin(\alpha)}{h} \end{bmatrix} \qquad &(14) \end{aligned}
T
4
×
3
=
⎣
⎢
⎢
⎢
⎢
⎡
f
x
cos
(
γ
)
f
x
sin
(
γ
)
0
0
f
y
−
sin
(
γ
)
sin
(
α
)
f
y
cos
(
γ
)
sin
(
α
)
0
h
f
y
cos
(
α
)
f
x
f
y
−
c
x
f
y
cos
(
γ
)
+
c
y
f
x
sin
(
γ
)
sin
(
α
)
−
f
x
f
y
sin
(
γ
)
cos
(
α
)
f
x
f
y
−
c
x
f
y
sin
(
γ
)
−
c
y
f
x
cos
(
γ
)
sin
(
α
)
+
f
x
f
y
cos
(
γ
)
cos
(
α
)
0
h
f
y
−
c
y
cos
(
α
)
−
f
y
sin
(
α
)
⎦
⎥
⎥
⎥
⎥
⎤
=
⎣
⎢
⎢
⎢
⎢
⎡
f
x
cos
(
γ
)
f
x
sin
(
γ
)
0
0
−
f
y
sin
(
γ
)
sin
(
α
)
f
y
cos
(
γ
)
sin
(
α
)
0
h
f
y
cos
(
α
)
−
f
x
c
x
cos
(
γ
)
+
f
y
c
y
sin
(
γ
)
sin
(
α
)
−
sin
(
γ
)
cos
(
α
)
−
f
x
c
x
sin
(
γ
)
−
f
y
c
y
cos
(
γ
)
sin
(
α
)
+
cos
(
γ
)
cos
(
α
)
0
−
h
f
y
c
y
cos
(
α
)
−
h
sin
(
α
)
⎦
⎥
⎥
⎥
⎥
⎤
(
1
4
)
如果式(14)中的角度
α
,
γ
\alpha,\gamma
α
,
γ
取负值，同时矩阵乘
−
1
-1
−
1
（不影响结果）。
则所有的
s
i
n
sin
s
i
n
值取反，同时矩阵的第三行与式(14)中的第四行乘
h
h
h
（设定成什么都可以，不影响结果），此时即得到：
T
G
I
=
[
cos
(
−
γ
)
f
x
−
sin
(
−
γ
)
sin
(
−
α
)
f
y
−
c
x
f
y
cos
(
−
γ
)
+
c
y
f
x
sin
(
−
γ
)
sin
(
−
α
)
−
f
x
f
y
sin
(
−
γ
)
cos
(
−
α
)
f
x
f
y
sin
(
−
γ
)
f
x
cos
(
−
γ
)
sin
(
−
α
)
f
y
−
c
x
f
y
sin
(
−
γ
)
−
c
y
f
x
cos
(
−
γ
)
sin
(
−
α
)
+
f
x
f
y
cos
(
−
γ
)
cos
(
−
α
)
f
x
f
y
0
−
cos
(
−
α
)
f
y
−
−
c
y
cos
(
−
α
)
−
f
y
sin
(
−
α
)
f
y
0
cos
(
−
α
)
h
f
y
−
c
y
cos
(
−
α
)
−
f
y
sin
(
−
α
)
h
f
y
]
⋅
−
1
=
[
−
cos
(
γ
)
f
x
sin
(
γ
)
sin
(
α
)
f
y
c
x
cos
(
γ
)
f
x
−
c
y
sin
(
γ
)
sin
(
α
)
f
y
−
sin
(
γ
)
cos
(
α
)
sin
(
γ
)
f
x
cos
(
γ
)
sin
(
α
)
f
y
−
c
x
sin
(
γ
)
f
x
−
c
y
cos
(
γ
)
sin
(
α
)
f
y
−
cos
(
γ
)
cos
(
α
)
0
cos
(
α
)
f
y
−
c
y
cos
(
α
)
f
y
+
sin
(
α
)
0
−
cos
(
α
)
h
f
y
c
y
cos
(
α
)
h
f
y
−
sin
(
α
)
h
]
=
[
−
h
cos
(
γ
)
f
x
h
sin
(
γ
)
sin
(
α
)
f
y
h
c
x
cos
(
γ
)
f
x
−
h
c
y
sin
(
γ
)
sin
(
α
)
f
y
−
h
sin
(
γ
)
cos
(
α
)
h
sin
(
γ
)
f
x
h
cos
(
γ
)
sin
(
α
)
f
y
−
h
c
x
sin
(
γ
)
f
x
−
h
c
y
cos
(
γ
)
sin
(
α
)
f
y
−
h
cos
(
γ
)
cos
(
α
)
0
h
cos
(
α
)
f
y
−
h
c
y
cos
(
α
)
f
y
+
h
sin
(
α
)
0
−
cos
(
α
)
f
y
c
y
cos
(
α
)
f
y
−
sin
(
α
)
]
1
h
(
15
)
{\color{darkorange} \begin{aligned} T_{GI} &=\begin{bmatrix} \frac{\cos(-\gamma)}{f_x} &\frac{-\sin(-\gamma) \sin(-\alpha)}{f_y} &\frac{-c_x f_y \cos(-\gamma) + c_y f_x \sin(-\gamma) \sin(-\alpha) – f_x f_y \sin(-\gamma) \cos(-\alpha)}{f_x f_y}\\ \frac{\sin(-\gamma)}{f_x} &\frac{\cos(-\gamma) \sin(-\alpha)}{f_y} &\frac{-c_x f_y \sin(-\gamma) – c_y f_x \cos(-\gamma) \sin(-\alpha) + f_x f_y \cos(-\gamma) \cos(-\alpha)}{f_x f_y}\\ 0 &-\frac{\cos(-\alpha)}{f_y}&-\frac{- c_y \cos(-\alpha) – f_y \sin(-\alpha)}{f_y}\\ 0 &\frac{\cos(-\alpha)}{h f_y} &\frac{-c_y \cos(-\alpha) – f_y \sin(-\alpha)}{h f_y}\end{bmatrix} \cdot -1\\ &=\begin{bmatrix} -\frac{\cos(\gamma)}{f_x} &\frac{\sin(\gamma) \sin(\alpha)}{f_y} &\frac{c_x \cos(\gamma)}{f_x} – \frac{c_y \sin(\gamma) \sin(\alpha)}{f_y} – \sin(\gamma) \cos(\alpha)\\ \frac{\sin(\gamma)}{f_x} &\frac{\cos(\gamma) \sin(\alpha)}{f_y} &-\frac{c_x \sin(\gamma)}{f_x} – \frac{c_y \cos(\gamma) \sin(\alpha)}{f_y} – \cos(\gamma) \cos(\alpha)\\ 0 &\frac{\cos(\alpha)}{f_y} &- \frac{c_y \cos(\alpha)}{f_y} + \sin(\alpha) \\ 0 &-\frac{\cos(\alpha)}{h f_y} &\frac{c_y \cos(\alpha)}{h f_y} – \frac{\sin(\alpha)}{h} \end{bmatrix} \\ &=\begin{bmatrix} -h\frac{\cos(\gamma)}{f_x} &h\frac{\sin(\gamma) \sin(\alpha)}{f_y} &h\frac{c_x \cos(\gamma)}{f_x} – h\frac{c_y \sin(\gamma) \sin(\alpha)}{f_y} – h\sin(\gamma) \cos(\alpha)\\ h\frac{\sin(\gamma)}{f_x} &h\frac{\cos(\gamma) \sin(\alpha)}{f_y} &-h\frac{c_x \sin(\gamma)}{f_x} – h\frac{c_y \cos(\gamma) \sin(\alpha)}{f_y} – h\cos(\gamma) \cos(\alpha)\\ 0 &h\frac{\cos(\alpha)}{f_y} &-h \frac{c_y \cos(\alpha)}{f_y} + h\sin(\alpha) \\ 0 &-\frac{\cos(\alpha)}{f_y} &\frac{c_y \cos(\alpha)}{f_y} – \sin(\alpha) \end{bmatrix} \frac{1}{h} \qquad&(15) \end{aligned} }
T
G
I
=
⎣
⎢
⎢
⎢
⎢
⎡
f
x
cos
(
−
γ
)
f
x
sin
(
−
γ
)
0
0
f
y
−
sin
(
−
γ
)
sin
(
−
α
)
f
y
cos
(
−
γ
)
sin
(
−
α
)
−
f
y
cos
(
−
α
)
h
f
y
cos
(
−
α
)
f
x
f
y
−
c
x
f
y
cos
(
−
γ
)
+
c
y
f
x
sin
(
−
γ
)
sin
(
−
α
)
−
f
x
f
y
sin
(
−
γ
)
cos
(
−
α
)
f
x
f
y
−
c
x
f
y
sin
(
−
γ
)
−
c
y
f
x
cos
(
−
γ
)
sin
(
−
α
)
+
f
x
f
y
cos
(
−
γ
)
cos
(
−
α
)
−
f
y
−
c
y
cos
(
−
α
)
−
f
y
sin
(
−
α
)
h
f
y
−
c
y
cos
(
−
α
)
−
f
y
sin
(
−
α
)
⎦
⎥
⎥
⎥
⎥
⎤
⋅
−
1
=
⎣
⎢
⎢
⎢
⎢
⎡
−
f
x
cos
(
γ
)
f
x
sin
(
γ
)
0
0
f
y
sin
(
γ
)
sin
(
α
)
f
y
cos
(
γ
)
sin
(
α
)
f
y
cos
(
α
)
−
h
f
y
cos
(
α
)
f
x
c
x
cos
(
γ
)
−
f
y
c
y
sin
(
γ
)
sin
(
α
)
−
sin
(
γ
)
cos
(
α
)
−
f
x
c
x
sin
(
γ
)
−
f
y
c
y
cos
(
γ
)
sin
(
α
)
−
cos
(
γ
)
cos
(
α
)
−
f
y
c
y
cos
(
α
)
+
sin
(
α
)
h
f
y
c
y
cos
(
α
)
−
h
sin
(
α
)
⎦
⎥
⎥
⎥
⎥
⎤
=
⎣
⎢
⎢
⎢
⎢
⎡
−
h
f
x
cos
(
γ
)
h
f
x
sin
(
γ
)
0
0
h
f
y
sin
(
γ
)
sin
(
α
)
h
f
y
cos
(
γ
)
sin
(
α
)
h
f
y
cos
(
α
)
−
f
y
cos
(
α
)
h
f
x
c
x
cos
(
γ
)
−
h
f
y
c
y
sin
(
γ
)
sin
(
α
)
−
h
sin
(
γ
)
cos
(
α
)
−
h
f
x
c
x
sin
(
γ
)
−
h
f
y
c
y
cos
(
γ
)
sin
(
α
)
−
h
cos
(
γ
)
cos
(
α
)
−
h
f
y
c
y
cos
(
α
)
+
h
sin
(
α
)
f
y
c
y
cos
(
α
)
−
sin
(
α
)
⎦
⎥
⎥
⎥
⎥
⎤
h
1
(
1
5
)
之所进行这样的对比，主要是
网络上大部分关于IPM的代码都是使用式(15)进行代码编写
，而且在没有多少优化的情况下，导致代码阅读性较差，极不友好。