坐标系转换

（一）三维空间坐标互转

1 问题背景

如图，有两个坐标系空间A和B。P为空间中一点，我们目的是：根据P在B中的坐标BP，获取P在A中的坐标AP。P在A坐标系中的坐标AP，如下图所示：

P在B中的坐标BP如下图所示：

无人车中存在各种坐标系的转换，如：世界坐标系和lidar坐标系，lidar坐标系和gps坐标系，gps坐标系和camera坐标系等。

2 转换原理

讨论变换前，先确定下坐标系的方向。一共只有两种：（1） x、y、z正朝向逆时针，右手坐标系（大拇指对应x轴正向，食指对应y轴正向，中指对应z轴正向）；（2） x、y、z正朝向顺时针，左手坐标系（大拇指对应x轴正向，食指对应y轴正向，中指对应z轴正向），如图：

由于这两种坐标系中，旋转角会有差异，所以我们的公式中，统一采用（1）x、y、z正朝向逆时针的右手坐标系。

（1） 平移转换

平移转换关系如上图所示，红字部分是A坐标系中坐标，蓝字是B坐标系坐标，三者关系如公式1.1所示：

（公式1.1）

（2） 旋转转换

学习旋转变换前，先了解下角度的方向。右手坐标系中角度遵循右手法则，即：右手大拇指朝向坐标轴正向，其余四指方向为旋转正方向，我们后面所说的角度都遵循这个标准。

下面，我们以绕z轴旋转为例，说明旋转转换的公式推导：

左上图是三维空间图，图中我们以z轴为轴心，右手法则正向旋转A空间，角度如图绿线部分，得到空间B。空间A和B在z轴坐标没有变化，x、y坐标发生了变化。变化后的x、y平面图如右上图所示。下图中，我们增加两条红色虚线，辅助点P在A和B两个坐标系中的坐标进行转换。

根据上图两条红色虚线，不难理解下面的转换关系公式1.2：

（公式1.2）

公式转换为矩阵表示为公式1.3：

（公式1.3）

即：

（公式1.4）

上面只是以z轴为轴心旋转的结果，同理，沿着x和y轴，也可以得到类似转换关系（以下简称cos为c，sin为s）：

（公式1.5）

三个矩阵相乘，可以得到旋转变换公式：

（公式1.6）

旋转转换关系如下：

（公式1.7）

（3） 复合转换

复合变换就是综合平移变换和旋转变换，结合公式1.1、公式1.7得到公式1.8如下：

（公式1.8）

经过齐次变换之后，上公式可以转换为：

（公式1.9）

抽取转换矩阵：

（公式1.10）

最终，空间转换矩阵为：

（公式1.11）

该转换矩阵的目的，是将B坐标系中的坐标，转换为A坐标系中的坐标。perception代码中，Eigen:Matrix4d存储的，就是这个4*4矩阵，是将GPS坐标系中的坐标，转换为世界坐标。其中：

1、c和s表示cos和sin；

2、α、β、γ分别表示B空间到A空间转换，沿z、y、x轴旋转的角度；

3、第四列的x、y、z，表示B空间的原点，在A空间中的坐标；

另，该矩阵的逆矩阵，可以将A坐标系中的坐标，转换为B坐标系中的坐标。

（二）三维空间转二维图像空间

1 几种空间定义

A：三维世界坐标系，以空间中一点为原点

B：三维摄像机坐标系，以光心为原点

C：二维图像物理坐标系，以平面中心为原点

D：二维图像像素坐标系，以图像左上角为原点

本节的目的是说明如何通过三维空间坐标等参数，计算三维空间物体映射在二维图像中的位置，主要以红绿灯为例说明，如下图所示：

2 三维世界坐标系（A）转三维摄像机坐标系（B）

这步中，我们的目的是将红绿灯的四个点的世界坐标系（A）坐标转换为摄像机坐标系（B）坐标，直接利用（一）中描述的三维空间坐标互转即可。

P点在A和B坐标系中的转换关系公式2.1所示：

（公式2.1）

由公式2.1推导出公式2.2：

（公式2.2）

3 三维摄像机坐标系（B）映射二维物理坐标系（C）

这步中，我们的目的是，将红绿灯顶点的摄像机坐标系（B）坐标，转换为二维物理坐标系（C）坐标。P点在B和C坐标系中的转换关系公式2.3所示：

(公式2.3)

其中的f是焦距，BZP是P点在摄像机坐标系中的z坐标，直观理解就是点P到摄像机平面的垂直距离。由公式2.3推导出公式2.4：

（公式2.4）

4 二维物理坐标系（C）映射二维像素坐标系（D）

二维物理坐标系（C）和二维像素坐标系（D）的主要区别是：1、原点不同，C的中心在摄像机中心，D的中心在图像左上角；2、转换不同，需要由C的单位毫米，转换为D的单位像素，dx和dy分别是横纵坐标的转换率（举个例子，CCD的尺寸是8mm * 6mm，帧画面的分辨率设置为640X480，那么毫米与像素点之间的转换关系就是80pixel/mm。”设CCD每个像素点的物理大小为dx*dy，相应地，就有 dx=dy=1/80），转换关系如公式2.5：

（公式2.5）

由公式2.5推导出公式2.6

（公式2.6）

5 D和A的关系推导

由公式2.6、2.4、2.2推导得出：

（公式2.7）、

通过合并进一步推导：

（公式2.8）

令fx=f/dx，fy=f/dy推导得出：

（公式2.9）

上面公式就是由三维世界坐标系（A）坐标转换为二维图像坐标系（D）坐标的转换关系，其中：

D：就是BzP，直观理解是目标点P和摄像机平面的垂直距离；

M1：就是摄像机内参矩阵，主要由fx、fy、u0、v0等参数组成，代码中在production/trafic_light/data/models/projection/目录下，有长焦、短焦两个摄像机的参数；

M2：是世界坐标系和摄像机坐标系的旋转矩阵，一般称为外参；