DAIR-V2X点云可视化以及添加3D框

1 单个点云可视化

1.1 代码

import open3d as o3d
import numpy as np

#读取数据
pcd = o3d.io.read_point_cloud("pcd\\000009.pcd")
print(pcd)

vis = o3d.visualization.Visualizer()
vis.create_window()

#点云渲染
opt = vis.get_render_option()
opt.point_size = 1  #点云大小
opt.background_color = np.asarray([0, 0, 0])       #点云背景色

vis.add_geometry(pcd)
vis.run()
vis.destroy_window() 

1.2 运行效果

2 多个点云可视化

2.1 代码

import open3d as o3d
import numpy as np

#读取数据
pcd1 = o3d.io.read_point_cloud("peizhun\\001079.pcd")
print(pcd1)

#读取数据
pcd2 = o3d.io.read_point_cloud("peizhun\\004994.pcd")
print(pcd2)

vis = o3d.visualization.Visualizer()
vis.create_window()

#点云渲染
opt = vis.get_render_option()
opt.point_size = 1  #点云大小
opt.background_color = np.asarray([0, 0, 0])       #点云背景色

vis.add_geometry(pcd1)
vis.add_geometry(pcd2)
vis.run()
vis.destroy_window() 

2.2 运行效果

3 获取单个3D框

#box_dir 为json文件对应的路径
def get_box(box_dir):
    with open(box_dir, 'r') as f:
        data = json.load(f)
        for dict in data:
            #读取x,y,z
            x=dict.get('3d_location').get('x')
            y=dict.get('3d_location').get('y')
            z=dict.get('3d_location').get('z')

            #读取h,w,l
            h=dict.get('3d_dimensions').get('h')
            w=dict.get('3d_dimensions').get('w')
            l=dict.get('3d_dimensions').get('l')

            #8个定点信息
            point1=point2=point3=point4=point5=point6=point7=point8=[0,0,0]
            point1=[x+0.5*w,y-0.5*l,z-0.5*h]    
            point2=[x+0.5*w,y+0.5*l,z-0.5*h]    
            point3=[x-0.5*w,y+0.5*l,z-0.5*h]    
            point4=[x-0.5*w,y-0.5*l,z-0.5*h]    
            point5=[x+0.5*w,y-0.5*l,z+0.5*h]    
            point6=[x+0.5*w,y+0.5*l,z+0.5*h]    
            point7=[x-0.5*w,y+0.5*l,z+0.5*h]    
            point8=[x-0.5*w,y-0.5*l,z+0.5*h]    
            
            #3d box
            box=[point1,point2,point3,point4,point5,point6,point7,point8]  
    return box

4 获取多个3D框

def get_box(box_dir):
    box = [0] * count
    yaw = [0] * count
    with open(box_dir, 'r') as f:
        data = json.load(f)
        i = 0
        for dict in data:
            #读取x,y,z
            x=dict.get('3d_location').get('x')
            # x=round(x,3)
            y=dict.get('3d_location').get('y')
            z=dict.get('3d_location').get('z')

            #读取h,w,l
            h=dict.get('3d_dimensions').get('h')
            w=dict.get('3d_dimensions').get('w')
            l=dict.get('3d_dimensions').get('l')

            #读取rotation
            yaw[i] = dict.get('rotation')

            #8个定点信息
            point1=point2=point3=point4=point5=point6=point7=point8=[0,0,0]
            point1=[x+0.5*w,y-0.5*l,z-0.5*h]    
            point2=[x+0.5*w,y+0.5*l,z-0.5*h]    
            point3=[x-0.5*w,y+0.5*l,z-0.5*h]    
            point4=[x-0.5*w,y-0.5*l,z-0.5*h]    
            point5=[x+0.5*w,y-0.5*l,z+0.5*h]    
            point6=[x+0.5*w,y+0.5*l,z+0.5*h]    
            point7=[x-0.5*w,y+0.5*l,z+0.5*h]    
            point8=[x-0.5*w,y-0.5*l,z+0.5*h]    
            
            #3d box
            box[i]=[point1,point2,point3,point4,point5,point6,point7,point8]
            i += 1   
    return box

