KITTIBin点云可视化+转PCD

# 将bin+label转为xyzrgb格式的PCD文件

import numpy as np
import logging
import open3d as o3d

logging.basicConfig(format='%(asctime)s.%(msecs)03d [%(levelname)s] [%(filename)s:%(lineno)d] %(message)s',
                    datefmt='## %Y-%m-%d %H:%M:%S')

logging.getLogger().setLevel(logging.DEBUG)
logger = logging.getLogger()


label_name_mapping = {
    0: 'unlabeled',
    1: 'outlier',
    10: 'car',
    11: 'bicycle',
    13: 'bus',
    15: 'motorcycle',
    16: 'on-rails',
    18: 'truck',
    20: 'other-vehicle',
    30: 'person',
    31: 'bicyclist',
    32: 'motorcyclist',
    40: 'road',
    44: 'parking',
    48: 'sidewalk',
    49: 'other-ground',
    50: 'building',
    51: 'fence',
    52: 'other-structure',
    60: 'lane-marking',
    70: 'vegetation',
    71: 'trunk',
    72: 'terrain',
    80: 'pole',
    81: 'traffic-sign',
    99: 'other-object',
    252: 'moving-car',
    253: 'moving-bicyclist',
    254: 'moving-person',
    255: 'moving-motorcyclist',
    256: 'moving-on-rails',
    257: 'moving-bus',
    258: 'moving-truck',
    259: 'moving-other-vehicle'
}

kept_labels = [
    'road', 'sidewalk', 'parking', 'other-ground', 'building', 'car', 'truck',
    'bicycle', 'motorcycle', 'other-vehicle', 'vegetation', 'trunk', 'terrain',
    'person', 'bicyclist', 'motorcyclist', 'fence', 'pole', 'traffic-sign'
]


cmap = np.array([
    [245, 150, 100, 255],
    [245, 230, 100, 255],
    [150, 60, 30, 255],
    [180, 30, 80, 255],
    [255, 0, 0, 255],
    [30, 30, 255, 255],
    [200, 40, 255, 255],
    [90, 30, 150, 255],
    [255, 0, 255, 255],
    [255, 150, 255, 255],
    [75, 0, 75, 255],
    [75, 0, 175, 255],
    [0, 200, 255, 255],
    [50, 120, 255, 255],
    [0, 175, 0, 255],
    [0, 60, 135, 255],
    [80, 240, 150, 255],
    [150, 240, 255, 255],
    [0, 0, 255, 255],
])
cmap = cmap[:, [2, 1, 0, 3]]  # convert bgra to rgba

class BinConvertor:
    def __init__(self, bin_name, label_name):
        self.bin_name = bin_name
        self.label_name = label_name

        self.points = np.zeros((0, 3), dtype=np.float32)

        self.sem_label = np.zeros((0, 1), dtype=np.uint32)  # [m, 1]: label
        self.sem_label_color = np.zeros((0, 3), dtype=np.float32)  # [m ,3]: color

        # label map
        reverse_label_name_mapping = {}
        self.label_map = np.zeros(260)
        cnt = 0
        for label_id in label_name_mapping:
            if label_id > 250:
                if label_name_mapping[label_id].replace('moving-',
                                                        '') in kept_labels:
                    self.label_map[label_id] = reverse_label_name_mapping[
                        label_name_mapping[label_id].replace('moving-', '')]
                else:
                    self.label_map[label_id] = 255
            elif label_id == 0:
                self.label_map[label_id] = 255
            else:
                if label_name_mapping[label_id] in kept_labels:
                    self.label_map[label_id] = cnt
                    reverse_label_name_mapping[
                        label_name_mapping[label_id]] = cnt
                    cnt += 1
                else:
                    self.label_map[label_id] = 255
        self.reverse_label_name_mapping = reverse_label_name_mapping

    def read_bin(self):
        scan = np.fromfile(self.bin_name, dtype=np.float32)
        scan = scan.reshape((-1, 4))
        self.points = scan[:,:3]

    def read_label(self):
        label = np.fromfile(self.label_name, dtype=np.uint32)
        label = label.reshape((-1))
        self.sem_label = label & 0xFFFF
        assert self.points.shape[0] == self.sem_label.shape[0]
        self.sem_label = self.label_map[self.sem_label].astype(np.int64)


    def set_cloud(self):
        # make color table
        color_dict = {}
        for i in range(19):
            color_dict[i] = cmap[i, :]
        color_dict[255] = [0,0,0,255]

        pc = o3d.geometry.PointCloud()
        pc.points = o3d.utility.Vector3dVector(self.points)

        cloud_color = [color_dict[i] for i in list(self.sem_label)]
        self.sem_label_color = np.array(cloud_color).reshape((-1,4))[:,:3] / 255

        pc.colors = o3d.utility.Vector3dVector(self.sem_label_color)

        o3d.visualization.draw_geometries([pc])

        output_path = "F:\git\qt_visualizer_pcl\qt_visualizer\data\\000005.pcd"
        o3d.io.write_point_cloud(output_path, pc)
        logger.info("===> point cloud save to {}".format(output_path))

if __name__ == '__main__':
    bin_name = "F:\git\qt_visualizer_pcl\qt_visualizer\data\\velodyne\\000005.bin"
    label_name = "F:\git\qt_visualizer_pcl\qt_visualizer\data\labels\\000005.label"
    bc = BinConvertor(bin_name, label_name)
    bc.read_bin()
    bc.read_label()
    bc.set_cloud()