IPM逆透视变换问题3/3：Ground >> Image
1. 同样如果前置条件如下：
- 坐标系坐标系定义为：世界（右-前-上），相机（右-下-前），像素（右-下）；
- 旋转角度表示为：绕\(z\)轴为\(yaw\)偏航角，绕\(y\)轴为\(roll\)滚转角，绕\(x\)轴负方向为\(pitch\)俯仰角，正负按照右手定则；
- 实际上在表达\(yaw,pitch,roll\)姿态角时默认在（前-左-上）坐标系上进行，因此在表达这些角度时也可以增加一个（前-左-上）坐标系，从乘一次旋转矩阵。
2. 则点从世界坐标系到像素坐标系透视变换如下：
像素坐标系到图像坐标系的变换为：
\[\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} \]图像坐标系到相机坐标系的变换（对齐过程）为：
\[\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} \]相机坐标系到世界坐标系的变换为：
\[\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} \]则世界坐标系到图像像素坐标系的变换为：
\[\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} \]则世界坐标系的点变换到图像像素坐标系的点的过程为：
\[\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} \]具体计算时由于只处理地面上的点，即\(z_w = 0\)，所以\(T_{pw}\)第三列均是与0相乘，为了简化计算第三列可以全部设为0，且最后经过单位化消去了\(z_c\)。最后得到计算IPM时从世界地面到像素的变换可从式(18)简化表示为\(T_{3\times 4}\)：
\[\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} \]同样的如果式(20)中的角度\(\alpha,\gamma\)取负值，提取最后一列的\(-h\)。
则所有的 \(sin\) 值取反，同时矩阵的第三列均为0，第四列提取出\(-h\)，此时即得到下式：
标签：cos,right,Image,IPM,bmatrix,alpha,left,sin,Ground From： https://www.cnblogs.com/streamwill/p/17189658.html