自动驾驶坐标变换以及NuSences数据集解析

进阶的CVCoder

已于 2024-05-02 20:23:30 修改

阅读量1.3k

点赞数 3

分类专栏：小白自动驾驶感知算法学习之路文章标签：自动驾驶人工智能机器学习

于 2023-10-28 19:21:24 首次发布

本文链接：https://blog.csdn.net/qq_43547049/article/details/134095029

版权

小白自动驾驶感知算法学习之路专栏收录该内容

3 篇文章 0 订阅

订阅专栏

先简单过一遍相机模型中的坐标变换。

世界坐标系 => 图像像素坐标系

一点点概念：

外参：物理空间中不同位置的坐标系之间的坐标变换，涉及原点的平移对齐和坐标轴的旋转对齐

推导过程：

旋转矩阵推导

三维坐标旋转矩阵推导过程（包看懂）_三维旋转矩阵_xiaohai@Linux的博客-CSDN博客

内参：是诸如相机等传感器内部，由于其本身的特性，导致获得的数据与真实世界有一定的对应关系，为了充分利用不同传感器的数据，需要尽可能将它们统一到相同的格式、坐标系下。

从相机坐标系到图像坐标系，属于透视投影关系，从3D转换到2D。也可以看成是针孔模型的改变模型。满足三角形相似定理。

图像物理坐标系到像素坐标系此时没有旋转变换，但是坐标原点位置不一致，大小不一致，则设计伸缩变换及平移变换。

其中相机的内参和外参可以通过张正友标定获取。

自动驾驶中的坐标系

自动驾驶中的坐标系主要分为全局坐标系、车身坐标系、相机坐标系和激光坐标系。后面三个比较好理解，都是相对坐标系，目标的位置随本车的运动而变化；而全局坐标系是绝对坐标系，是目标在地图中的绝对坐标，不随本车的运动而变化。

NuScenes坐标系转换

nuScenes中表与表之间的关系可以归纳如下：

总的来说，nuScenes数据集分为mini、trainval、test三个部分，每个部分的数据结构完全相同，可以分成scene、sample、sample_data三个层级，数据访问通过token（可以理解为指针）来实现：

scene：是一段约20s的视频片段，由于关键帧采样频率为2Hz，所以每个scene大约包含40个关键帧，可以通过scene中的pre和next来访问上下相邻的sample
sample：对应着一个关键帧数据，存储了相机、激光雷达、毫米波雷达的token信息，mini和trainval数据集中的sample还存储了标注信息的token
sample_data：sample中存储的token指向的数据，即我们最终真正关心的信息，比如图片路径、位姿数据、传感器标定结果、标注目标的3d信息等。获取到这些信息就可以开始训练模型了。

遍历数据集

from nuscenes.nuscenes import NuScenes
def get_dataset_info1(nusc):
    scene_num = len(nusc.scene)
    sample_num = 0
    ann_num = 0
    for scene in nusc.scene:
        sample = None
        while True:
            if sample is None:
                sample = nusc.get('sample', scene['first_sample_token'])
            sample_num += 1
            ann_num += len(sample['anns'])
    print('Scene Num: %d\nSample Num: %d\nAnnotation Num: %d' % (scene_num, sample_num, ann_num))

if __name__ == '__main__':
    nusc = NuScenes(version='v1.0-trainval',
                    dataroot='./data/nuscenes',
                    verbose=True)

获取数据

# 1. 激光数据
lidar_file = nusc.get('sample_data', sample['data']['LIDAR_TOP'])
# 2. 相机数据
camera_file = dict()
for key in sample['data']:
	if key.startswith('CAM'):
    	sample_data = nusc.get('sample_data', sample['data'][key])
    	camera_file[sample_data['channel']] = sample_data
# 3. ann
for token in sample['anns']:
    anns_data = nusc.get('sample_annotation', token)
    anns_data['data_type'] = 'result'
    anns_info.append(anns_data)
"""
目标的标注信息包含如下几个字段：
translation: 3D框的中心位置(x,y,z)，单位m，是全局坐标系下的坐标
rotation: 3D框的旋转量，用四元数(w,x,y,z)表示
size: 3D框的尺寸(w,l,h)，单位米
category_name: 类别名称，包含10个检测类别
"""

坐标变换

nuScenes存在四个坐标系：全局坐标系、车身坐标系、相机坐标系和激光坐标系。标注真值的坐标是全局坐标系下的坐标。各坐标系的转换关系如下图所示，所有转换都必须先转到车身坐标系(ego vehicle frame)，然后再转换到目标坐标系。需要注意的是，由于每张图像的时间戳、激光的时间戳都两两不相同，它们有各自的位姿补偿（ego data），进行坐标系转换的时候需要注意一下。

1、标注真值(global frame)转换到激光坐标系(lidar frame)：使用位姿补偿转换到车身坐标系，然后再根据激光雷达外参转换到激光坐标系。

# 标注真值到激光坐标系
ann = nusc.get('sample_annotation', token)
calib_data = nusc.get('calibrated_sensor', lidar_data['calibrated_sensor_token'])
ego_data = nusc.get('ego_pose', lidar_data['ego_pose_token'])
# global frame
center = np.array(ann['translation'])
orientation = np.array(ann['rotation'])
# 从global frame转换到ego vehicle frame
quaternion = Quaternion(ego_data['rotation']).inverse
center -= np.array(ego_data['translation'])
center = np.dot(quaternion.rotation_matrix, center)
orientation = quaternion * orientation
# 从ego vehicle frame转换到sensor frame
quaternion = Quaternion(calib_data['rotation']).inverse
center -= np.array(calib_data['translation'])
center = np.dot(quaternion.rotation_matrix, center)
orientation = quaternion * orientation

