【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)表示如下
[ u v 1 ] = [ 1 / d x 0 u 0 0 1 / d y v 0 0 0 1 ] 3 × 3 [ x y 1 ] \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \begin{bmatrix} {1/dx} & 0 & {u_0} \\ 0 & {1/dy} & {v_0} \\ 0 & 0 & 1 \\ \end{bmatrix}_{3\times 3}\begin{bmatrix} x \\ y \\ 1 \\ \end{bmatrix} ​uv1​ ​= ​1/dx00​01/dy0​u0​v0​1​ ​3×3​ ​xy1​ ​

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

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

[ x y 1 ] = [ d x 0 − u 0 d x 0 d y − v 0 d y 0 0 1 ] 3 × 3 [ u v 1 ] \begin{bmatrix} x \\ y \\ 1 \\ \end{bmatrix} = \begin{bmatrix} {dx} & 0 & {- u_0dx} \\ 0 & {dy} & {- v_0dy} \\ 0 & 0 & 1 \\ \end{bmatrix}_{3\times 3}\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} ​xy1​ ​= ​dx00​0dy0​−u0​dx−v0​dy1​ ​3×3​ ​uv1​ ​

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

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

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

K 1 = [ f 0 0 0 f 0 0 0 1 ] 3 × 3 K_1= \begin{bmatrix} {f} & 0 & 0 \\ 0 & {f} & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}_{3\times 3} K1​= ​f00​0f0​001​ ​3×3​

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

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

[ u v 1 ] = 1 z c [ f / d x 0 u 0 0 0 f / d y v 0 0 0 0 1 0 ] 3 × 4 [ x c y c z c ] \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \frac{1}{zc}\begin{bmatrix} {f/dx} & 0 & {u_0} &0 \\ 0 & {f/dy} & {v_0} &0\\ 0 & 0 & 1 & 0\\ \end{bmatrix}_{3\times 4}\begin{bmatrix} {xc} \\ {yc} \\ {zc} \\ \end{bmatrix} ​uv1​ ​=zc1​ ​f/dx00​0f/dy0​u0​v0​1​000​ ​3×4​ ​xcyczc​ ​

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)表示如下
[ x c y c z c ] = K 2 ∗ [ x w y w z w 1 ] = [ R T 0 1 ] 4 × 4 [ x w y w z w 1 ] \begin{bmatrix} {xc} \\ {yc} \\ {zc} \\ \end{bmatrix} = K_2*\begin{bmatrix} {xw} \\ {yw} \\ {zw} \\ 1 \\ \end{bmatrix} = \begin{bmatrix} R & T \\ 0 & 1 \\ \end{bmatrix}_{4\times4}\begin{bmatrix} {xw} \\ {yw} \\ {zw} \\ 1 \\ \end{bmatrix} ​xcyczc​ ​=K2​∗ ​xwywzw1​ ​=[R0​T1​]4×4​ ​xwywzw1​ ​

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

由于Global坐标系 => Camera坐标系 => 图像坐标系 => 像素坐标系，因此我们可以得到Global坐标系 => 像素坐标系的关系式。则像素坐标系下的点(u,v)可以被Global坐标系的点(xw,yw,zw)表示如下
z c [ u v 1 ] = [ 1 / d x 0 u 0 0 1 / d y v 0 0 0 1 ] 3 × 3 [ f x 0 0 0 0 f y 0 0 0 0 1 0 ] 3 × 4 [ R T 0 1 ] 4 × 4 [ x w y w z w 1 ] zc\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \begin{bmatrix} {1/dx} & 0 & {u0} \\ 0 & {1/dy} & {v0} \\ 0 & 0 & 1 \\ \end{bmatrix}_{3\times 3}\begin{bmatrix} {fx} & 0 & 0 & 0 \\ 0 & {fy} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ \end{bmatrix}_{3\times 4}\begin{bmatrix} R & T \\ 0 & 1 \\ \end{bmatrix}_{4\times 4}\begin{bmatrix} {xw} \\ {yw} \\ {zw} \\ 1 \\ \end{bmatrix} zc ​uv1​ ​= ​1/dx00​01/dy0​u0v01​ ​3×3​ ​fx00​0fy0​001​000​ ​3×4​[R0​T1​]4×4​ ​xwywzw1​ ​

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

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

[ x w y w z w 1 ] = z c ∗ K 2 − 1 K 1 − 1 [ 1 / d x 0 u 0 0 1 / d y v 0 0 0 1 ] 3 × 3 − 1 [ u v 1 ] \begin{bmatrix} {xw} \\ {yw} \\ {zw} \\ 1 \\ \end{bmatrix} =zc* {K_2}^{- 1}{K_1}^{- 1} \begin{bmatrix} {1/dx} & 0 & {u0} \\ 0 & {1/dy} & {v0} \\ 0 & 0 & 1 \\ \end{bmatrix}_{3\times 3}^{-1}\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} ​xwywzw1​ ​=zc∗K2​−1K1​−1 ​1/dx00​01/dy0​u0v01​ ​3×3−1​ ​uv1​ ​

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工具整理了下大致的思路，不是很全很深刻，如果有其他见解，可以在评论区留言。
如果喜欢的话，可以点个赞哦~