【KnowledgeBase】一份较为全面的自动驾驶各坐标系介绍

---

前言

提示：本篇文章非专业人士所作，内容仅可作为入门，如果喜欢，请点赞~

介绍了一下自动驾驶领域的各个坐标系的知识。

• 首先，介绍多种坐标系的定义。
• 其次，介绍各个坐标系之间的转换过程。

---

一、坐标系种类

坐标系按照维度分，可以分为2种：

• 2D坐标系；
• 3D坐标系。

按照种类分，可以分为多种：

1. 像素坐标系；
2. 图像坐标系；
3. Camera坐标系，也称相机坐标系；
4. Lidar坐标系，也称激光雷达坐标系；
5. Ego坐标系，也称车自身坐标系；
6. Global坐标系，也称世界坐标系；
7. Radar坐标系（本篇文章不讨论）。

---

二、各种坐标系的定义

1.像素坐标系

像素坐标系（Pixel Coordinate System）是图像处理中最基本的坐标系，是2D坐标系，使用像素坐标来表示图像中每个像素点的位置。与CV图像处理的坐标系一致

• 原点$O(0,0)$为整个图像的左上角点；
• u轴正向水平向右，v轴正向竖直向下；
• 坐标系的像素位置用(u,v)来表示。

2.图像坐标系

图像坐标系（Image Coordinate System）是基于像素坐标系的扩展，是2D坐标系，用来表示图像中像素点的物理位置。

• 原点$O_{IMG}(0,0)$为像素坐标系中的图像的中点位置；
  
• Y轴正向水平向右，Y轴正向竖直向下；
• 坐标系的位置用(x,y)来表示。

3.Camera坐标系

Camera坐标系（Camera Coordinate System）相机坐标系是相机（摄像机）内部的坐标系，是3D坐标系，用来表示相机成像时的物体位置，用于描述相机看到的三维世界。

• Camera坐标系的原点为光心（光学中心）；
• 以Nuscenes数据集为例，X轴正向指向相机右侧，Y轴正向指向相机的下方，Z轴正向指向相机的光轴（光学轴）方向；

这里需要注意一点，就是Nuscenes数据集中，另外的5个Camera的坐标系都会被转换到Camera0位置的坐标系上进行统一，具体代码自己发掘下，我记得是这样。

• 坐标系的位置用(xc,yc,zc)来表示。

4.Lidar坐标系

Lidar坐标系（Lidar Coordinate System）是激光雷达感知系统内部的坐标系，是3D坐标系，用来表示Lidar扫描时物体的位置，也即描述Lidar看到的三维世界。

• LiDAR坐标系的原点通常位于Lidar传感器本身的位置；
• 以Nuscenes数据集为例，X轴正向指向传感器的右侧，Y轴正向指向传感器的正前，Z轴正向指向传感器的上方；
• 坐标系的位置用(xl,yl,zl)来表示。

5.Ego坐标系

Ego坐标系（Egocentric coordinate system），在自动驾驶中是以车辆为参考物体建立的坐标系，也称为本体坐标系或局部坐标系。

• 原点通常是车辆的中心点，与车辆或机器人的运动方向相关；
• X轴通常指向车辆的前进方向，Y轴指向车辆的左侧，Z轴垂直于地面。

6.Global坐标系

Global坐标系，也被称为世界坐标系（World coordinate system），在自动驾驶领域，Global坐标系用于建立一个全局参考框架，以描述车辆在地图或整个环境中的绝对位置和方向。

• 原点是一个被选定的固定点；
• 坐标系的位置用(xw,yw,zw)来表示。

---

三、各种坐标系之间的转换

主要就看下面这张图的转换关系来理解吧！
其中的圆圈123...与本节的转换内容小章节编号对应。

1.图像坐标系 => 像素坐标系

假设物理坐标系的单位为毫米，dx、dy的单位都为毫米/像素。

根据上图的关系，假设图像坐标系原点$O_{IMG}$在像素坐标系中的位置为(u_0,v_0)。则像素坐标系中的点(u,v)可以通过一个3x3矩阵被图像坐标系中的点(x,y)表示如下
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]={\left[\begin{array}{ccc}1/dx& 0& u_0\\ 0& 1/dy& v_0\\ 0& 0& 1\end{array}\right]}_{3\times 3}\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

2.像素坐标系 => 图像坐标系

根据上面的结论，可以通过3x3矩阵的逆将像素坐标系中的点(u,v)用图像坐标系中的点(x,y)表示，如下

$$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]={\left[\begin{array}{ccc}dx& 0& -u_{0}dx\\ 0& dy& -v_{0}dy\\ 0& 0& 1\end{array}\right]}_{3\times 3}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$

3.Camera坐标系 => 图像坐标系

Camera坐标系是3D坐标系，而图像坐标系是2D坐标系，根据上图逻辑与相似三角形原理，f表示相机的焦距，假设相机坐标系中的点为(xc,yc,zc)，则图像坐标系的点(x,y)可以被表示为
$$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\frac{1}{zc}{\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]}_{3\times 4}\left[\begin{array}{c}xc\\ yc\\ zc\\ 1\end{array}\right]$$
其中，这个3x4矩阵是相机的内参矩阵$K_1$，其也可以写成3x3的形式，写成3x4是为了齐次。

相机的内参矩阵（Camera Intrinsic Matrix）是用来描述相机成像的内部几何特性。

$$K_1={\left[\begin{array}{ccc}f& 0& 0\\ 0& f& 0\\ 0& 0& 1\end{array}\right]}_{3\times 3}$$

