用Python和Nuscenes数据集，手把手教你搞懂自动驾驶的6大坐标系转换

用Python和Nuscenes数据集实战自动驾驶6大坐标系转换

第一次接触自动驾驶感知系统时，最让人头疼的莫过于各种坐标系之间的转换关系。记得去年参与一个多传感器融合项目时，团队花了整整两周时间调试坐标系对齐问题——雷达检测到的行人位置总是比摄像头看到的偏移两米。后来发现是某个外参矩阵的旋转顺序写反了。本文将用Nuscenes数据集和Python代码，带您亲手实现六大坐标系的相互转换，避开那些年我们踩过的坑。

1. 环境准备与数据加载

1.1 安装必要工具包

在开始前需要配置以下环境（建议使用Python 3.8+）：

pip install nuscenes-devkit matplotlib numpy opencv-python pyquaternion

注意：pyquaternion库用于处理三维旋转，比手动计算旋转矩阵更可靠

1.2 加载Nuscenes数据集样本

我们从数据集中选取一个包含相机和激光雷达数据的样本：

from nuscenes.nuscenes import NuScenes nusc = NuScenes(version='v1.0-mini', dataroot='/path/to/dataset', verbose=True) sample = nusc.sample[10] # 选取第10个样本 # 获取前视摄像头(CAM_FRONT)数据 cam_data = nusc.get('sample_data', sample['data']['CAM_FRONT']) # 获取激光雷达数据 lidar_data = nusc.get('sample_data', sample['data']['LIDAR_TOP'])

2. 坐标系定义与可视化

2.1 六大坐标系精解

在自动驾驶系统中，这些坐标系构成了感知的骨架：

2.2 使用Matplotlib可视化坐标系

让我们绘制出各坐标系的关系示意图：

import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D fig = plt.figure(figsize=(12, 8)) ax = fig.add_subplot(111, projection='3d') # 绘制坐标系箭头 def draw_axis(ax, origin, directions, color, label): for i, (dir, c) in enumerate(zip(directions, ['r', 'g', 'b'])): ax.quiver(*origin, *dir, color=c, arrow_length_ratio=0.1) ax.scatter(*origin, c=color, label=label) # 示例坐标系（实际应用中需替换为真实外参） draw_axis(ax, [0,0,0], [[1,0,0], [0,1,0], [0,0,1]], 'k', 'World') draw_axis(ax, [2,2,2], [[0.7,0.7,0], [-0.7,0.7,0], [0,0,1]], 'b', 'Camera') draw_axis(ax, [3,0,1], [[0.8,0,0.6], [0,1,0], [-0.6,0,0.8]], 'r', 'Lidar') ax.set_xlim([0, 5]); ax.set_ylim([0, 5]); ax.set_zlim([0, 5]) ax.set_xlabel('X'); ax.set_ylabel('Y'); ax.set_zlabel('Z') ax.legend(); plt.tight_layout(); plt.show()

3. 核心转换实战

3.1 相机内参：从3D到2D的桥梁

相机内参矩阵K将相机坐标系下的3D点投影到2D图像平面：

import numpy as np # 从Nuscenes获取相机内参 cam_calib = nusc.get('calibrated_sensor', cam_data['calibrated_sensor_token']) K = np.array(cam_calib['camera_intrinsic']) # 3x3内参矩阵 def project_3d_to_2d(points_3d, K): """ 将相机坐标系下的3D点投影到像素坐标系 """ points_2d = K @ points_3d.T points_2d = points_2d / points_2d[2, :] # 齐次坐标归一化 return points_2d[:2].T # 测试投影 test_point = np.array([10, 0, 50, 1]) # 相机前方50米，右侧10米 uv = project_3d_to_2d(test_point.reshape(1,-1), K) print(f"投影像素坐标: {uv.squeeze()}")

3.2 外参矩阵：坐标系间的空间关系

外参矩阵描述传感器之间的相对位姿：

# 获取相机到车体的变换矩阵 cam2ego = np.eye(4) cam2ego[:3, :3] = np.array(cam_calib['rotation']) cam2ego[:3, 3] = np.array(cam_calib['translation']) # 获取激光雷达到车体的变换 lidar_calib = nusc.get('calibrated_sensor', lidar_data['calibrated_sensor_token']) lidar2ego = np.eye(4) lidar2ego[:3, :3] = np.array(lidar_calib['rotation']) lidar2ego[:3, 3] = np.array(lidar_calib['translation']) # 计算相机到激光雷达的变换 cam2lidar = np.linalg.inv(lidar2ego) @ cam2ego

3.3 完整转换链路示例

实现世界坐标系到像素坐标系的完整转换：

def world_to_pixel(point_world, cam_data, nusc): """ 世界坐标 -> 像素坐标 """ # 获取各变换矩阵 calib = nusc.get('calibrated_sensor', cam_data['calibrated_sensor_token']) K = np.array(calib['camera_intrinsic']) ego2global = nusc.get('ego_pose', cam_data['ego_pose_token']) # 构建变换矩阵 world2ego = np.eye(4) world2ego[:3, :3] = Quaternion(ego2global['rotation']).rotation_matrix world2ego[:3, 3] = np.array(ego2global['translation']) ego2cam = np.linalg.inv(np.eye(4)) ego2cam[:3, :3] = np.array(calib['rotation']) ego2cam[:3, 3] = np.array(calib['translation']) # 坐标变换 point_cam = ego2cam @ np.linalg.inv(world2ego) @ point_world uv = K @ point_cam[:3] uv /= uv[2] return uv[:2] # 使用示例 point_world = np.array([20, 5, 0, 1]) # 齐次坐标 uv = world_to_pixel(point_world, cam_data, nusc)

4. 典型问题排查指南

4.1 常见错误排查表

4.2 调试技巧

1. 可视化中间结果：在每个转换步骤后输出并可视化坐标值

   print(f"Camera坐标系下的点: {point_cam}")

2. 使用已知几何体验证：

   # 创建立方体点云验证投影 cube_points = np.array([[0,0,0], [1,0,0], [0,1,0], [0,0,1], [1,1,0], [1,0,1], [0,1,1], [1,1,1]])

3. 检查矩阵可逆性：

   assert np.linalg.det(rotation_matrix) ≈ 1.0, "旋转矩阵行列式应为1"

在真实项目中，坐标系对齐问题往往需要结合标定板数据反复验证。最近我们在处理一个雨天场景时发现，激光雷达和相机的坐标转换在50米外会出现厘米级偏差，后来发现是温度变化导致雷达支架轻微变形所致。这类问题没有银弹，扎实的基础加细致的调试才是王道。