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)