NuScenes数据集中的未来自车轨迹可视化技术解析

背景介绍

NuScenes数据集作为自动驾驶领域重要的开源数据集，提供了丰富的传感器数据和标注信息。在实际应用中，我们经常需要可视化自车(ego vehicle)的未来轨迹，这对于理解自动驾驶系统的行为预测和规划模块至关重要。

问题分析

在NuScenes数据集中，自车的未来轨迹信息可以通过连续采样点(sample)的ego_pose记录来获取。然而，直接将世界坐标系下的轨迹点投影到相机图像上会导致可视化结果不准确，这是因为：

1. 坐标系不匹配：ego_pose记录的是车辆在世界坐标系中的位置，而相机图像是相机坐标系下的投影
2. 缺少坐标转换：没有进行必要的坐标系转换就直接绘制

技术解决方案

正确的可视化流程应该包含以下步骤：

1. 获取未来轨迹点：通过sample的next指针遍历未来帧，收集ego_pose信息
2. 坐标系转换：将世界坐标系下的轨迹点转换到相机坐标系
3. 投影到图像平面：使用相机内参将3D点投影到2D图像

实现细节

1. 坐标系转换原理

在NuScenes中，完整的坐标转换链为： 世界坐标系 → 自车坐标系 → 相机坐标系 → 图像坐标系

需要使用以下转换矩阵：

• 世界到自车的变换矩阵：从ego_pose获取
• 自车到相机的变换矩阵：从calibrated_sensor记录获取

2. 具体实现代码

import numpy as np
from nuscenes.utils.geometry_utils import view_points

# 获取相机内参和变换矩阵
cam_data = nusc.get('sample_data', sd_rec['data']['CAM_FRONT'])
calibrated_sensor = nusc.get('calibrated_sensor', cam_data['calibrated_sensor_token'])
K = np.array(calibrated_sensor['camera_intrinsic'])

# 转换轨迹点到相机坐标系
points = np.array(sdc_fut_traj).T  # 2xN
points = np.vstack((points, np.zeros(points.shape[1])))  # 3xN (z=0)
points = np.vstack((points, np.ones(points.shape[1])))  # 齐次坐标

# 世界到自车的变换
ego_pose = nusc.get('ego_pose', cam_data['ego_pose_token'])
global_from_car = transform_matrix(ego_pose['translation'], 
                                 Quaternion(ego_pose['rotation']))

# 自车到相机的变换
car_from_sensor = transform_matrix(calibrated_sensor['translation'],
                                 Quaternion(calibrated_sensor['rotation']))

# 组合变换
trans_matrix = np.dot(np.linalg.inv(car_from_sensor), 
                     np.linalg.inv(global_from_car))

# 应用变换
transformed_points = np.dot(trans_matrix, points)

# 投影到图像
image_points = view_points(transformed_points[:3], K, normalize=True)

3. 可视化优化

对于更好的可视化效果，可以：

1. 使用不同颜色表示时间序列
2. 添加轨迹点之间的连线
3. 调整点的大小表示时间远近

plt.figure(figsize=(12, 6))
plt.imshow(img)
colors = plt.cm.jet(np.linspace(0, 1, len(image_points[0])))
for i in range(len(image_points[0])-1):
    plt.plot(image_points[0, i:i+2], image_points[1, i:i+2], 
            color=colors[i], linewidth=2)
    plt.scatter(image_points[0, i], image_points[1, i], 
               color=colors[i], s=50*(i+1))
plt.axis('off')
plt.tight_layout()

应用场景

这种可视化技术在以下场景中特别有用：

1. 自动驾驶系统调试：验证预测模块的输出
2. 数据集分析：理解自车运动模式
3. 算法评估：直观比较不同算法的预测结果

总结

在NuScenes数据集中正确可视化自车未来轨迹需要注意坐标系转换问题。通过将世界坐标系下的轨迹点转换到相机坐标系并正确投影，可以获得准确的图像可视化效果。这一技术在自动驾驶研发的多个环节都具有重要价值。