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 img
        front_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 = 100
    for 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 = None
        o3dvis = 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 None
        self.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 pcd

    def 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))]  # red
        elif color == 'green':
            colors = [[0, 1, 0] for _ in range(len(bbox_lines))]  # green
        else:
            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_bboxes

    def 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 / 2
        center[2] = center[2] + h / 2
        R = self.rotz(1 * heading_angle)
        l = l / 2
        w = w / 2
        h = h / 2
        x_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 = 1
        opt.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 you
        view_ctrl.set_up([0, 1, 0])   # set the positive direction of the y-axis as the up direction
        view_ctrl.set_lookat((0, 0, -2)) # set the original point as the center point of the window
        view_ctrl.set_zoom(0.2) # zoom scale

        self.vis.run()
        self.vis.destroy_window()

nuscenesData.py

from nuscenes.nuscenes import NuScenes
from nuscenes.utils.data_classes import Box, Quaternion
import numpy as np

from 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 = dataroot
        self.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, canvas

    def 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_matrix
        e2g_r_mat = Quaternion(e2g_r).rotation_matrix
        for 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.T
        lidar2camera_rt[3, :3] = -lidar2camera_t
        camera_intrinsics = np.eye(4).astype(np.float32)
        camera_intrinsics[:3, :3] = camera_info["cam_intrinsic"]
        lidar2image = camera_intrinsics @ lidar2camera_rt.T
        return lidar2image

    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

    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

    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 img
        front_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
    def 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.T
            coords = 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 canvas
    def 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 path
            data_path = data_path.split(f'{os.getcwd()}/')[-1]  # relative path
        sweep = {
            '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'->lidar
        l2e_r_s_mat = Quaternion(l2e_r_s).rotation_matrix
        e2g_r_s_mat = Quaternion(e2g_r_s).rotation_matrix
        R = (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).T
        sweep['sensor2lidar_rotation'] = R.T  # points @ R.T + T
        sweep['sensor2lidar_translation'] = T
        return sweep