人工智能 机器学习 自动驾驶坐标变换以及NuSences数据集解析

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

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

一点点概念：

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

推导过程：

旋转矩阵推导

三维坐标旋转矩阵推导过程（包看懂）_三维旋转矩阵_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框

# 根据中心点和旋转量生成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代码）

文章链接

 您阅读本篇文章共花了：