label和velodyne数据通过mayavi在点云图上画3D标注框并可视化

公司的label和velodyne数据参考自kitti数据集格式，算法主要集成自pointpillar+second，但只做雷达数据，没有kitti中各种相机的配合，所以不需要calib文件，这时就需要通过label中的框坐标和yaw角去画3D框并可视化，原理非常简单，有借鉴，画雷达图、提取每个框8个角点的位置并画3D框。记录留存一下。

请注意，我们没有配合相机，所以没有5-8列2D图形左上右下角的xy，而且坐标系也不和kitti用相机坐标系而是用的雷达坐标系。（相机坐标系中，y方向是高度方向，以向下为正方向；z方向是汽车前进方向；前进右手边方向为x方向（车身方向），满足右手定则。激光雷达坐标系中，z方向是高度方向，x方向是汽车前进方向，前进左手边方向为y方向（车身方向），满足右手定则。）

txt格式的label文件：

需要先安装vtk然后安装mayavi。

测试文件：百度网盘链接: https://pan.baidu.com/s/1J8bxPDLrZoKhDJSDVZd3vQ 提取码: kg43

附可视化效果如下，路径读取文件夹中所有文件并可视化，窗口关闭会自动跳到下一张图。

路径修改到label文件夹和velodyne文件夹即可，源码如下： 

# -*- coding: utf-8 -*-
'''
Created on 28.07.2022
visualization lidarcloud from ground truth, must check before training
-- please pip install vtk & mayavi first,and just change datapath
@author: kianakaslanana
'''
from __future__ import division
import os
import numpy as np
import mayavi.mlab as mlab
from math import cos,sin
import pandas as pd

def draw_lidar(lidar, is_grid=False, is_top_region=True, fig=None):
    pxs=lidar[:,0]
    pys=lidar[:,1]
    pzs=lidar[:,2]
    prs=lidar[:,3]
    if fig is None: fig = mlab.figure(figure=None, bgcolor=(1,1,1), fgcolor=None, engine=None, size=(1000, 500))
    mlab.points3d(
        pxs, pys, pzs, prs,
        mode='point',
        colormap='spectral',
        figure=fig)

def draw_gt_boxes3d(gt_boxes3d, fig, color=(1,0,0), line_width=1):
    num = len(gt_boxes3d)
    for n in range(num):
        b = gt_boxes3d[n]
        for k in range(0,4):
            i,j=k,(k+1)%4
            mlab.plot3d([b[i,0], b[j,0]], [b[i,1], b[j,1]], [b[i,2], b[j,2]], color=color, tube_radius=None, line_width=line_width, figure=fig)
            i,j=k+4,(k+3)%4 + 4
            mlab.plot3d([b[i,0], b[j,0]], [b[i,1], b[j,1]], [b[i,2], b[j,2]], color=color, tube_radius=None, line_width=line_width, figure=fig)
            i,j=k,k+4
            mlab.plot3d([b[i,0], b[j,0]], [b[i,1], b[j,1]], [b[i,2], b[j,2]], color=color, tube_radius=None, line_width=line_width, figure=fig)
    mlab.view(azimuth=180,elevation=None,distance=50,focalpoint=[12.0909996 , -1.04700089, -2.03249991])

def load_kitti_label(label_path):
    label = pd.read_csv(label_path, sep=' ', header=None, index_col=None)
    label = label[label.iloc[:, 0] != "DontCare"]
    # print("label:", label)

    n = len(label)
    box_points = []

    for i in range(n):
        box = label.iloc[i]
        points = np.zeros((8, 3))

        x = np.float(box[11])
        y = np.float(box[12])
        z = np.float(box[13])
        angle = box[14]
        depth = np.float(box[8])
        width = np.float(box[9])
        lang = np.float(box[10])

        A_top = [x + lang / 2 * cos(angle) + width / 2 * sin(angle), y + lang / 2 * sin(angle) - width / 2 * cos(angle),
                 z + depth / 2]
        A_under = [x + lang / 2 * cos(angle) + width / 2 * sin(angle),
                   y + lang / 2 * sin(angle) - width / 2 * cos(angle), z - depth / 2]

        B_top = [x + lang / 2 * cos(angle) - width / 2 * sin(angle), y + width / 2 * cos(angle) + lang / 2 * sin(angle),
                 z + depth / 2]
        B_under = [x + lang / 2 * cos(angle) - width / 2 * sin(angle),
                   y + width / 2 * cos(angle) + lang / 2 * sin(angle), z - depth / 2]

        C_top = [2 * x - (x + lang / 2 * cos(angle) + width / 2 * sin(angle)),
                 2 * y - (y + lang / 2 * sin(angle) - width / 2 * cos(angle)), z + depth / 2]
        C_under = [2 * x - (x + lang / 2 * cos(angle) + width / 2 * sin(angle)),
                   2 * y - (y + lang / 2 * sin(angle) - width / 2 * cos(angle)), z - depth / 2]

        D_top = [2 * x - (x + lang / 2 * cos(angle) - width / 2 * sin(angle)),
                 2 * y - (y + width / 2 * cos(angle) + lang / 2 * sin(angle)), z + depth / 2]
        D_under = [2 * x - (x + lang / 2 * cos(angle) - width / 2 * sin(angle)),
                   2 * y - (y + width / 2 * cos(angle) + lang / 2 * sin(angle)), z - depth / 2]

        points[0] = A_top
        points[1] = B_top
        points[2] = B_under
        points[3] = A_under
        points[4] = D_top
        points[5] = C_top
        points[6] = C_under
        points[7] = D_under

        box_points.append(points)
    return np.array(box_points)

if __name__ == '__main__':
    lidar_path = os.path.join('/path_to/velodyne/')
    label_path = os.path.join('/path_to/label_2/')
    for lidars in os.listdir(lidar_path):
        lidar_file = lidar_path + str(lidars)
        firstname,lastname = lidars.split('.')
        labels = firstname + '.txt'
        if labels in os.listdir(label_path):
            label_file = label_path + str(labels)

            lidar = np.fromfile(lidar_file, dtype=np.float32)
            lidar = lidar.reshape((-1, 4))

            gt_box3d = load_kitti_label(label_file)

            fig = draw_lidar(lidar, is_grid=True, is_top_region=True)
            draw_gt_boxes3d(gt_boxes3d=gt_box3d, fig=fig)
            mlab.show()