无人驾驶中常用坐标系及相互转换的数学原理介绍
无人驾驶是一个非常困难的任务，它需要多种传感器的配合，所以在无人驾驶中存在多种传感器，例如相机camera，激光雷达lidar，毫米波雷达radar，IMU，GPS等。在每个传感器中都有自己的坐标系，所以在无人驾驶中存在诸多的坐标系转换问题。本文将会基于nusenes数据集介绍一下世界坐标系，自车坐标系，相机坐标系，像素坐标系，激光雷达坐标系，毫米波雷达坐标系，IMU坐标系，以及最近在无人驾驶领域最流行的bev检测方案中的bev坐标系和bev像素坐标系，之后会说明一下nusenes数据集与argoverse数据集的区别，最后会介绍相机内外参和不同坐标系之间的转换。
1 nusenes数据集下的各个坐标系
(1) 像素坐标系与相机坐标系
上图所示为像素坐标系和相机坐标系，观察方向是沿着z轴方向观察，所以像素坐标系坐标原点在左上角，向右为x轴方向，向下为y轴方向，为左手坐标系，其与矩阵存储形式相同，只是x轴为列标，y轴为行标。对于相机坐标系，坐标原点在光心位置，z轴朝向前方，x轴指向右侧，y轴指向下方，为右手坐标系。
需要特别说明的是，在像素坐标系中或在矩阵存储形式下，图片整体与现实世界完全相同，并不存在中心对称，原因是从相机坐标系转换到像素坐标系的过程中，x轴和y轴分别都乘了-1，所以图片在像素坐标系中的样子并非相机成像于CCD传感器表面时的样子，而是通过乘-1做了镜像对称。
在nuscenes数据集中，像素坐标系和相机坐标系的定义与上面的描述完全相同。
(2) 自车坐标系与全局坐标系
如上图，自车坐标系指的是固定在本车车身上的坐标系，一般用本车后车轴中心点作为坐标原点，以车辆朝向为x方向，以车辆左侧为y方向，以垂直向上为z方向，形成右手坐标系。沿x轴的逆时针转动为roll翻滚角，沿y轴的逆时针转动为pitch俯仰角，沿z轴的逆时针转动为yaw航向角。全局坐标系一般为z轴垂直向上的右手坐标系。
在nuscenes数据集中，自车坐标系和全局坐标系的定义与上面的描述完全相同。
自车坐标系的介绍参考：关于整车坐标系的疑问，xyz的正负向各指的哪个方向? - 知乎
(3) 激光雷达坐标系与毫米波雷达坐标系
如图1所示，激光雷达坐标系，一般以传感器处为坐标原点，以车辆朝向为y方向，以车辆右侧为x方向，以垂直向上为z轴方向，形成右手坐标系。毫米波雷达坐标系，一般以传感器处为坐标原点，以雷达照射方向为x方向，以雷达照射方向的左侧为y方向，以垂直向上为z轴方向，形成右手坐标系。
在nuscenes数据集中，激光雷达坐标系和毫米波雷达坐标系的定义与上面的描述完全相同。
(4) bev坐标系与bev像素坐标系(nusenes数据集与argoverse数据集的区别)
bev坐标系是一个跟算法相关的虚拟坐标系，现实中并不存在bev传感器，bev坐标系与数据集所使用的“标注坐标系”有关。上图所示为nuscenes数据集的bev坐标系和bev像素坐标系，在nuscenes数据集中，标注结果是建立在激光雷达坐标系基础上的，而激光雷达采用的是以传感器处为坐标原点，以车辆朝向为y方向，以车辆右侧为x方向，以垂直向上为z轴方向，所形成的右手坐标系。所以在nuscenes数据集中，bev坐标系使用与激光雷达坐标系相同的坐标系。而bev像素坐标系则是坐标原点位于bev图的左下角，向上是y轴，向右是x轴，形成右手坐标系。所以在bev像素坐标系中，bev图不会上下颠倒，但bev像素坐标系与bev矩阵存储形式存在上下颠倒，两种坐标系下的旋转量相差一个正负号，平移量是完全相同，所以在自己写函数或调用库函数时，一定注意当前是在bev像素坐标系下进行操作还是在bev矩阵存储形式下进行操作。除此以外，两个坐标系的索引相反，矩阵索引是(y，x)，而像素坐标是(x, y)。所以在nuscenes数据集中的bev图一般h大于等于w，车头朝上。
上图所示为argoverse数据集的bev坐标系和bev像素坐标系，在argoverse数据集中，标注结果是建立在自车系基础上的，而自车坐标系是以车轴中心点作为坐标原点，以车辆朝向为x方向，以车辆左侧为y方向，以垂直向上为z方向，所形成的右手坐标系。所以在argoverse数据集中，bev坐标系使用与自车坐标系相同的坐标系。而bev像素坐标系则是坐标原点位于bev图的左下角，向上是y轴，向右是x轴，形成右手坐标系。所以在argoverse数据集中的bev图一般h小于等于w，车头朝右。
2 坐标系转换的数学表达
(1) 旋转的数学表达
对于旋转的数学表示，存在欧拉角，旋转矩阵，轴角和四元数表示法，四种表示法之间可以相互转换，本文并不讲解具体转换方式，仅仅说明一下四种表示法的物理和数学表示，具体转换方式见：
三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换 - 知乎
旋转矩阵、欧拉角、四元数以及旋转矢量的介绍及工程应用 - 知乎
注意本节接下来对旋转所有的描述都是假定坐标系坐标轴不动，而坐标点转动，虽然对于局部坐标系内部本身来说，旋转坐标系坐标轴和旋转坐标点是相对运动，本质上没有区别，但是当局部坐标系外面还存在一个观察坐标系时，那为了保证局部坐标系内所计算结果的一致性，在观察坐标系看来，对局部坐标系坐标轴的旋转角度与对局部坐标系中坐标点的旋转角度之间，相差一个正负号。
a 旋转角度定义
在右手坐标系中，顺时针旋转时角度为负，逆时针旋转时角度为正，沿z轴旋转时，以x轴为角度起点；沿x轴旋转时，以y轴为角度起点；沿y轴旋转时，以z轴为角度起点。
在左手坐标系中，顺时针旋转时角度为正，逆时针旋转时角度为负，沿z轴旋转时，以x轴为角度起点；沿x轴旋转时，以y轴为角度起点；沿y轴旋转时，以z轴为角度起点。
在数学中角度本身是有正负号的，如上图右手坐标系中，∠a和∠b均为正数，∠a的物理意义是起于x轴止于红色直线的夹角，∠b的物理意义是起于x轴止于绿色直线的夹角，那∠b-∠a得到的∠c，∠c的物理意义如何解释，显然∠c也为正数，其物理意义是起于红色直线止于绿色直线的夹角，所以是起于减数止于被减数的夹角，同样顺时针为负，逆时针为正。
编程调用库函数时一定要注意角度的单位是角度还是弧度！！！
b 旋转的欧拉角表示法
空间中的任何一种旋转都可以拆解为沿x,y,z轴的某个顺序旋转，可以拆解的顺序有12种，分别是：6种绕三条轴的旋转(也叫Tait-Bryan Angle，XYZ,XZY,YXZ,YZX,ZXY,ZYX)，另外6种只绕两条轴的旋转(也叫Proper Euler Angle，XYX,YXY,XZX,ZXZ,YZY,ZYZ)，所以欧拉角的数学表示就是：三个角度 + 一个顺序，物理意义就是按照这个顺序沿着对应坐标轴旋转对应角度。注意欧拉角表示法不唯一，有12种，所以在给出欧拉角时，必须说明旋转顺序。欧拉角表示法不区分左右手坐标系，左右手坐标系的不同体现在对旋转角度的定义里。
c 旋转的旋转矩阵表示法
欧拉角的数学表示无法直接参与数学运算，为此提出了旋转矩阵表示法，欧拉角表示法中的每一个旋转操作都对应一个旋转矩阵表示，默认所有旋转矩阵都是左乘表示，即旋转矩阵在乘号左边，尺寸是3*3，坐标点在乘号右边，尺寸是3*n，所以旋转矩阵总共有3个，分别对应沿x,y,z轴的旋转。如上图为ZXY旋转顺序计算得到的总旋转矩阵。注意总旋转矩阵计算过程与欧拉角相匹配，所以是不唯一的，但是最终总的旋转矩阵是唯一的。旋转矩阵表示法不区分左右手坐标系，左右手坐标系的不同体现在对旋转角度的定义里。
关于旋转矩阵的左乘和右乘可以参考：旋转的左乘与右乘 - 知乎
d 旋转的轴角表示法
空间中的任何一种旋转都可以归纳为沿空间中某一条三维轴旋转一个角度，所以轴角的数学表示就是：一个角度 + 一条三维轴，物理意义就是沿着这条三维轴旋转这个角度。
e 旋转的四元数表示法
类似于欧拉角，轴角表示法同样无法直接参与数学运算，为此提出了四元数表示法，四元数表示法就是用一个三维复数表示一个旋转，一般使用单位三维复数表示，三维复数存在一个实部和三个复部，四元数可以通过轴角方便求得。四元数表示法可以直接参与数学运算。
编程调用库函数时一定要注意是(w,x,y,z)格式还是(x,y,z,w)格式，python中pyquaternion库要求(w,x,y,z)格式，scipy库要求(x,y,z,w)格式。
(2) 像素坐标系与相机坐标系之间的转换
如上公式为相机的内参公式，K为内参矩阵，可以实现相机坐标到像素坐标的转换，但不能反向转换，原因是Z值未知，相机坐标系为右手坐标系，像素坐标系为左手坐标系。注意一般情况下，像素坐标系是所有坐标系里唯一的左手坐标系，左手坐标系的旋转角度与右手坐标系的旋转角度相差一个正负号，旋转矩阵的定义和计算形式不变。
在nusceness数据集中，"cam_intrinsic"为相机的内参矩阵，相机坐标系为右手坐标系。
(3) 相机坐标系与自车坐标系之间的转换
相机的位姿由旋转矩阵R和平移向量T来描述，如上为相机的外参公式，可以实现相机坐标与自车坐标之间的相互转换，R和T即为外参，有时用四维矩阵统一表示。实际应用中，由于自车并非刚体而是存在悬架，所以外参需要实时校正，相机坐标系和自车坐标系必须同为左手坐标系或右手坐标系。实际使用中，在没有特殊说明时，均默认使用右手坐标系。
在nuscenes数据集中，"cam2ego_rotation"和"cam2ego_translation"为从相机坐标系转换到自车坐标系的旋转参数和平移向量，这里旋转使用的是四元数描述法(w, x, y, z)，右手坐标系，平移量单位为m。
关于相机内参和外参的公式推导可以参考：一文带你搞懂相机内参外参(Intrinsics & Extrinsics) - 知乎
(4）三维坐标系之间的转换
除像素坐标系外，当前坐标系下的一坐标点如果想要用另一坐标系进行表示，就需要对当前坐标系的坐标轴进行旋转和平移操作，使得当前坐标系坐标轴与另一坐标系坐标轴完全重合，此时得到的新坐标即为在另一坐标系下的新坐标表示，但前提是所有坐标系全部为左手坐标系或者右手坐标系。实际使用中，没有特殊说明时，均默认使用右手坐标系。
所以一个坐标从一个坐标系转换到另一个坐标系的过程，其实就是第一个坐标系的坐标轴本身通过旋转和平移到达第二个坐标系的坐标轴的过程。但实际操作过程中，为了直接使用旋转矩阵的理论(旋转矩阵默认坐标轴不动，坐标点围着坐标轴转动)，往往采用旋转和平移坐标点，而非旋转和平移坐标轴来实现。对于旋转操作，旋转坐标轴和旋转坐标点的角度是相反的，差一个正负号，对于平移操作，平移坐标轴与平移坐标点的数值是相反的，差一个正负号。在任何一个坐标系内部，旋转/平移坐标轴与反向旋转/平移坐标点是相对运动，从该坐标系内部来看是没有任何区别的。
所以坐标在全局坐标系，自车坐标系，bev坐标系，激光雷达坐标系，毫米波雷达坐标系，等3D坐标系之间进行转换时，只需要一个旋转矩阵R和平移向量T即可实现，这些坐标系没有特殊说明时，均默认使用右手坐标系。
在nuscenes数据集中，"lidar2ego_rotation"和"lidar2ego_translation"为从激光雷达坐标系转换到自车坐标系的旋转参数和平移向量； "sensor2lidar_rotation"和"sensor2lidar_translation"为从传感器坐标系转换到激光雷达坐标系的旋转参数和平移向量；"ego2global_rotation" 和"ego2global_translation"为从自车坐标系转换到世界坐标系的旋转参数和平移向量。旋转使用的是四元数描述法(w, x, y, z)，右手坐标系，平移量单位为m。通过自车坐标系转换到世界坐标系的旋转参数和平移向量，可以直接求出自车坐标系在世界坐标系中的绝对位置和角度，在nuscenes中，不同视频不用场景的世界坐标系并不一样。
(5）bev坐标系与bev像素坐标系之间的转换
bev坐标系与bev像素坐标系采用完全同向的坐标系，只是坐标原点位置不一样而已，所以两者之间转换时，只需要平移和缩放即可，两者均为右手坐标系。
对于像素坐标系，在图片上下正常放置时，坐标系原点在图片左上角，u指向下面是行标，v指向右侧是列标，所以像素坐标系与矩阵的存储格式完全相同，只是索引相反而已，矩阵索引是(u，v)，而像素坐标是(v, u)，注意在不同的库函数里，u-v与行列的对应关系可能不同。
但对于bev像素坐标系，在bev特征图上下(对应自车前后)放置时，坐标系原点在图片左下角，x指向右侧，y指向上方，所以bev像素坐标系与矩阵的存储格式上下相反，但由于坐标系和图片同时上下相反，所以对于两个坐标系的任何一个坐标系本身来说，本质上并没有区别。但是对于某些库函数在使用时一定要先查清楚，该函数是在bev像素坐标系下进行操作还是在矩阵存储格式下进行操作，两种操作下的旋转量均相差一个正负号，但两种操作下的平移量是完全相同的。除此以外，两个坐标的索引相反，矩阵索引是(y，x)，而像素坐标是(x, y)。