可视化点云
点云的可视化--标注的3D box的可视化
基础运算理解
magnitude 向量大小
两个向量的欧式距离
np.sqrt(np.sum(np.power(vecA-vecB,2)))
numpy库中的sum函数则求得的是整个matrix的所有数据之和，返回的是一个float型数值，
两个列向量的差的平方求和后再开方，输出的应该是一个float型数值，
python的平方运算符是**,那么python中x的平方,就是这么写:x**2
numpy.square()
numpy.linalg.matrix_power
numpy.sqrt
numpy.power
np.sqrt(np.square(vector_1 - vector_2).sum())
np.sqrt(np.sum(np.power(vecA-vecB,2)))
magnitude = np.sqrt(np.sum((vecA-vecB)**2))
代码示例
# -*- coding: utf-8 -*-
import open3d as o3d
import numpy as np
from pathlib import Path
def rotz(t):
c = np.cos(t)
s = np.sin(t)
return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
def box2corner(box):
x = box[0]
y = box[1]
z = box[2]
l = box[3] # dx
w = box[4] # dy
h = box[5] # dz
yaw = box[6]
Box = np.array(
[
[-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],
[-h / 2, -h / 2, -h / 2, -h / 2, h / 2, h / 2, h / 2, h / 2],
]
)
# 先旋转再平移
R = rotz(yaw)
corners_3d = np.dot(R, Box) # corners_3d: (3, 8)
corners_3d[0, :] = corners_3d[0, :] + x
corners_3d[1, :] = corners_3d[1, :] + y
corners_3d[2, :] = corners_3d[2, :] + z
return np.transpose(corners_3d)
def get_line_set(corners):
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 i in range(len(lines))]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(corners)
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(colors)
return line_set
def load_pre_label(gt_json_path):
if Path(gt_json_path).exists():
pred_box = []
with open(gt_json_path, 'r') as fobj:
print("load pre label: ", gt_json_path)
for line in fobj:
l = line.strip().split(" ")
cx, cy, cz, sx, sy, sz, yaw = l[0], l[1],l[2], l[3],l[4], l[5],l[6]
box_data = list(map(float,[ cx, cy, cz, sx, sy, sz, yaw]))
pred_box.append(box_data)
return pred_box
##############
def get_draw_box(pre_box_set):
draw_boxes = []
for box_p in pre_box_set:
cx, cy, cz, sx, sy, sz, yaw = box_p[0],box_p[1],box_p[2], box_p[3],box_p[4], box_p[5],box_p[6]
corner_box = box2corner([cx, cy, cz, sx, sy, sz, yaw])
draw_box = get_line_set(corner_box)
draw_boxes.append(draw_box)
return draw_boxes
def draw_points_and_boxes(gt_pcd,pred_json):
mesh_frame = o3d.io.read_point_cloud(gt_pcd, remove_infinite_points=True, remove_nan_points=True)
box_sigle_pcd_all = load_pre_label(pred_json)
draw_boxes = get_draw_box(box_sigle_pcd_all)
vis = o3d.visualization.Visualizer()
vis.create_window(window_name="show_pred_pcd")
render_option = vis.get_render_option()
render_option.point_size = 2
coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=2.0, origin=[0, 0, 0])
vis.add_geometry(coordinate_frame)
vis.add_geometry(mesh_frame)
for box in draw_boxes:
vis.add_geometry(box)
vis.run()
vis.destroy_window()
############
def calculate_zy_rotation_for_arrow(vec):
"""rotation over axis z and y required to
get the vector vec into the same orientation as axis z
of the original FOR
Args:
- vec ():
"""
# Rotation over z axis of the FOR
gamma = np.arctan(vec[1] / vec[0])
Rz = np.array(
[
[np.cos(gamma), -np.sin(gamma), 0],
[np.sin(gamma), np.cos(gamma), 0],
[0, 0, 1],
]
)
# Rotate vec to calculate next rotation
vec = Rz.T @ vec.reshape(-1, 1)
vec = vec.reshape(-1)
# Rotation over y axis of the FOR
beta = np.arctan(vec[0] / (vec[2] + 1e-8))
Ry = np.array(
[[np.cos(beta), 0, np.sin(beta)], [0, 1, 0], [-np.sin(beta), 0, np.cos(beta)]]
)
return (Rz, Ry)
def get_arrow(scale=10):
"""
Create an arrow in for Open3D
"""
cone_height = scale * 0.2
cylinder_height = scale * 0.8
cone_radius = scale / 10
cylinder_radius = scale / 20
mesh_frame = o3d.geometry.TriangleMesh.create_arrow(
cone_radius=cone_radius,
cone_height=cone_height,
cylinder_radius=cylinder_radius,
cylinder_height=cylinder_height,
)
return mesh_frame
def create_arrow(origin=[0, 0, 0], end=None, vec=None):
"""
Creates an arrow from an origin point to an end point,
or create an arrow from a vector vec starting from origin.
Args:
- end (): End point. [x,y,z]
- vec (): Vector. [i,j,k]
"""
scale = 10
Ry = Rz = np.eye(3)
T = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
T[:3, -1] = origin
if end is not None:
vec = np.array(end) - np.array(origin)
elif vec is not None:
vec = np.array(vec)
if end is not None or vec is not None:
scale = np.sqrt(np.sum(vec ** 2))
Rz, Ry = calculate_zy_rotation_for_arrow(vec)
mesh = get_arrow(scale)
# Create the arrow
mesh.rotate(Ry, center=np.array([0, 0, 0]))
mesh.rotate(Rz, center=np.array([0, 0, 0]))
mesh.translate(origin)
## set Arrow clors
mesh.paint_uniform_color([1.0, 0.0, 0.0])
return mesh
def create_box_with_arrow(box):
"""
box: list(8) [ x, y, z, dx, dy, dz, yaw]
"""
cx, cy, cz, sx, sy, sz, yaw = box[0],box[1],box[2], box[3],box[4], box[5],box[6]
corner_box = box2corner([cx, cy, cz, sx, sy, sz, yaw])
box_o3d = get_line_set(corner_box)
x = box[0]
y = box[1]
z = box[2]
l = box[3]
yaw = box[6]
# get direction arrow
dir_x = l / 2.0 * np.cos(yaw)
dir_y = l / 2.0 * np.sin(yaw)
arrow_origin = [x - dir_x, y - dir_y, z]
arrow_end = [x + dir_x, y + dir_y, z]
if l > 2:
arrow = create_arrow(arrow_origin, arrow_end)
else:
arrow = None
return box_o3d, arrow
def draw_points_and_arrow_boxes(gt_pcd,pred_json):
mesh_frame = o3d.io.read_point_cloud(gt_pcd, remove_infinite_points=True, remove_nan_points=True)
box_sigle_pcd_all = load_pre_label(pred_json)
boxes_o3d=[]
for box_sig in box_sigle_pcd_all:
box_o3d, arrow = create_box_with_arrow(box_sig)
boxes_o3d.append(box_o3d)
boxes_o3d.append(arrow)
vis = o3d.visualization.Visualizer()
vis.create_window(window_name="show_pred_with arrow pcd")
render_option = vis.get_render_option()
render_option.point_size = 2
coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=2.0, origin=[0, 0, 0])
vis.add_geometry(coordinate_frame)
vis.add_geometry(mesh_frame)
for element in boxes_o3d:
if element is not None:
vis.add_geometry(element)
vis.run()
vis.destroy_window()
####################
if __name__ == '__main__':
exp_gt_pcd = r"~/test/833.9.pcd"
exp_pred_json = r"~/test/833predected.txt"
#draw_points_and_boxes(exp_gt_pcd,exp_pred_json)
draw_points_and_arrow_boxes(exp_gt_pcd,exp_pred_json)
参考
https://stackoverflow.com/questions/59026581/create-arrows-in-open3d
