逆透视矩阵原理以及应用（平视图转鸟瞰图、俯视图）

参考博客：逆透视变化详解
参考论文：Lane Detector

真实世界的景象转换为数字图片的步骤流程如下图：

1、刚体变换

从下图中可以看到，世界坐标为(X,Y,Z)相机坐标为(Xc,Yc,Zc)，而世界坐标变换到相机坐标存在一个旋转矩阵变换R以及一个位移变换T。

其数学公式表示如下,其中(X,Y,Z)如果写成(X,Y,Z,0)，它就是个向量；如果是(X,Y,Z,1)，它就是个点。而旋转和缩放对于向量和点都有意义，平移变换只对于点才有意义，因为普通向量没有位置概念，只有大小和方向.

如果假设不存在坐标平移，可以简化公式为：

关于R下面有对应的旋转情况：

（1）、绕X轴旋转

（2）、绕Y轴旋转

(3)、绕Z轴旋转

2、透视投影

根据上述的关系我们可以推得：

矩阵形式为：

3、数字化图像

数字图像在计算机内为M*N的数组，M行N列的图像中每一个元素（pixel）数值就是图像点的亮度（灰度）。
如图，在图像上定义直角坐标系U，V，每一个像素为单位的图像坐标系坐标，
由于（u,v）只能表示像素位于数组中的列数与行数，并没有使用物理单位表示该像素在图像中位置，所以需要再建立以物理单位（mm）表示的图像坐标系，该图像坐标系以图像内某一点xy(0,0)为原点，x轴和y轴分别平行于u、v。

如图中，（u、v）表示以像素为单位的图像坐标系的坐标，（X、Y）表示以mm为单位的图像坐标系的坐标。
假设每一个像素在X轴与Y轴方向上的物理尺寸为dx、dy，则图像任意一个像素在两个坐标系下的坐标有如下关系:

其中分别表示xy坐标系原点在uv坐标系中的位置。

进而得到如下的矩阵表达：

4、整体变换的示意图（未考虑畸形校正）

5、原理应用

IPM坐标
左：坐标轴（世界，相机和图像坐标系）。 右：定义俯仰α和偏航β角。

为了获得输入图像的IPM，我们使用相机内参（焦距和光学中心）和外部（俯仰角，偏航角和地面高度）参数来执行此转换。如图所示， 我们首先定义一个世界坐标系{Fw} = {Xw，Yw，Zw}（以相机光学中心为中心），相机坐标系{Fc} ={Xc，Yc，Zc}，以及图像坐标系{Fi} = {u，v}。 我们假设相机框坐标系的Xc轴停留在世界坐标系XwYw平面中，且相机只存在俯仰角α（上下）和偏转角β（左右）但没有滚动。 相机坐标系中心在地平面上方的高度为h。

IPM转换的步骤为：
图片到世界坐标系的转换矩阵为：

h为高度，
fu、fv为相机焦距
cu、cv为相机光学中心
c1 = np.cos(cameraInfo[5] * np.pi / 180)
s1 = np.sin(cameraInfo[5] * np.pi / 180)
c2 = np.cos(cameraInfo[6] * np.pi / 180)
s2 = np.sin(cameraInfo[6] * np.pi / 180)
这是我的相机参数CameraInfo=[cameraInfo_focalLengthX,cameraInfo_focalLengthY,cameraInfo_opticalCenterX,cameraInfo_opticalCenterY,cameraInfo_pitch,cameraInfo_yaw,cameraInfo_imageWidth,cameraInfo_imageHeight]

世界坐标系到图片坐标系的转换矩阵：

参数同上

消失点的求取：

效果（自己的相机、自己的图片）