4.Camera坐标系 => 像素坐标系

由于Camera坐标系 => 图像坐标系 => 像素坐标系，因此我们可以得到Camera坐标系 => 像素坐标系的关系式。像素坐标系的点(u,v)可以被Camera坐标系的点(xc,yc,zc)表示如下

$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\frac{1}{zc}{\left[\begin{array}{cccc}f/dx& 0& u_0& 0\\ 0& f/dy& v_0& 0\\ 0& 0& 1& 0\end{array}\right]}_{3\times 4}\left[\begin{array}{c}xc\\ yc\\ zc\end{array}\right]$$

5.Global坐标系 => Camera坐标系

Global坐标系与Camera坐标系的转换，需要用到相机的外参矩阵$K_2$，是一个4x4矩阵。

相机的外参矩阵（Camera Extrinsic Matrix）是用来描述相机在世界坐标系中的位置和方向的矩阵。表示为
$$K_2={\left[\begin{array}{cc}{R}_{3\times 3}& T_{3\times 1}\\ 0_{1\times 3}& 1\end{array}\right]}_{4\times 4}$$
其中R是一个3x3矩阵，表示相机坐标系相对于世界坐标系的旋转关系；T是一个3x1向量，表示相机坐标系相对于世界坐标系的平移关系。

则Camera坐标系下的点(xc,yc,zc)可以被Global坐标系的点(xw,yw,zw)表示如下
$$\left[\begin{array}{c}xc\\ yc\\ zc\end{array}\right]=K_2\ast \left[\begin{array}{c}xw\\ yw\\ zw\\ 1\end{array}\right]={\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]}_{4\times 4}\left[\begin{array}{c}xw\\ yw\\ zw\\ 1\end{array}\right]$$

6.Global坐标系 => 像素坐标系

由于Global坐标系 => Camera坐标系 => 图像坐标系 => 像素坐标系，因此我们可以得到Global坐标系 => 像素坐标系的关系式。则像素坐标系下的点(u,v)可以被Global坐标系的点(xw,yw,zw)表示如下
$$zc\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]={\left[\begin{array}{ccc}1/dx& 0& u0\\ 0& 1/dy& v0\\ 0& 0& 1\end{array}\right]}_{3\times 3}{\left[\begin{array}{cccc}fx& 0& 0& 0\\ 0& fy& 0& 0\\ 0& 0& 1& 0\end{array}\right]}_{3\times 4}{\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]}_{4\times 4}\left[\begin{array}{c}xw\\ yw\\ zw\\ 1\end{array}\right]$$

7.像素坐标系 => Global坐标系

反之，Global坐标系的点(xw,yw,zw)可以被像素坐标系下的点(u,v)表示如下

$$\left[\begin{array}{c}xw\\ yw\\ zw\\ 1\end{array}\right]=zc\ast {K_2}^{-1}{K_1}^{-1}{\left[\begin{array}{ccc}1/dx& 0& u0\\ 0& 1/dy& v0\\ 0& 0& 1\end{array}\right]}_{3\times 3}^{-1}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$

8.Camera坐标系 => Lidar坐标系

根据MMDetection3D的mmdet3d/datasets/nuscenes_dataset.py代码，

if self.modality['use_camera']:
	image_paths = []
	lidar2img_rts = [] # 用来保存6个lidar2img_rt矩阵的（因为有6个相机）
	camera2lidar_rts = []  # 用来保存6个camera2lidar矩阵
	# 为每一个相机cam生成了lidar2img_rt矩阵与camera2lidar矩阵
	for cam_type, cam_info in info['cams'].items():
		image_paths.append(cam_info['data_path'])
		# obtain lidar to image transformation matrix 3
		lidar2cam_r = np.linalg.inv(cam_info['sensor2lidar_rotation'])
		lidar2cam_t = cam_info[
		    'sensor2lidar_translation'] @ lidar2cam_r.T
		lidar2cam_rt = np.eye(4)
		lidar2cam_rt[:3, :3] = lidar2cam_r.T
		lidar2cam_rt[3, :3] = -lidar2cam_t
		intrinsic = cam_info['cam_intrinsic']
		viewpad = np.eye(4)
		viewpad[:intrinsic.shape[0], :intrinsic.shape[1]] = intrinsic
		lidar2img_rt = (viewpad @ lidar2cam_rt.T)
		lidar2img_rts.append(lidar2img_rt)
		
		# camera to lidar transform
		camera2lidar = np.eye(4).astype(np.float32)   # 4x4
		camera2lidar[:3, :3] = cam_info["sensor2lidar_rotation"]  # 旋转3x3矩阵
		camera2lidar[:3, 3] = cam_info["sensor2lidar_translation"] # 平移3x1向量
		camera2lidar_rts.append(camera2lidar)
	
	input_dict.update(
		dict(
		    img_filename=image_paths,
		    lidar2img=lidar2img_rts,
		    camera2lidar=camera2lidar_rts,
		))

在Nuscenes数据集的处理代码中，可以看到：camera2lidar是一个4x4矩阵，其将Camera坐标系下的点(xc, yc,zc,1)转化到Lidar坐标系中的点(xl,yl,zl,1)。

至此，所有的转换线路都是已知。

---

总结

很浅显的跟着Chat工具整理了下大致的思路，不是很全很深刻，如果有其他见解，可以在评论区留言。
如果喜欢的话，可以点个赞哦~