Open3D上可视化Nuscenes 数据集

1、点云可视化

直接加载雷达点云数据即可

    def get_lidar(self, sample):# 获取激光雷达数据lidar_data = self.nusc.get('sample_data', sample['data']['LIDAR_TOP'])lidar_file = self.nusc.get_sample_data_path(lidar_data['token'])# 加载点云数据（NuScenes 使用 .bin 格式）#print("lidar_file: ", lidar_file)points = np.fromfile(lidar_file, dtype=np.float32).reshape(-1, 5)  # x,y,z,intensity,ring_index#points = points.reshape(-1, 5)[:, [0, 1, 2]]return points

2、标注bbox可视化

bbox在全局坐标系下标注，需要转到激光雷达坐标系下显示。

    def get_bbox(self, sample):# 获取样本中的所有标注sd_rec = self.nusc.get('sample_data', sample['data']['LIDAR_TOP'])cs_record = self.nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token'])pose_record = self.nusc.get('ego_pose', sd_rec['ego_pose_token'])annotations = sample['anns']#为每个标注创建边界框boxes = []labels = []for ann_token in annotations:ann = self.nusc.get('sample_annotation', ann_token)# print("BBox中心:", ann['translation'])# print("BBox尺寸:", ann['size'])# print("BBox旋转:", ann['rotation'])#print("类别:", ann['category_name'])# box = Box(ann['translation'], ann['size'], Quaternion(ann['rotation']))#label 映射labels.append(object_classes[official_categories[ann['category_name']]])box = self.nusc.get_box(ann_token)# pose_record：ego2global;这里需要将box从global 转到ego下，所以对平移加了负号，旋转取了逆；box.translate(-np.array(pose_record['translation']))box.rotate(Quaternion(pose_record['rotation']).inverse)#Move box to sensor coord system.box.translate(-np.array(cs_record['translation']))  # cs_record：sensor2ego;box.rotate(Quaternion(cs_record['rotation']).inverse)#转成8个角点corners = box.corners().T  # 转换为(8,3)boxes.append(corners)return boxes, labels

3、3D bbox 反投影图像可视化

需要找到激光雷达到每个相机的投影矩阵，然后进行映射。

    def show3DinImage(self, sample, bboxes, labels):caminfos = self.get_cam_info(sample)canvas = {}for cam_type, cam_info in caminfos.items():transform = self.get_lidar2image(cam_info)image_path = cam_info['data_path']images= cv2.imread(image_path)canvas[cam_type] = self.visualize_camera(images,bboxes=bboxes,labels=labels,transform=transform,classes=object_classes,)# merge imgfront_combined = cv2.hconcat([canvas['CAM_FRONT_LEFT'], canvas['CAM_FRONT'], canvas['CAM_FRONT_RIGHT']])back_combined = cv2.hconcat([canvas['CAM_BACK_LEFT'], canvas['CAM_BACK'], canvas['CAM_BACK_RIGHT']])back_combined = cv2.flip(back_combined, 1)  # 左右翻转img_combined = cv2.vconcat([front_combined, back_combined])return img_combined

4 、全部代码

main.py

from open3DVis import Open3D_visualizer
from nuscenesData import NuscenesData
import cv2
if __name__ == '__main__':nuscenes_data = NuscenesData(version ='v1.0-mini', dataroot = '/xxx/data/nuscenes/')n = 100for i in range(100):points, boxes, imgcanvas = nuscenes_data(i)cv2.namedWindow("imgcanvas", cv2.WINDOW_NORMAL)cv2.imshow("imgcanvas", imgcanvas)cv2.waitKey(0)pred_bboxes = Noneo3dvis = Open3D_visualizer(points, boxes, pred_bboxes)o3dvis.show()

open3DVis.py

