IPM逆透视变换问题（3）：Ground --> Image
1. 同样如果前置条件如下：
- 坐标系坐标系定义为：世界（右-前-上），相机（右-下-前），像素（右-下）；
- 旋转角度表示为：绕 z z z轴为 y a w yaw yaw偏航角，绕 y y y轴为 r o l l roll roll滚转角，绕 x x x轴负方向为 p i t c h pitch pitch俯仰角，正负按照右手定则；
- 实际上在表达 y a w , p i t c h , r o l l yaw,pitch,roll yaw,pitch,roll姿态角时默认在（前-左-上）坐标系上进行，因此在表达这些角度时也可以增加一个（前-左-上）坐标系，从乘一次旋转矩阵。
2. 则点从世界坐标系到像素坐标系透视变换如下：
像素坐标系到图像坐标系的变换为：
T
p
i
=
T
i
p
−
1
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
(
16
)
\begin{aligned} T_{pi} &= T_{ip}^{-1} \\ &=\begin{bmatrix}f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1\end{bmatrix} \qquad &(16) \end{aligned}
Tpi=Tip−1=⎣⎡fx000fy0cxcy1⎦⎤(16)
图像坐标系到相机坐标系的变换（对齐过程）为：
T
i
c
=
[
1
z
c
0
0
0
0
1
z
c
0
0
0
0
1
z
c
0
]
(
17
)
\begin{aligned} T_{ic} &=\begin{bmatrix}\frac{1}{z_c}&0&0&0\\ 0&\frac{1}{z_c}&0&0\\ 0&0&\frac{1}{z_c}&0\end{bmatrix} \qquad &(17) \end{aligned}
Tic=⎣⎡zc1000zc1000zc1000⎦⎤(17)
相机坐标系到世界坐标系的变换为：
T
c
w
=
T
w
c
−
1
=
T
o
c
−
1
⋅
T
w
o
−
1
=
T
o
c
−
1
⋅
T
w
r
−
1
⋅
T
w
t
−
1
=
T
o
c
−
1
⋅
T
w
x
−
1
⋅
T
w
y
−
1
⋅
T
w
z
−
1
⋅
T
w
t
−
1
T
c
w
=
[
1
0
0
0
0
0
−
1
0
0
1
0
0
0
0
0
1
]
⋅
[
1
0
0
0
0
cos
(
α
)
sin
(
α
)
0
0
−
sin
(
α
)
cos
(
α
)
0
0
0
0
1
]
⋅
[
1
0
0
0
0
1
0
0
0
0
1
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
−
h
0
0
0
1
]
T
c
w
=
[
cos
(
γ
)
sin
(
γ
)
0
0
−
sin
(
α
)
sin
(
γ
)
sin
(
α
)
cos
(
γ
)
−
cos
(
α
)
h
cos
(
α
)
−
cos
(
α
)
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
sin
(
α
)
−
h
sin
(
α
)
0
0
0
1
]
(
17
)
\begin{aligned} T_{cw} &= T_{wc}^{-1} \\ &= T_{oc}^{-1} \cdot T_{wo}^{-1} \\ &= T_{oc}^{-1} \cdot T_{wr}^{-1} \cdot T_{wt}^{-1} \\ &= T_{oc}^{-1} \cdot T_{wx}^{-1} \cdot T_{wy}^{-1} \cdot T_{wz}^{-1} \cdot T_{wt}^{-1} \\ T_{cw} &= \begin{bmatrix}1&0&0&0\\ 0&0&-1&0\\ 0&1&0&0\\ 0&0&0&1\end{bmatrix} \cdot \\ &\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} \cdot \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \cdot \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} \cdot \\ &\begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&-h\\ 0&0&0&1\end{bmatrix}\\ T_{cw} &= \begin{bmatrix}\cos \left(γ\right)&\sin \left(γ\right)&0&0\\ -\sin \left(α\right)\sin \left(γ\right)&\sin \left(α\right)\cos \left(γ\right)&-\cos \left(α\right)&h\cos \left(α\right)\\ -\cos \left(α\right)\sin \left(γ\right)&\cos \left(α\right)\cos \left(γ\right)&\sin \left(α\right)&-h\sin \left(α\right)\\ 0&0&0&1\end{bmatrix} \qquad &(17) \end{aligned}
TcwTcwTcw=Twc−1=Toc−1⋅Two−1=Toc−1⋅Twr−1⋅Twt−1=Toc−1⋅Twx−1⋅Twy−1⋅Twz−1⋅Twt−1=⎣⎢⎢⎡100000100−1000001⎦⎥⎥⎤⋅⎣⎢⎢⎡10000cos(α)−sin(α)00sin(α)cos(α)00001⎦⎥⎥⎤⋅⎣⎢⎢⎡1000010000100001⎦⎥⎥⎤⋅⎣⎢⎢⎡cos(γ)−sin(γ)00sin(γ)cos(γ)0000100001⎦⎥⎥⎤⋅⎣⎢⎢⎡10000100001000−h1⎦⎥⎥⎤=⎣⎢⎢⎡cos(γ)−sin(α)sin(γ)−cos(α)sin(γ)0sin(γ)sin(α)cos(γ)cos(α)cos(γ)00−cos(α)sin(α)00hcos(α)−hsin(α)1⎦⎥⎥⎤(17)
则世界坐标系到图像像素坐标系的变换为：
T
p
w
=
T
p
i
⋅
T
i
c
⋅
T
c
w
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
1
z
c
0
0
0
0
1
z
c
0
0
0
0
1
z
c
0
]
[
cos
(
γ
)
sin
(
γ
)
0
0
−
sin
(
α
)
sin
(
γ
)
sin
(
α
)
cos
(
γ
)
−
cos
(
α
)
h
cos
(
α
)
−
cos
(
α
)
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
sin
(
α
)
−
h
sin
(
α
)
0
0
0
1
]
=
1
z
c
[
cos
(
γ
)
f
x
−
sin
(
γ
)
cos
(
α
)
c
x
sin
(
γ
)
f
x
+
cos
(
γ
)
cos
(
α
)
c
x
sin
(
α
)
c
x
−
h
sin
(
α
)
c
x
−
sin
(
γ
)
(
sin
(
α
)
f
y
+
cos
(
α
)
c
y
)
cos
(
γ
)
(
sin
(
α
)
f
y
+
cos
(
α
)
c
y
)
sin
(
α
)
c
y
−
cos
(
α
)
f
y
−
h
(
sin
(
α
)
c
y
−
cos
(
α
)
f
y
)
−
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
cos
(
α
)
sin
(
α
)
−
h
sin
(
α
)
]
(
18
)
\begin{aligned} T_{pw} &= T_{pi} \cdot T_{ic} \cdot T_{cw} \\ &=\begin{bmatrix}f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1\end{bmatrix} \begin{bmatrix}\frac{1}{z_c}&0&0&0\\ 0&\frac{1}{z_c}&0&0\\ 0&0&\frac{1}{z_c}&0\end{bmatrix} \begin{bmatrix}\cos \left(γ\right)&\sin \left(γ\right)&0&0\\ -\sin \left(α\right)\sin \left(γ\right)&\sin \left(α\right)\cos \left(γ\right)&-\cos \left(α\right)&h\cos \left(α\right)\\ -\cos \left(α\right)\sin \left(γ\right)&\cos \left(α\right)\cos \left(γ\right)&\sin \left(α\right)&-h\sin \left(α\right)\\ 0&0&0&1\end{bmatrix} \\ &= {\normalsize \frac{1}{z_c}\begin{bmatrix} \cos(\gamma)f_x-\sin(\gamma)\cos(\alpha)c_x &\sin(\gamma)f_x+\cos(\gamma)\cos(\alpha)c_x &\sin(\alpha)c_x &-h\sin(\alpha)c_x \\ -\sin(\gamma)(\sin(\alpha)f_y+\cos(\alpha)c_y) &\cos(\gamma)(\sin(\alpha)f_y+\cos(\alpha)c_y) &\sin(\alpha)c_y-\cos(\alpha)f_y &-h(\sin(\alpha)c_y-\cos(\alpha)f_y) \\ -\sin(\gamma)\cos(\alpha) &\cos(\gamma)\cos(\alpha) &\sin(\alpha)&-h\sin(\alpha)\end{bmatrix}} \qquad &(18) \end{aligned}
Tpw=Tpi⋅Tic⋅Tcw=⎣⎡fx000fy0cxcy1⎦⎤⎣⎡zc1000zc1000zc1000⎦⎤⎣⎢⎢⎡cos(γ)−sin(α)sin(γ)−cos(α)sin(γ)0sin(γ)sin(α)cos(γ)cos(α)cos(γ)00−cos(α)sin(α)00hcos(α)−hsin(α)1⎦⎥⎥⎤=zc1⎣⎡cos(γ)fx−sin(γ)cos(α)cx−sin(γ)(sin(α)fy+cos(α)cy)−sin(γ)cos(α)sin(γ)fx+cos(γ)cos(α)cxcos(γ)(sin(α)fy+cos(α)cy)cos(γ)cos(α)sin(α)cxsin(α)cy−cos(α)fysin(α)−hsin(α)cx−h(sin(α)cy−cos(α)fy)−hsin(α)⎦⎤(18)
则世界坐标系的点变换到图像像素坐标系的点的过程为：
[
u
v
1
]
=
T
p
w
⋅
[
x
w
y
w
z
w
1
]
=
1
z
c
[
cos
(
γ
)
f
x
−
sin
(
γ
)
cos
(
α
)
c
x
sin
(
γ
)
f
x
+
cos
(
γ
)
cos
(
α
)
c
x
sin
(
α
)
c
x
−
h
sin
(
α
)
c
x
−
sin
(
γ
)
(
sin
(
α
)
f
y
+
cos
(
α
)
c
y
)
cos
(
γ
)
(
sin
(
α
)
f
y
+
cos
(
α
)
c
y
)
sin
(
α
)
c
y
−
cos
(
α
)
f
y
−
h
(
sin
(
α
)
c
y
−
cos
(
α
)
f
y
)
−
sin
(
γ
)
cos
(
α
)
cos
(
γ
)
cos
(
α
)
sin
(
α
)
−
h
sin
(
α
)
]
[
x
w
y
w
z
w
1
]
=
1
z
c
[
x
w
(
cos
(
γ
)
f
x
−
sin
(
γ
)
c
x
cos
(
α
)
)
+
y
w
(
cos
(
γ
)
c
x
cos
(
α
)
+
sin
(
γ
)
f
x
)
+
c
x
z
w
sin
(
α
)
−
c
x
h
sin
(
α
)
−
sin
(
γ
)
x
w
(
cos
(
α
)
c
y
+
sin
(
α
)
f
y
)
+
cos
(
γ
)
y
w
(
cos
(
α
)
c
y
+
sin
(
α
)
f
y
)
+
z
w
(
sin
(
α
)
c
y
−
cos
(
α
)
f
y
)
−
h
(
sin
(
α
)
c
y
−
cos
(
α
)
f
y
)
−
sin
(
γ
)
x
w
cos
(
α
)
+
cos
(
γ
)
y
w
cos
(
α
)
+
z
w
sin
(
α
)
−
h
sin
(
α
)
]
(
19
)
\begin{aligned} \begin{bmatrix}u\\ v\\ 1\end{bmatrix} &= T_{pw} \cdot \begin{bmatrix}x_w\\ y_w\\ z_w\\ 1\end{bmatrix} \\ &={\normalsize \frac{1}{z_c}\begin{bmatrix} \cos(\gamma)f_x-\sin(\gamma)\cos(\alpha)c_x &\sin(\gamma)f_x+\cos(\gamma)\cos(\alpha)c_x &\sin(\alpha)c_x &-h\sin(\alpha)c_x \\ -\sin(\gamma)(\sin(\alpha)f_y+\cos(\alpha)c_y) &\cos(\gamma)(\sin(\alpha)f_y+\cos(\alpha)c_y) &\sin(\alpha)c_y-\cos(\alpha)f_y &-h(\sin(\alpha)c_y-\cos(\alpha)f_y) \\ -\sin(\gamma)\cos(\alpha) &\cos(\gamma)\cos(\alpha) &\sin(\alpha)&-h\sin(\alpha)\end{bmatrix}} \begin{bmatrix}x_w\\ y_w\\ z_w\\ 1\end{bmatrix} \\ &={\normalsize \frac{1}{z_c}\begin{bmatrix} x_w\left(\cos \left(γ\right)f_x-\sin \left(γ\right)c_x\cos \left(α\right)\right)+y_w\left(\cos \left(γ\right)c_x\cos \left(α\right)+\sin \left(γ\right)f_x\right)+c_xz_w\sin \left(α\right)-c_xh\sin \left(α\right)\\ -\sin \left(γ\right)x_w\left(\cos \left(α\right)c_y+\sin \left(α\right)f_y\right)+\cos \left(γ\right)y_w\left(\cos \left(α\right)c_y+\sin \left(α\right)f_y\right)+z_w\left(\sin \left(α\right)c_y-\cos \left(α\right)f_y\right)-h\left(\sin \left(α\right)c_y-\cos \left(α\right)f_y\right)\\ -\sin \left(γ\right)x_w\cos \left(α\right)+\cos \left(γ\right)y_w\cos \left(α\right)+z_w\sin \left(α\right)-h\sin \left(α\right)\end{bmatrix}} \qquad &(19) \end{aligned}
⎣⎡uv1⎦⎤=Tpw⋅⎣⎢⎢⎡xwywzw1⎦⎥⎥⎤=zc1⎣⎡cos(γ)fx−sin(γ)cos(α)cx−sin(γ)(sin(α)fy+cos(α)cy)−sin(γ)cos(α)sin(γ)fx+cos(γ)cos(α)cxcos(γ)(sin(α)fy+cos(α)cy)cos(γ)cos(α)sin(α)cxsin(α)cy−cos(α)fysin(α)−hsin(α)cx−h(sin(α)cy−cos(α)fy)−hsin(α)⎦⎤⎣⎢⎢⎡xwywzw1⎦⎥⎥⎤=zc1⎣⎡xw(cos(γ)fx−sin(γ)cxcos(α))+yw(cos(γ)cxcos(α)+sin(γ)fx)+cxzwsin(α)−cxhsin(α)−sin(γ)xw(cos(α)cy+sin(α)fy)+cos(γ)yw(cos(α)cy+sin(α)fy)+zw(sin(α)cy−cos(α)fy)−h(sin(α)cy−cos(α)fy)−sin(γ)xwcos(α)+cos(γ)ywcos(α)+zwsin(α)−hsin(α)⎦⎤(19)
具体计算时由于只处理地面上的点，即 z w = 0 z_w = 0 zw=0，所以 T p w T_{pw} Tpw第三列均是与0相乘，为了简化计算第三列可以全部设为0，且最后经过单位化消去了 z c z_c zc。最后得到计算IPM时从世界地面到像素的变换可从式(18)简化表示为 T 3 × 4 T_{3\times 4} T3×4：
T 3 × 4 = [ cos ( γ ) f x − sin ( γ ) cos ( α ) c x sin ( γ ) f x + cos ( γ ) cos ( α ) c x 0 − h sin ( α ) c x − sin ( γ ) ( sin ( α ) f y + cos ( α ) c y ) cos ( γ ) ( sin ( α ) f y + cos ( α ) c y ) 0 − h ( sin ( α ) c y − cos ( α ) f y ) − sin ( γ ) cos ( α ) cos ( γ ) cos ( α ) 0 − h sin ( α ) ] ( 20 ) \begin{aligned} T_{3\times 4} &= {\normalsize \begin{bmatrix} \cos(\gamma)f_x-\sin(\gamma)\cos(\alpha)c_x &\sin(\gamma)f_x+\cos(\gamma)\cos(\alpha)c_x &0 &-h\sin(\alpha)c_x \\ -\sin(\gamma)(\sin(\alpha)f_y+\cos(\alpha)c_y) &\cos(\gamma)(\sin(\alpha)f_y+\cos(\alpha)c_y) &0 &-h(\sin(\alpha)c_y-\cos(\alpha)f_y) \\ -\sin(\gamma)\cos(\alpha) &\cos(\gamma)\cos(\alpha) &0&-h\sin(\alpha)\end{bmatrix}} \qquad &(20) \end{aligned} T3×4=⎣⎡cos(γ)fx−sin(γ)cos(α)cx−sin(γ)(sin(α)fy+cos(α)cy)−sin(γ)cos(α)sin(γ)fx+cos(γ)cos(α)cxcos(γ)(sin(α)fy+cos(α)cy)cos(γ)cos(α)000−hsin(α)cx−h(sin(α)cy−cos(α)fy)−hsin(α)⎦⎤(20)
同样的如果式(20)中的角度
α
,
γ
\alpha,\gamma
α,γ取负值，提取最后一列的
−
h
-h
−h。
则所有的
s
i
n
sin
sin 值取反，同时矩阵的第三列均为0，第四列提取出
−
h
-h
−h，此时即得到下式：
T I G = [ f x cos ( γ ) + cos ( α ) sin ( γ ) c x − f x sin ( γ ) + cos ( α ) cos ( γ ) c x 0 − sin ( α ) c x sin ( γ ) ( − f y sin ( α ) + cos ( α ) c y ) cos ( γ ) ( − f y sin ( α ) + cos ( α ) c y ) 0 − f y cos ( α ) − sin ( α ) c y cos ( α ) sin ( γ ) cos ( α ) cos ( γ ) 0 − sin ( α ) ] ( 21 ) {\color{darkorange} \begin{aligned} T_{IG} &= \begin{bmatrix} f_x\cos(\gamma)+\cos(\alpha) \sin(\gamma)c_x &-f_x\sin(\gamma)+\cos(\alpha) \cos(\gamma)c_x &0 &-\sin(\alpha)c_x \\ \sin(\gamma)(-f_y \sin(\alpha)+\cos(\alpha)c_y) &\cos(\gamma)(-f_y \sin(\alpha)+\cos(\alpha)c_y) &0 &-f_y\cos(\alpha)-\sin(\alpha)c_y \\ \cos(\alpha) \sin(\gamma) &\cos(\alpha) \cos(\gamma) &0 &-\sin(\alpha) \end{bmatrix} \qquad(21) \end{aligned}} TIG=⎣⎡fxcos(γ)+cos(α)sin(γ)cxsin(γ)(−fysin(α)+cos(α)cy)cos(α)sin(γ)−fxsin(γ)+cos(α)cos(γ)cxcos(γ)(−fysin(α)+cos(α)cy)cos(α)cos(γ)000−sin(α)cx−fycos(α)−sin(α)cy−sin(α)⎦⎤(21)
986
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
