IPM逆透视变换问题（3）：Ground --＞ Image

IPM逆透视变换问题（3）：Ground --> Image

1. 同样如果前置条件如下：

1. 坐标系坐标系定义为：世界（右-前-上），相机（右-下-前），像素（右-下）；
2. 旋转角度表示为：绕$z$轴为$yaw$偏航角，绕$y$轴为$roll$滚转角，绕$x$轴负方向为$pitch$俯仰角，正负按照右手定则；
3. 实际上在表达$yaw,pitch,roll$姿态角时默认在（前-左-上）坐标系上进行，因此在表达这些角度时也可以增加一个（前-左-上）坐标系，从乘一次旋转矩阵。

2. 则点从世界坐标系到像素坐标系透视变换如下：

像素坐标系到图像坐标系的变换为：
$$\begin{array}{rlr}{T}_{pi}& =T_{ip}^{-1}& \\ & =\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]& (16)\end{array}$$

图像坐标系到相机坐标系的变换（对齐过程）为：
$$\begin{array}{rlr}{T}_{ic}& =\left[\begin{array}{cccc}\frac{1}{z_c}& 0& 0& 0\\ 0& \frac{1}{z_c}& 0& 0\\ 0& 0& \frac{1}{z_c}& 0\end{array}\right]& (17)\end{array}$$

相机坐标系到世界坐标系的变换为：
$$\begin{array}{rlr}{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}& =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& -1& 0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]\cdot & \\ & \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \left(\alpha \right)& \sin \left(\alpha \right)& 0\\ 0& -\sin \left(\alpha \right)& \cos \left(\alpha \right)& 0\\ 0& 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{cccc}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{array}\right]\cdot & \\ & \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -h\\ 0& 0& 0& 1\end{array}\right]& \\ T_{cw}& =\left[\begin{array}{cccc}\cos \left(\gamma \right)& \sin \left(\gamma \right)& 0& 0\\ -\sin \left(\alpha \right)\sin \left(\gamma \right)& \sin \left(\alpha \right)\cos \left(\gamma \right)& -\cos \left(\alpha \right)& h\cos \left(\alpha \right)\\ -\cos \left(\alpha \right)\sin \left(\gamma \right)& \cos \left(\alpha \right)\cos \left(\gamma \right)& \sin \left(\alpha \right)& -h\sin \left(\alpha \right)\\ 0& 0& 0& 1\end{array}\right]& (17)\end{array}$$

则世界坐标系到图像像素坐标系的变换为：
$$\begin{array}{rlr}{T}_{pw}& =T_{pi}\cdot T_{ic}\cdot T_{cw}& \\ & =\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}\frac{1}{z_c}& 0& 0& 0\\ 0& \frac{1}{z_c}& 0& 0\\ 0& 0& \frac{1}{z_c}& 0\end{array}\right]\left[\begin{array}{cccc}\cos \left(\gamma \right)& \sin \left(\gamma \right)& 0& 0\\ -\sin \left(\alpha \right)\sin \left(\gamma \right)& \sin \left(\alpha \right)\cos \left(\gamma \right)& -\cos \left(\alpha \right)& h\cos \left(\alpha \right)\\ -\cos \left(\alpha \right)\sin \left(\gamma \right)& \cos \left(\alpha \right)\cos \left(\gamma \right)& \sin \left(\alpha \right)& -h\sin \left(\alpha \right)\\ 0& 0& 0& 1\end{array}\right]& \\ & =\frac{1}{z_c}\left[\begin{array}{cccc}\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{array}\right]& (18)\end{array}$$

则世界坐标系的点变换到图像像素坐标系的点的过程为：
$$\begin{array}{rlr}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]& =T_{pw}\cdot \left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]& \\ & =\frac{1}{z_c}\left[\begin{array}{cccc}\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{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]& \\ & =\frac{1}{z_c}\left[\begin{array}{c}{x}_w\left(\cos \left(\gamma \right)f_x-\sin \left(\gamma \right)c_x\cos \left(\alpha \right)\right)+y_w\left(\cos \left(\gamma \right)c_x\cos \left(\alpha \right)+\sin \left(\gamma \right)f_x\right)+c_x{z}_w\sin \left(\alpha \right)-c_{x}h\sin \left(\alpha \right)\\ -\sin \left(\gamma \right)x_w\left(\cos \left(\alpha \right)c_y+\sin \left(\alpha \right)f_y\right)+\cos \left(\gamma \right)y_w\left(\cos \left(\alpha \right)c_y+\sin \left(\alpha \right)f_y\right)+z_w\left(\sin \left(\alpha \right)c_y-\cos \left(\alpha \right)f_y\right)-h\left(\sin \left(\alpha \right)c_y-\cos \left(\alpha \right)f_y\right)\\ -\sin \left(\gamma \right)x_w\cos \left(\alpha \right)+\cos \left(\gamma \right)y_w\cos \left(\alpha \right)+z_w\sin \left(\alpha \right)-h\sin \left(\alpha \right)\end{array}\right]& (19)\end{array}$$

具体计算时由于只处理地面上的点，即$z_w=0$，所以$T_{pw}$第三列均是与0相乘，为了简化计算第三列可以全部设为0，且最后经过单位化消去了$z_c$。最后得到计算IPM时从世界地面到像素的变换可从式(18)简化表示为$T_{3\times 4}$：

$$\begin{array}{rlr}{T}_{3\times 4}& =\left[\begin{array}{cccc}\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{array}\right]& (20)\end{array}$$

同样的如果式(20)中的角度$\alpha ,\gamma$取负值，提取最后一列的$-h$。
则所有的 $sin$ 值取反，同时矩阵的第三列均为0，第四列提取出$-h$，此时即得到下式：

$$\begin{array}{rl}{T}_{IG}& =\left[\begin{array}{cccc}{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{array}\right](21)\end{array}$$