IPM逆透视变换问题2/3之：Image >> Ground
IPM逆透视变换问题2/3之：Image >> Ground
1. 如果前置条件如下：
- 坐标系坐标系定义为：世界（右-前-上），相机（右-下-前），像素（右-下）；
- 旋转角度表示为：绕\(z\)轴为\(yaw\)偏航角，绕\(y\)轴为\(roll\)滚转角，绕\(x\)轴负方向为\(pitch\)俯仰角，正负按照右手定则；
- 实际上在表达\(yaw,pitch,roll\)姿态角时默认在（前-左-上）坐标系上进行，因此在表达这些角度时也可以增加一个（前-左-上）坐标系，从乘一次旋转矩阵。
2. 则点从像素坐标系到世界坐标系逆透视变换如下：
世界坐标系（右-前-上）沿着自身\(Z\)轴平移\(h\)，得到\(T_{wt}\):
\[\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}
\]
相机的姿态角度（滚转,俯仰,偏航）\((roll,pitch,yaw)\)对应在世界坐标系(右前上)是\((\alpha = -pitch, \beta = roll, \gamma = yaw)\)，计算消失点时滚转角无影响即\(roll=0\)，则绕着固定坐标系\(X,Y,Z\)三个坐标轴旋转后得到的变换矩阵\(T_{wr}\):
\[\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_{wo}\)
\[\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_{co}\)
\[\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}
\]
世界坐标系到相机坐标系的变换为：
\[\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}
\]
相机坐标系到图像坐标系的变换（对齐过程）为：
\[\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}
\]
图像坐标系到像素坐标系的变换为：
\[\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}
\]
则世界坐标系到图像像素坐标系的变换为：
\[\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}
\]
则图像像素坐标系的点变换到世界坐标系的点的过程为：
\[\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坐标为0，即\(z_w == 0\)，所以有：
\[\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)
\]
则可以得到：
\[\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\)带入上式即有：
\[\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}
\]
具体计算时要分离（u, v）的 影响，\(T_{wp}\)第三行中的\(v\)难以分离，但是第三行用于计算\(z_w\)，且地面上的\(z_w\)恒为零，所以可以全部设为0；\(T_{wp}\)的第四行则比较容易分离。最后得到计算IPM时从像素到世界的变换\(T_{4\times 3}\)：
\[\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}
\]
如果式(14)中的角度\(\alpha,\gamma\)取负值，同时矩阵乘\(-1\)（不影响结果）。
则所有的\(sin\)值取反，同时矩阵的第三行与式(14)中的第四行乘\(h\)（设定成什么都可以，不影响结果），此时即得到：
\[{\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} }
\]
之所进行这样的对比，主要是网络上大部分关于IPM的代码都是使用式(15)进行代码编写，而且在没有多少优化的情况下，导致代码阅读性较差，极不友好。
浙公网安备 33010602011771号