import numpy as np
import open3d
class Open3D_visualizer():def __init__(self, points, gt_bboxes, pred_bboxes) -> None:self.vis = open3d.visualization.Visualizer()self.points = self.points2o3d(points)self.gt_boxes = self.box2o3d(gt_bboxes, 'red') if gt_bboxes is not None else Noneself.pred_boxes = self.box2o3d(pred_bboxes, 'green') if pred_bboxes is not None else None# 注册按键回调#self.vis.register_key_callback(key_callback)def points2o3d(self, points):# 创建 Open3D 点云对象pcd = open3d.geometry.PointCloud()pcd.points = open3d.utility.Vector3dVector(points[:, :3])  # 只使用x,y,z坐标# 可选：根据强度值着色intensity = points[:, 3]intensity_colors = np.full((points.shape[0], 3), 255)intensity_colors[:, 0] = intensity / np.max(intensity)  # 通道表示强度pcd.colors = open3d.utility.Vector3dVector(intensity_colors)return pcddef box2o3d(self, bboxes, color):"""bboxes: np.array, shape(N, 7)color: 'red' or 'green'"""# 创建Open3D线框框bbox_lines = [[0, 1], [1, 2], [2, 3], [3, 0],  # 底面[4, 5], [5, 6], [6, 7], [7, 4],  # 顶面[0, 4], [1, 5], [2, 6], [3, 7]]  # 侧面if color == 'red':colors = [[1, 0, 0] for _ in range(len(bbox_lines))]  # redelif color == 'green':colors = [[0, 1, 0] for _ in range(len(bbox_lines))]  # greenelse:print("请输入 green 或者 red。green 表示预测框，red 表示真值框。")all_bboxes = []for bbox in bboxes:#corners_3d = self.compute_box_3d(bbox[0:3], bbox[3:6], bbox[6])o3d_bbox = open3d.geometry.LineSet()o3d_bbox.points = open3d.utility.Vector3dVector(bbox)o3d_bbox.lines = open3d.utility.Vector2iVector(bbox_lines)o3d_bbox.colors = open3d.utility.Vector3dVector(colors)all_bboxes.append(o3d_bbox)return all_bboxesdef compute_box_3d(self, center, size, heading_angle):"""计算 box 的 8 个顶点坐标"""h = size[2]w = size[0]l = size[1]heading_angle = -heading_angle - np.pi / 2center[2] = center[2] + h / 2R = self.rotz(1 * heading_angle)l = l / 2w = w / 2h = h / 2x_corners = [-l, l, l, -l, -l, l, l, -l]y_corners = [w, w, -w, -w, w, w, -w, -w]z_corners = [h, h, h, h, -h, -h, -h, -h]corners_3d = np.dot(R, np.vstack([x_corners, y_corners, z_corners]))corners_3d[0, :] = center[0] + corners_3d[0, :]corners_3d[1, :] = center[1] + corners_3d[1, :]corners_3d[2, :] = center[2] + corners_3d[2, :]return np.transpose(corners_3d)def rotz(self, t):"""Rotation about the z-axis."""c = np.cos(t)s = np.sin(t)return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])def show(self):# 创建窗口self.vis.create_window(window_name="Open3D_visualizer")opt = self.vis.get_render_option()opt.point_size = 1opt.background_color = np.asarray([0, 0, 0])# 添加点云、真值框、预测框self.vis.add_geometry(self.points)if self.gt_boxes is not None:for box in self.gt_boxes:self.vis.add_geometry(box)if self.pred_boxes is not None:self.vis.add_geometry(self.pred_boxes)view_ctrl = self.vis.get_view_control()view_ctrl.set_front([0, 0, 1]) # set the positive direction of the z-axis toward youview_ctrl.set_up([0, 1, 0])   # set the positive direction of the y-axis as the up directionview_ctrl.set_lookat((0, 0, -2)) # set the original point as the center point of the windowview_ctrl.set_zoom(0.2) # zoom scaleself.vis.run()self.vis.destroy_window()

nuscenesData.py

