Apolloscapes 数据可视化

1 实现代码

import numpy as np
import open3d
import time


file_path = "home/jianqiang/233.txt"
def read_txt(file_path):
    file = open(file_path)
    data = []
    for line in file.readlines():
        curLine = line.strip().split(" ")
        data.append(curLine[1:len(curLine)])
    data = np.array(data,dtype=np.float32)
    # print(data)
    # print(data.shape)
    return data

def boxes3d_to_corners3d(boxes3d, rotate=True):
    """
    :param boxes3d: (N, 7) [x, y, z, h, w, l, ry]
    :param rotate:
    :return: corners3d: (N, 8, 3)
    """
    boxes_num = boxes3d.shape[0]
    # h, w, l = boxes3d[:, 3], boxes3d[:, 4], boxes3d[:, 5]
    l, w, h = boxes3d[:, 3], boxes3d[:, 4], boxes3d[:, 5]
    x_corners = np.array([l / 2., l / 2., -l / 2., -l / 2., l / 2., l / 2., -l / 2., -l / 2.], dtype=np.float32).T  # (N, 8)
    z_corners = np.array([h / 2., -h / 2., -h / 2., h / 2., h / 2., -h / 2., -h / 2., h / 2.], dtype=np.float32).T  # (N, 8)

    # y_corners = np.zeros((boxes_num, 8), dtype=np.float32)
    # y_corners[:, 4:8] = -h.reshape(boxes_num, 1).repeat(4, axis=1)  # (N, 8)

    y_corners = np.array([-w / 2., -w / 2., -w / 2., -w / 2., w / 2., w / 2., w / 2., w / 2.], dtype=np.float32).T

    if rotate:
        ry = boxes3d[:, 6]
        zeros, ones = np.zeros(ry.size, dtype=np.float32), np.ones(ry.size, dtype=np.float32)
        rot_list = np.array([[np.cos(ry), zeros, -np.sin(ry)],
                             [zeros,       ones,       zeros],
                             [np.sin(ry), zeros,  np.cos(ry)]])  # (3, 3, N)
        R_list = np.transpose(rot_list, (2, 0, 1))  # (N, 3, 3)

        temp_corners = np.concatenate((x_corners.reshape(-1, 8, 1), y_corners.reshape(-1, 8, 1),
                                       z_corners.reshape(-1, 8, 1)), axis=2)  # (N, 8, 3)
        rotated_corners = np.matmul(temp_corners, R_list)  # (N, 8, 3)
        x_corners, y_corners, z_corners = rotated_corners[:, :, 0], rotated_corners[:, :, 1], rotated_corners[:, :, 2]

    x_loc, y_loc, z_loc = boxes3d[:, 0], boxes3d[:, 1], boxes3d[:, 2]

    x = x_loc.reshape(-1, 1) + x_corners.reshape(-1, 8)
    y = y_loc.reshape(-1, 1) + y_corners.reshape(-1, 8)
    z = z_loc.reshape(-1, 1) + z_corners.reshape(-1, 8)
    corners = np.concatenate((x.reshape(-1, 8, 1), y.reshape(-1, 8, 1), z.reshape(-1, 8, 1)), axis=2)

    print(type(corners))
    print(corners.shape)
    # return corners.astype(np.float32)
    return corners

def draw_bboxes(bbox_v):

    vis = open3d.Visualizer()
    vis.create_window(window_name="apollo")
    vis.get_render_option().point_size = 10


    for i in range(bbox_v.shape[0]):
        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]]
        colors = [[1, 0, 0] for _ in range(len(bbox_lines))]  # Default blue

        bbox = open3d.LineSet()
        bbox.lines  = open3d.Vector2iVector(bbox_lines)
        bbox.colors = open3d.Vector3dVector(colors)
        bbox.points = open3d.Vector3dVector(bbox_v[i])
        vis.add_geometry(bbox)

    return vis

def main():

    pcd = open3d.io.read_point_cloud("233.pcd")
    print(pcd)
    axis_pcd = open3d.create_mesh_coordinate_frame(size=3,origin=[0,0,0])

=
    boxes3d = read_txt(file_path)
    # boxes3d = np.array([[24.86589813232422,10.29004383087158,-0.4389992952346802,11.100807,3.134082,3.275622,-3.116289]])
    bbox_v =  boxes3d_to_corners3d(boxes3d,rotate=False)
    vis = draw_bboxes(bbox_v)
    vis.add_geometry(pcd)
    vis.add_geometry(axis_pcd)

    while True:
        # update_renderer显示当前的数据
        vis.update_geometry()
        vis.poll_events()
        vis.update_renderer()


    # 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]]
    # colors = [[0, 0, 1] for _ in range(len(bbox_lines))]  # Default blue
    #
    # # 定义PCA的三条线
    # bbox = open3d.LineSet()
    # bbox.lines  = open3d.Vector2iVector(bbox_lines)
    # bbox.colors = open3d.Vector3dVector(colors)
    # bbox.points = open3d.Vector3dVector(bbox_v[0])
    #
    #

    # open3d.visualization.draw_geometries([pcd,axis_pcd,bbox], window_name='init_pointcloud')

if __name__ == '__main__':
    main()

2 可视化效果