得到激光雷达的“ego_pose”的参数，该ego_pose就是将ego坐标转到global坐标的。因此，求其逆，将global坐标转到ego坐标；然后，通过lidar的calibrated矩阵（将传感器坐标转到ego坐标），求其逆，将ego坐标转到lidar传感器坐标。

2、激光真值(lidar frame)投影至图像(pixel coord)就相对麻烦一点，因为图像和激光时间戳不一致，需要多进行一步时间戳的变换。

# step1: lidar frame -> ego frame
calib_data = nusc.get('calibrated_sensor', lidar_file['calibrated_sensor_token'])
rot_matrix = Quaternion(calib_data['rotation']).rotation_matrix
points[:3, :] = np.dot(rot_matrix, points[:3, :])
for i in range(3):
    points[i, :] += calib_data['translation'][i]

# step2: ego frame -> global frame
ego_data = nusc.get('ego_pose', lidar_file['ego_pose_token'])
rot_matrix = Quaternion(ego_data['rotation']).rotation_matrix
points[:3, :] = np.dot(rot_matrix, points[:3, :])
for i in range(3):
    points[i, :] += ego_data['translation'][i]

# step3: global frame -> ego frame
ego_data = nusc.get('ego_pose', camera_data['ego_pose_token'])
for i in range(3):
    points[i, :] -= ego_data['translation'][i]
rot_matrix = Quaternion(ego_data['rotation']).rotation_matrix.T
points[:3, :] = np.dot(rot_matrix, points[:3, :])

# step4: ego frame -> cam frame
calib_data = nusc.get('calibrated_sensor', camera_data['calibrated_sensor_token'])
for i in range(3):
    points[i, :] -= calib_data['translation'][i]
rot_matrix = Quaternion(calib_data['rotation']).rotation_matrix.T
points[:3, :] = np.dot(rot_matrix, points[:3, :])

# step5: cam frame -> uv pixel
visible = points[2, :] > 0.1
colors = get_rgb_by_distance(points[2, :], min_val=0, max_val=50)
intrinsic = calib_data['camera_intrinsic']
trans_mat = np.eye(4)
trans_mat[:3, :3] = np.array(intrinsic)
points = np.concatenate((points[:3, :], np.ones((1, points.shape[1]))), axis=0)
points = np.dot(trans_mat, points)[:3, :]
points /= points[2, :]
points = points[:2, :]

3、标注框中的box可视化：

#把全局坐标上的中心点坐标转化为3D框
# 2. 3D框
# global frame
center = np.array(ann['translation'])
orientation = np.array(ann['rotation'])
# 从global frame转换到ego vehicle frame
quaternion = Quaternion(ego_data['rotation']).inverse
center -= np.array(ego_data['translation'])
center = np.dot(quaternion.rotation_matrix, center)
orientation = quaternion * orientation
# 从ego vehicle frame转换到sensor frame
quaternion = Quaternion(calib_data['rotation']).inverse
center -= np.array(calib_data['translation'])
center = np.dot(quaternion.rotation_matrix, center)
orientation = quaternion * orientation
# 根据中心点和旋转量生成3D框
x, y, z = center
w, l, h = ann['size']
x_corners = l / 2 * np.array([-1, 1, 1, -1, -1, 1, 1, -1])
y_corners = w / 2 * np.array([1, 1, -1, -1, 1, 1, -1, -1])
z_corners = h / 2 * np.array([-1, -1, -1, -1, 1, 1, 1, 1])
# 初始中心为(0, 0, 0)
box3d = np.vstack((x_corners, y_corners, z_corners))
# 旋转3D框
box3d = np.dot(orientation.rotation_matrix, box3d)
# 平移3D框
box3d[0, :] = box3d[0, :] + x
box3d[1, :] = box3d[1, :] + y
box3d[2, :] = box3d[2, :] + z

# plot
# 相机图像
box = box.astype(np.int)  # shape (3, 8)
for i in range(4):
    j = (i + 1) % 4
    # 下底面
    cv2.line(camera_img, (box[0, i], box[1, i]), (box[0, j], box[1, j]), color, thickness=thickness[1])
    # 上底面
    cv2.line(camera_img, (box[0, i + 4], box[1, i + 4]), (box[0, j + 4], box[1, j + 4]), color, thickness=thickness[1])
    # 侧边线
    cv2.line(camera_img, (box[0, i], box[1, i]), (box[0, i + 4], box[1, i + 4]), color, thickness=thickness[1])

参考：
nuScenes 3D目标检测数据集解析（完整版附python代码）

进阶的CVCoder

关注

3
点赞
踩
11

收藏

觉得还不错? 一键收藏
3
评论
自动驾驶坐标变换以及NuSences数据集解析

先简单过一遍相机模型中的坐标变换。一点点概念：外参：物理空间中不同位置的坐标系之间的坐标变换，涉及原点的平移对齐和坐标轴的旋转对齐内参：是诸如相机等传感器内部，由于其本身的特性，导致获得的数据与真实世界有一定的对应关系，为了充分利用不同传感器的数据，需要尽可能将它们统一到相同的格式、坐标系下。从相机坐标系到图像坐标系，属于透视投影关系，从3D转换到2D。也可以看成是针孔模型的改变模型。满足三角形相似定理。
复制链接

扫一扫