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

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

1. 如果前置条件如下：

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

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

世界坐标系（右-前-上）沿着自身$Z$轴平移$h$，得到$T_{wt}$:
$$\begin{array}{rlr}{T}_{wt}& =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& h\\ 0& 0& 0& 1\end{array}\right]& (1)\end{array}$$

相机的姿态角度（滚转,俯仰,偏航）$(roll,pitch,yaw)$对应在世界坐标系(右前上)是$(\alpha =-pitch,\beta =roll,\gamma =yaw)$，计算消失点时滚转角无影响即$roll=0$，则绕着固定坐标系$X,Y,Z$三个坐标轴旋转后得到的变换矩阵$T_{wr}$:
$$\begin{array}{rlr}{T}_{wr}& =T_{wz}\cdot T_{wy}\cdot T_{wx}& \\ & =\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]\left[\begin{array}{cccc}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{array}\right]\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]& \\ & =\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]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\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]& \\ & =\left[\begin{array}{cccc}\cos \left(\gamma \right)& -\sin \left(\gamma \right)\cos \left(\alpha \right)& \sin \left(\gamma \right)\sin \left(\alpha \right)& 0\\ \sin \left(\gamma \right)& \cos \left(\gamma \right)\cos \left(\alpha \right)& -\cos \left(\gamma \right)\sin \left(\alpha \right)& 0\\ 0& \sin \left(\alpha \right)& \cos \left(\alpha \right)& 0\\ 0& 0& 0& 1\end{array}\right]& (2)\end{array}$$

世界坐标系（右前上）到相机自身的(右前上)坐标系到相机的变换$T_{wo}$
$$\begin{array}{rlr}{T}_{wo}& =T_{wt}\cdot T_{wr}& \\ & =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& h\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}\cos \left(\gamma \right)& -\sin \left(\gamma \right)\cos \left(\alpha \right)& \sin \left(\gamma \right)\sin \left(\alpha \right)& 0\\ \sin \left(\gamma \right)& \cos \left(\gamma \right)\cos \left(\alpha \right)& -\cos \left(\gamma \right)\sin \left(\alpha \right)& 0\\ 0& \sin \left(\alpha \right)& \cos \left(\alpha \right)& 0\\ 0& 0& 0& 1\end{array}\right]& \\ & =\left[\begin{array}{cccc}\cos \left(\gamma \right)& -\sin \left(\gamma \right)\cos \left(\alpha \right)& \sin \left(\gamma \right)\sin \left(\alpha \right)& 0\\ \sin \left(\gamma \right)& \cos \left(\gamma \right)\cos \left(\alpha \right)& -\cos \left(\gamma \right)\sin \left(\alpha \right)& 0\\ 0& \sin \left(\alpha \right)& \cos \left(\alpha \right)& h\\ 0& 0& 0& 1\end{array}\right]& (3)\end{array}$$

相机自身的(右前上)坐标系到相机坐标系的变换$T_{co}$
$$\begin{array}{rl}{T}_{oc}& =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right][\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& -1& 1& \\ 0& 0& 0& \end{array}\\ & \end{array}$$