提取點雲的代碼實作思路來自Shawn_1223大佬的部落格：基于OpenPCDet的三維目标檢測點雲學習--取出标注框内的點雲
***************************************************************************************************
OpenPCDet使用如下指令對點雲(.bin檔案)進行可視化：（使用的是open3d）
python demo.py --cfg_file ./cfgs/kitti_models/pv_rcnn.yaml --ckpt /home/dell/project/OpenPCDet/checkpoints/pv_rcnn_8369.pth --data_path /home/dell/project/OpenPCDet/data/kitti/testing/velodyne/000009.bin
如何在檢測可視化時将檢測框中的點雲染成紅色？
答：将如下代碼替換掉OpenPCDet/tools/visual_utils/open3d_vis_utils.py中的代碼即可
"""
Open3d visualization tool box
Written by Jihan YANG
All rights preserved from 2021 - present.
"""
import open3d
import torch
import matplotlib
import numpy as np
import scipy
from scipy.spatial import Delaunay
box_colormap = [
[1, 1, 1],
[0, 1, 0],
[0, 1, 1],
[1, 1, 0],
]
'''
一行代表點的顔色
二行代表汽車的顔色
三四行代表行人和自行車的顔色
'''
box_colormap = [
[1, 1, 1],
[0, 1, 0],
[0, 1, 1],
[1, 1, 0],
]
def check_numpy_to_torch(x):
if isinstance(x, np.ndarray):
return torch.from_numpy(x).float(), True
return x, False
def rotate_points_along_z(points, angle):
"""
Args:
points: (B, N, 3 + C)
angle: (B), angle along z-axis, angle increases x ==> y
Returns:
"""
points, is_numpy = check_numpy_to_torch(points)
angle, _ = check_numpy_to_torch(angle)
cosa = torch.cos(angle)
sina = torch.sin(angle)
zeros = angle.new_zeros(points.shape[0])
ones = angle.new_ones(points.shape[0])
rot_matrix = torch.stack((
cosa, sina, zeros,
-sina, cosa, zeros,
zeros, zeros, ones
), dim=1).view(-1, 3, 3).float()
points_rot = torch.matmul(points[:, :, 0:3], rot_matrix)
points_rot = torch.cat((points_rot, points[:, :, 3:]), dim=-1)
return points_rot.numpy() if is_numpy else points_rot
# 可視化框boxes 轉corner角（如下圖 和pcdet/utils/box_utils.py中boxes_to_corners_3d函數一樣
def boxes_to_corners_3d(boxes3d):
"""
7 -------- 4
/| /|
6 -------- 5 .
| | | |
. 3 -------- 0
|/ |/
2 -------- 1
Args:
boxes3d: (N, 7) [x, y, z, dx, dy, dz, heading], (x, y, z) is the box center
Returns:
"""
boxes3d, is_numpy = check_numpy_to_torch(boxes3d)
template = boxes3d.new_tensor((
[1, 1, -1], [1, -1, -1], [-1, -1, -1], [-1, 1, -1],
[1, 1, 1], [1, -1, 1], [-1, -1, 1], [-1, 1, 1],
)) / 2
corners3d = boxes3d[:, None, 3:6].repeat(1, 8, 1) * template[None, :, :]
corners3d = rotate_points_along_z(corners3d.view(-1, 8, 3), boxes3d[:, 6]).view(-1, 8, 3)
corners3d += boxes3d[:, None, 0:3]
return corners3d.numpy() if is_numpy else corners3d
def draw_point_in_box3d(vis, points, boxes3d):
corner3ds = boxes_to_corners_3d(boxes3d)#[N,8,3]
pc_in_boxes_sum = np.zeros((1,4))
for i in range(corner3ds.shape[0]):
flag = in_hull(points[:,0:3], corner3ds[i])
pc_in_boxes = points[flag]
pc_in_boxes_sum = np.vstack((pc_in_boxes_sum,pc_in_boxes))
points_in_boxes = pc_in_boxes_sum
pts = open3d.geometry.PointCloud()
pts.points = open3d.utility.Vector3dVector(points_in_boxes[:,:3])
vis.add_geometry(pts)
colors = [[1.,0., 1.] for _ in range(points_in_boxes.shape[0])]
pts.colors = open3d.utility.Vector3dVector(np.asarray(colors))
return vis
def in_hull(p, hull):
try:
if not isinstance(hull, Delaunay):
hull = Delaunay(hull)
flag = hull.find_simplex(p) >= 0
except scipy.spatial.qhull.QhullError:
print('Warning: not a hull %s' % str(hull))
flag = np.zeros(p.shape[0], dtype=np.bool)
return flag
def get_coor_colors(obj_labels):
"""
Args:
obj_labels: 1 is ground, labels > 1 indicates different instance cluster
Returns:
rgb: [N, 3]. color for each point.
"""
colors = matplotlib.colors.XKCD_COLORS.values()
max_color_num = obj_labels.max()
color_list = list(colors)[:max_color_num+1]
colors_rgba = [matplotlib.colors.to_rgba_array(color) for color in color_list]
label_rgba = np.array(colors_rgba)[obj_labels]
label_rgba = label_rgba.squeeze()[:, :3]
return label_rgba
def draw_scenes(points, gt_boxes=None, ref_boxes=None, ref_labels=None, ref_scores=None, point_colors=None, draw_origin=True):
# *************
# print('points: ',points)
# *************
if isinstance(points, torch.Tensor):
points = points.cpu().numpy()
if isinstance(gt_boxes, torch.Tensor):
gt_boxes = gt_boxes.cpu().numpy()
if isinstance(ref_boxes, torch.Tensor):
ref_boxes = ref_boxes.cpu().numpy()
vis = open3d.visualization.Visualizer()
vis.create_window()
vis.get_render_option().point_size = 1.0
vis.get_render_option().background_color = np.zeros(3) # RGB(0,0,0)表黑色
# draw origin
if draw_origin:
axis_pcd = open3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0])
# print('axis_pcd: ',axis_pcd) # axis_pcd: TriangleMesh with 1134 points and 2240 triangles.
vis.add_geometry(axis_pcd)
pts = open3d.geometry.PointCloud()
pts.points = open3d.utility.Vector3dVector(points[:, :3])
vis.add_geometry(pts)
if point_colors is None:
pts.colors = open3d.utility.Vector3dVector(np.ones((points.shape[0], 3)))
else:
pts.colors = open3d.utility.Vector3dVector(point_colors)
# *************
print('pts.colors: ',pts.colors)
print('point_colors: ',point_colors)
# *************
if gt_boxes is not None:
vis = draw_box(vis, gt_boxes, (0, 0, 1))
# *****将檢測框中的點雲染成紅色*****
vis = draw_point_in_box3d(vis, points, gt_boxes)
# *****将檢測框中的點雲染成紅色*****
# (0,1,0) green
if ref_boxes is not None:
vis = draw_box(vis, ref_boxes, (0, 1, 0), ref_labels, ref_scores)
# *****将檢測框中的點雲染成紅色*****
vis = draw_point_in_box3d(vis, points, ref_boxes)
# *****将檢測框中的點雲染成紅色*****
vis.run()
vis.destroy_window()
def translate_boxes_to_open3d_instance(gt_boxes):
"""
4-------- 6
/| /|
5 -------- 3 .
| | | |
. 7 -------- 1
|/ |/
2 -------- 0
"""
center = gt_boxes[0:3]
lwh = gt_boxes[3:6]
axis_angles = np.array([0, 0, gt_boxes[6] + 1e-10])
rot = open3d.geometry.get_rotation_matrix_from_axis_angle(axis_angles)
box3d = open3d.geometry.OrientedBoundingBox(center, rot, lwh)
line_set = open3d.geometry.LineSet.create_from_oriented_bounding_box(box3d)
# import ipdb; ipdb.set_trace(context=20)
lines = np.asarray(line_set.lines)
lines = np.concatenate([lines, np.array([[1, 4], [7, 6]])], axis=0)
line_set.lines = open3d.utility.Vector2iVector(lines)
return line_set, box3d
def draw_box(vis, gt_boxes, color=(0, 1, 0), ref_labels=None, score=None):
for i in range(gt_boxes.shape[0]):
line_set, box3d = translate_boxes_to_open3d_instance(gt_boxes[i])
if ref_labels is None:
line_set.paint_uniform_color(color)
else:
line_set.paint_uniform_color(box_colormap[ref_labels[i]])
vis.add_geometry(line_set)
# if score is not None:
# corners = box3d.get_box_points()
# vis.add_3d_label(corners[5], '%.2f' % score[i])
return vis
效果圖如下所示：
提取點雲的代碼實作思路參考部落格原址：基于OpenPCDet的三維目标檢測點雲學習--取出标注框内的點雲