from nuscenes.nuscenes import NuScenes
from nuscenes.utils.data_classes import Box, Quaternion
import numpy as npfrom mmdet3d.core.bbox import LiDARInstance3DBoxes
from typing import List, Optional, Tuple
import cv2
import copy
import os
OBJECT_PALETTE = {0: (255, 158, 0),1: (255, 99, 71),2: (233, 150, 70),3: (255, 69, 0),4: (255, 140, 0),5: (112, 128, 144),6: (255, 61, 99),7: (220, 20, 60),8: (0, 0, 230),9: (47, 79, 79),10: (220, 20, 60),11: (220, 200, 60),12: (100, 20, 60),
}
object_classes= {"car": 0, "truck": 1, "animal": 2, "bus": 3, "trailer": 4,"barrier": 5,"motorcycle": 6, "bicycle": 7, "pedestrian": 8,"traffic_cone": 9,"tricycle": 10,"cyclist": 11, 'unknow':12}
#构建反向索引
reverse_object_classes = {v: k for k, v in object_classes.items()}# NuScenes 官方类别 ID 映射（用于 LidarSeg 或目标检测）
official_categories = {'noise': 'unknow',                 # 噪声（无效点）'animal': "animal",'human.pedestrian.adult': "pedestrian",'human.pedestrian.child': "pedestrian",'human.pedestrian.construction_worker': "pedestrian",'human.pedestrian.police_officer': "pedestrian",'human.pedestrian.stroller': "pedestrian",'human.pedestrian.wheelchair': "pedestrian",'movable_object.barrier': "barrier",'movable_object.debris': 'unknow','movable_object.pushable_pullable': 'unknow','movable_object.trafficcone': "traffic_cone",'static_object.bicycle_rack': "barrier",'vehicle.bicycle': "bicycle",'vehicle.bus.bendy': "bus",'vehicle.bus.rigid': "bus",'vehicle.car': "car",'vehicle.construction': "truck",'vehicle.emergency.ambulance': "car",'vehicle.emergency.police': "car",'vehicle.motorcycle': "motorcycle",'vehicle.trailer': "trailer",'vehicle.truck': "truck",'flat.driveable_surface': 'unknow',  # 可驾驶平面（地面）'flat.other': 'unknow',              # 其他平面'flat.sidewalk': 'unknow','flat.terrain': 'unknow','static.manmade': 'unknow',          # 人造物体（建筑、电线杆）'static.other': 'unknow',            # 其他静态物体'static.vegetation': 'unknow',        # 植被'vehicle.ego': 'unknow'              # 自车（Ego Vehicle）
}
class NuscenesData():# 初始化 NuScenes 数据集def __init__(self, version ='v1.0-mini', dataroot = '/path/to/nuscenes'):self.nusc = NuScenes(version, dataroot, verbose=True)self.dataroot = datarootself.camera_types = ['CAM_FRONT','CAM_FRONT_RIGHT','CAM_FRONT_LEFT','CAM_BACK','CAM_BACK_LEFT','CAM_BACK_RIGHT',]def __call__(self, n = 0):sample = self.nusc.sample[n]points = self.get_lidar(sample)bboxes, labels = self.get_bbox(sample)canvas = self.show3DinImage(sample, bboxes, labels)return points, bboxes, canvasdef get_cam_info(self, sample):sd_rec = self.nusc.get('sample_data', sample['data']['LIDAR_TOP'])cs_record = self.nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token'])pose_record = self.nusc.get('ego_pose', sd_rec['ego_pose_token'])info = {'cams': dict(),'lidar2ego_translation': cs_record['translation'],'lidar2ego_rotation': cs_record['rotation'],'ego2global_translation': pose_record['translation'],'ego2global_rotation': pose_record['rotation'],}l2e_r = info['lidar2ego_rotation']l2e_t = info['lidar2ego_translation']e2g_r = info['ego2global_rotation']e2g_t = info['ego2global_translation']l2e_r_mat = Quaternion(l2e_r).rotation_matrixe2g_r_mat = Quaternion(e2g_r).rotation_matrixfor cam in self.camera_types:cam_token = sample['data'][cam]cam_path, _, cam_intrinsic = self.nusc.get_sample_data(cam_token)cam_info = self.obtain_sensor2top(cam_token, l2e_t, l2e_r_mat,e2g_t, e2g_r_mat, cam)cam_info.update(cam_intrinsic=cam_intrinsic)info['cams'].update({cam: cam_info})#print("info['cams']: ", info['cams'])return info['cams']def get_lidar2image(self, camera_info):lidar2camera_r = np.linalg.inv(camera_info["sensor2lidar_rotation"])lidar2camera_t = (camera_info["sensor2lidar_translation"] @ lidar2camera_r.T)lidar2camera_rt = np.eye(4).astype(np.float32)lidar2camera_rt[:3, :3] = lidar2camera_r.Tlidar2camera_rt[3, :3] = -lidar2camera_tcamera_intrinsics = np.eye(4).astype(np.float32)camera_intrinsics[:3, :3] = camera_info["cam_intrinsic"]lidar2image = camera_intrinsics @ lidar2camera_rt.Treturn lidar2imagedef get_bbox(self, sample):# 获取样本中的所有标注sd_rec = self.nusc.get('sample_data', sample['data']['LIDAR_TOP'])cs_record = self.nusc.get('calibrated_sensor', sd_rec['calibrated_sensor_token'])pose_record = self.nusc.get('ego_pose', sd_rec['ego_pose_token'])annotations = sample['anns']#为每个标注创建边界框boxes = []labels = []for ann_token in annotations:ann = self.nusc.get('sample_annotation', ann_token)# print("BBox中心:", ann['translation'])# print("BBox尺寸:", ann['size'])# print("BBox旋转:", ann['rotation'])#print("类别:", ann['category_name'])# box = Box(ann['translation'], ann['size'], Quaternion(ann['rotation']))#label 映射labels.append(object_classes[official_categories[ann['category_name']]])box = self.nusc.get_box(ann_token)# pose_record：ego2global;这里需要将box从global 转到ego下，所以对平移加了负号，旋转取了逆；box.translate(-np.array(pose_record['translation']))box.rotate(Quaternion(pose_record['rotation']).inverse)#Move box to sensor coord system.box.translate(-np.array(cs_record['translation']))  # cs_record：sensor2ego;box.rotate(Quaternion(cs_record['rotation']).inverse)#转成8个角点corners = box.corners().T  # 转换为(8,3)boxes.append(corners)return boxes, labelsdef get_lidar(self, sample):# 获取激光雷达数据lidar_data = self.nusc.get('sample_data', sample['data']['LIDAR_TOP'])lidar_file = self.nusc.get_sample_data_path(lidar_data['token'])# 加载点云数据（NuScenes 使用 .bin 格式）#print("lidar_file: ", lidar_file)points = np.fromfile(lidar_file, dtype=np.float32).reshape(-1, 5)  # x,y,z,intensity,ring_index#points = points.reshape(-1, 5)[:, [0, 1, 2]]return pointsdef show3DinImage(self, sample, bboxes, labels):caminfos = self.get_cam_info(sample)canvas = {}for cam_type, cam_info in caminfos.items():transform = self.get_lidar2image(cam_info)image_path = cam_info['data_path']images= cv2.imread(image_path)canvas[cam_type] = self.visualize_camera(images,bboxes=bboxes,labels=labels,transform=transform,classes=object_classes,)# merge imgfront_combined = cv2.hconcat([canvas['CAM_FRONT_LEFT'], canvas['CAM_FRONT'], canvas['CAM_FRONT_RIGHT']])back_combined = cv2.hconcat([canvas['CAM_BACK_LEFT'], canvas['CAM_BACK'], canvas['CAM_BACK_RIGHT']])back_combined = cv2.flip(back_combined, 1)  # 左右翻转img_combined = cv2.vconcat([front_combined, back_combined])return img_combineddef visualize_camera(self,image: np.ndarray,bboxes: Optional[LiDARInstance3DBoxes] = None,labels: Optional[np.ndarray] = None,transform: Optional[np.ndarray] = None,classes: Optional[List[str]] = None,color: Optional[Tuple[int, int, int]] = None,thickness: float = 4,) -> None:canvas = image.copy()canvas = cv2.cvtColor(canvas, cv2.COLOR_RGB2BGR)#bboxes已经转成8角点格式if bboxes is not None and len(bboxes) > 0:#corners = bboxes.corners#num_bboxes = corners.shape[0]labels = np.array(labels)bboxes = np.array(bboxes)num_bboxes = bboxes.shape[0]coords = np.concatenate([bboxes.reshape(-1, 3), np.ones((num_bboxes * 8, 1))], axis=-1)transform = copy.deepcopy(transform).reshape(4, 4)coords = coords @ transform.Tcoords = coords.reshape(-1, 8, 4)indices = np.all(coords[..., 2] > 0, axis=1)coords = coords[indices]labels = labels[indices]indices = np.argsort(-np.min(coords[..., 2], axis=1))coords = coords[indices]labels = labels[indices]coords = coords.reshape(-1, 4)coords[:, 2] = np.clip(coords[:, 2], a_min=1e-5, a_max=1e5)coords[:, 0] /= coords[:, 2]coords[:, 1] /= coords[:, 2]coords = coords[..., :2].reshape(-1, 8, 2)for index in range(coords.shape[0]):label_id = labels[index]name = reverse_object_classes[label_id]for start, end in [(0, 1),(0, 3),(0, 4),(1, 2),(1, 5),(3, 2),(3, 7),(4, 5),(4, 7),(2, 6),(5, 6),(6, 7),]:cv2.line(canvas,coords[index, start].astype(np.int),coords[index, end].astype(np.int),color or OBJECT_PALETTE[label_id],thickness,cv2.LINE_AA,)canvas = canvas.astype(np.uint8)canvas = cv2.cvtColor(canvas, cv2.COLOR_BGR2RGB)# cv2.imshow("canvas", canvas)# cv2.waitKey(0)return canvasdef obtain_sensor2top(self,sensor_token,l2e_t,l2e_r_mat,e2g_t,e2g_r_mat,sensor_type='lidar'):"""Obtain the info with RT matric from general sensor to Top LiDAR.Args:nusc (class): Dataset class in the nuScenes dataset.sensor_token (str): Sample data token corresponding to thespecific sensor type.l2e_t (np.ndarray): Translation from lidar to ego in shape (1, 3).l2e_r_mat (np.ndarray): Rotation matrix from lidar to ego in shape (3, 3).e2g_t (np.ndarray): Translation from ego to global in shape (1, 3).e2g_r_mat (np.ndarray): Rotation matrix from ego to global in shape (3, 3).sensor_type (str, optional): Sensor to calibrate. Default: 'lidar'.Returns:sweep (dict): Sweep information after transformation."""sd_rec = self.nusc.get('sample_data', sensor_token)cs_record = self.nusc.get('calibrated_sensor',sd_rec['calibrated_sensor_token'])pose_record = self.nusc.get('ego_pose', sd_rec['ego_pose_token'])data_path = str(self.nusc.get_sample_data_path(sd_rec['token']))if os.getcwd() in data_path:  # path from lyftdataset is absolute pathdata_path = data_path.split(f'{os.getcwd()}/')[-1]  # relative pathsweep = {'data_path': data_path,'type': sensor_type,'sample_data_token': sd_rec['token'],'sensor2ego_translation': cs_record['translation'],'sensor2ego_rotation': cs_record['rotation'],'ego2global_translation': pose_record['translation'],'ego2global_rotation': pose_record['rotation'],'timestamp': sd_rec['timestamp']}l2e_r_s = sweep['sensor2ego_rotation']l2e_t_s = sweep['sensor2ego_translation']e2g_r_s = sweep['ego2global_rotation']e2g_t_s = sweep['ego2global_translation']# obtain the RT from sensor to Top LiDAR# sweep->ego->global->ego'->lidarl2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrixe2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrixR = (l2e_r_s_mat.T @ e2g_r_s_mat.T) @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)T = (l2e_t_s @ e2g_r_s_mat.T + e2g_t_s) @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T)T -= e2g_t @ (np.linalg.inv(e2g_r_mat).T @ np.linalg.inv(l2e_r_mat).T) + l2e_t @ np.linalg.inv(l2e_r_mat).Tsweep['sensor2lidar_rotation'] = R.T  # points @ R.T + Tsweep['sensor2lidar_translation'] = Treturn sweep