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)
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]
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
z_corners = np.array([h / 2., -h / 2., -h / 2., h / 2., h / 2., -h / 2., -h / 2., h / 2.], dtype=np.float32).T
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)]])
R_list = np.transpose(rot_list, (2, 0, 1))
temp_corners = np.concatenate((x_corners.reshape(-1, 8, 1), y_corners.reshape(-1, 8, 1),
z_corners.reshape(-1, 8, 1)), axis=2)
rotated_corners = np.matmul(temp_corners, R_list)
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
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))]
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)
bbox_v = boxes3d_to_corners3d(boxes3d,rotate=False)
vis = draw_bboxes(bbox_v)
vis.add_geometry(pcd)
vis.add_geometry(axis_pcd)
while True:
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
if __name__ == '__main__':
main()
2 可视化效果
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/1e248011385f6027922cb17eb9b167ef.png)