def vis_lidar_prediction_on_img(self, img, boxes, scores, labels):
for i in range(len(boxes)):
score = scores[i]
if score > 0.5:
p = boxes[i]
xyz = np.array([p[:3]])
c2d = lidar_pt_to_cam0_frame(xyz, self.calib_data)
if c2d is not None:
cv2.circle(img, (int(c2d[0]), int(c2d[1])), 3,
(0, 255, 255), -1)
hwl = np.array([p[3:6]])
r_y = [p[6]]
pts3d = compute_3d_box_lidar_coords(xyz,
hwl,
angles=r_y,
origin=(0.5, 0.5, 0.5),
axis=2)[0]
pts3d_4 = np.ones((pts3d.shape[0], pts3d.shape[1] + 1))
pts3d_4[:, :-1] = pts3d
_, pts2d = lidar_pts_to_cam0_frame(pts3d_4, self.calib_data)
pts2d = pts2d[:2, :].T
c = get_unique_color_by_id(labels[i])
draw_3d_box(pts2d, img, c)
if len(pts2d) > 4:
cv2.putText(
img, '{0} {1:.1f}'.format(self.label_map[labels[i]],
score),
(int(pts2d[2][0]), int(pts2d[2][1])),
cv2.FONT_HERSHEY_COMPLEX, .5, (255, 255, 255))
return img
def set_line(dets):
xyz = np.array([dets[[3,4,5]]])
hwl = np.array([dets[[0,1,2]]])
r_y = [dets[6]]
pts3d = compute_3d_box_lidar_coords(xyz, hwl, angles=r_y, origin=(0.5, 0.5, 0.5), axis=2)
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]]
color = color_set(dets[9])
colors = [color for i in range(len(lines))]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(pts3d[0])
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(colors)
return line_set
def predict_on_nucenes_local_file(self, v_p):
tic = time.time()
points = self.load_pc_from_file(v_p)[:, :4]
print('points shape: ', points.shape)
example = self.load_an_in_example_from_points(points)
pred = self.net(example)[0]
box3d = pred['box3d_lidar'].detach().cpu().numpy()
scores = pred["scores"].detach().cpu().numpy()
labels = pred["label_preds"].detach().cpu().numpy()
idx = np.where(scores > 0.11)[0]
box3d = box3d[idx, :]
labels = np.take(labels, idx)
scores = np.take(scores, idx)
# show points first
geometries = []
pcs = np.array(points[:, :3])
pcobj = PointCloud()
pcobj.points = Vector3dVector(pcs)
geometries.append(pcobj)
# try getting 3d boxes coordinates
for p in box3d:
xyz = np.array([p[:3]])
hwl = np.array([p[3:6]])
r_y = [p[6]]
pts3d = compute_3d_box_lidar_coords(xyz,
hwl,
angles=r_y,
origin=(0.5, 0.5, 0.5),
axis=2)[0]
points = [[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0], [0, 0, 1],
[1, 0, 1], [0, 1, 1], [1, 1, 1]]
print(pts3d, points)
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, 1] for i in range(len(lines))]
line_set = LineSet()
line_set.points = Vector3dVector(pts3d)
line_set.lines = Vector2iVector(lines)
line_set.colors = Vector3dVector(colors)
geometries.append(line_set)
draw_pcs_open3d(geometries)
def visualize(lidar_dir, dets_dir):
# lidar_dir ='data/test_lidar'
# dets_dir = 'results/video1_backup'
filename = os.listdir(lidar_dir)
file_number = len(filename)
vis = o3d.visualization.Visualizer()
vis.create_window(window_name='Open3D-Trk')
opt = vis.get_render_option()
render_option(opt, point_size=1, line_width=5)
vis2 = o3d.visualization.Visualizer()
vis2.create_window(window_name='Open3D-Focus-Trk')
opt2 = vis2.get_render_option()
render_option(opt2, point_size=2, line_width=10)
pcd = o3d.geometry.PointCloud()
for i in range(file_number):
vis.clear_geometries()
vis2.clear_geometries()
path = os.path.join(lidar_dir, filename[i])
print(path)
bin_velodyne = read_bin_velodyne(path)
# From numpy to Open3D
pcd.points = o3d.utility.Vector3dVector(bin_velodyne)
pcd.colors = o3d.utility.Vector3dVector(
[[0, 0.84, 0.0001] for x in range(len(bin_velodyne))])
dets_path = os.path.join(dets_dir, filename[i][:-4] + '.txt')
multiple_replace(dets_path)
dets = np.loadtxt(
dets_path, delimiter=' '
) # load detections, N x 16 : label 1,2,3,4,5,6,7,w,l,h,x,y,z,theta,score
for d in dets:
xyz = np.array([d[[13, 14, 15]]])
hwl = np.array([d[[10, 11, 12]]])
r_y = [d[16]]
pts3d = compute_3d_box_lidar_coords(xyz,
hwl,
angles=r_y,
origin=(0.5, 0.5, 0.5),
axis=2)
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]]
color = color_set(d[2])
colors = [color for i in range(len(lines))]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(pts3d[0])
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(colors)
vis.add_geometry(line_set)
vis2.add_geometry(line_set)
vis.add_geometry(pcd)
vis2.add_geometry(pcd)
vis.add_geometry(pcd)
vis2.add_geometry(pcd)
ctr = vis.get_view_control()
view_control(ctr,
zoom=0.1,
look_at=[0, 0, 0],
front=[0, -np.pi / 4, np.pi / 4])
ctr2 = vis2.get_view_control()
view_control(ctr2,
zoom=0.02,
look_at=[0, 10, 0],
front=[0, -np.pi / 3, np.pi / 4])
# ctr.set_up([i, 0, 0])
vis.poll_events()
vis2.poll_events()
1.6880405
],
[
-17.38843, -6.5131726, -0.07191068, 0.6577756, 0.7161297, 1.8168749,
1.8645211
],
[
2.0013125, -16.632671, -0.54558295, 0.54916567, 1.8482145,
1.7980447, 5.3003416
]]
for p in res:
xyz = np.array([p[:3]])
hwl = np.array([p[3:6]])
r_y = [p[6]]
pts3d = compute_3d_box_lidar_coords(xyz,
hwl,
angles=r_y,
origin=(0.5, 0.5, 0.5),
axis=2)
print('points 3d box: {}'.format(pts3d))
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, 1] for i in range(len(lines))]
line_set = o3d.geometry.LineSet()
line_set.points = o3d.utility.Vector3dVector(pts3d[0])
line_set.lines = o3d.utility.Vector2iVector(lines)
line_set.colors = o3d.utility.Vector3dVector(colors)
geometries.append(line_set)
draw_pcs_open3d(geometries)