5 DAIR-V2X 3D框投影到点云

注：因为json文件中含有rotation变量，所以对矩阵做了旋转

import json
from tkinter import W
from matplotlib.pyplot import box
import open3d as o3d
import numpy as np
import math
import json 

def get_box_count(box_dir):
    with open(box_dir, 'r') as f:
        data = json.load(f)
        i = 0
        for dict in data:
            i += 1
    return i

def get_lidar_3d_8points(obj_size, yaw_lidar, center_lidar):
    center_lidar = [center_lidar[0], center_lidar[1], center_lidar[2]]#x,y,z

    lidar_r = np.matrix(
        [[math.cos(yaw_lidar), -math.sin(yaw_lidar), 0], [math.sin(yaw_lidar), math.cos(yaw_lidar), 0], [0, 0, 1]]
    )
    l, w, h = obj_size
    center_lidar[2] = center_lidar[2] - h / 2
    corners_3d_lidar = np.matrix(
        [
            [l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2],
            [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2],
            [0, 0, 0, 0, h, h, h, h],
        ]
    )
    corners_3d_lidar = lidar_r * corners_3d_lidar + np.matrix(center_lidar).T

    return corners_3d_lidar.T

def read_label_bboxes(label_path):
    with open(label_path, "r") as load_f:
        labels = json.load(load_f)
    i = 0
    boxes = [0] * count
    for label in labels:
        obj_size = [
            float(label["3d_dimensions"]["l"]),
            float(label["3d_dimensions"]["w"]),
            float(label["3d_dimensions"]["h"]),
        ]
        yaw_lidar = float(label["rotation"])
        center_lidar = [
            float(label["3d_location"]["x"]),
            float(label["3d_location"]["y"]),
            float(label["3d_location"]["z"]),
        ]

        box = get_lidar_3d_8points(obj_size, yaw_lidar, center_lidar)
        boxes[i] = np.matrix.tolist(box) 
        i += 1

    return boxes

#绘制点云与3d box
def draw_pcd_box(pcd,linesets):
    vis = o3d.visualization.Visualizer()
    vis.create_window()

    vis.add_geometry(pcd)
    for i in range(count): 
        vis.add_geometry(linesets[i])

    #点云渲染
    opt = vis.get_render_option()
    opt.point_size = 1  #点云大小
    opt.background_color = np.asarray([0, 0, 0])       #点云背景色

    vis.run()
    vis.destroy_window() 

if __name__=='__main__':

    #读取3d box数据
    box_dir = "json\\000009.json"
    count = get_box_count(box_dir)#获取3d框数量
    _3dbox = read_label_bboxes(box_dir)

    #读取点云数据
    pcd_dir = "pcd\\000009.pcd"
    pcd = o3d.io.read_point_cloud(pcd_dir)

    

    lines_box = np.array([[0, 1], [1, 2], [0, 3], [2, 3], [4, 5], [4, 7], [5, 6], [6, 7],
                            [0, 4], [1, 5], [2, 6], [3, 7]])
    colors = np.array([[0, 1, 0] for j in range(len(lines_box))])
    line_set = [0] * count
    for i in range(count):
        line_set[i] = o3d.geometry.LineSet()
        line_set[i].points = o3d.utility.Vector3dVector(_3dbox[i])
        line_set[i].lines = o3d.utility.Vector2iVector(lines_box)
        line_set[i].colors = o3d.utility.Vector3dVector(colors)
    # point_cloud = o3d.geometry.PointCloud()
    # point_cloud.points = o3d.utility.Vector3dVector(pcd[:,:3])
    draw_pcd_box(pcd,line_set)