def main(): nusc = NuScenes(version='v1.0-mini', dataroot='../../data/datasets/nuscenes', verbose=True) my_sample = nusc.sample[10] my_sample_token = my_sample['token'] sample_record = nusc.get('sample', my_sample_token) pointsensor_token = sample_record['data']['LIDAR_TOP'] camera_token = sample_record['data']['CAM_FRONT'] # Get point cloud pointsensor = nusc.get('sample_data', pointsensor_token) pcl_path = os.path.join(nusc.dataroot, pointsensor['filename']) if pointsensor['sensor_modality'] == 'lidar': pc_data = LidarPointCloud.from_file(pcl_path) else: raise NotImplementedError() pc = pc_data.points[0:3] vtk_window_size = (1280, 720) vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'Point Cloud', vtk_window_size, vtk_renderer) vtk_interactor = vtk.vtkRenderWindowInteractor() vtk_interactor.SetRenderWindow(vtk_render_window) vtk_interactor.SetInteractorStyle( vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer)) vtk_interactor.Initialize() vtk_pc = VtkPointCloudGlyph() vtk_pc.set_points(pc.T) vtk_renderer.AddActor(vtk_pc.vtk_actor) vtk_render_window.Render() vtk_interactor.Start() # Blocking
def main(): #################### # Options #################### # drive_id = '2011_09_26_drive_0001_sync' # No moving cars, depth completion sample 0 area # drive_id = '2011_09_26_drive_0002_sync' # depth completion sample 0 # drive_id = '2011_09_26_drive_0005_sync' # (gps bad) # drive_id = '2011_09_26_drive_0005_sync' # (moving) (gps bad) # drive_id = '2011_09_26_drive_0009_sync' # missing velo? # drive_id = '2011_09_26_drive_0011_sync' # both moving, then traffic light) # drive_id = '2011_09_26_drive_0013_sync' # both moving # drive_id = '2011_09_26_drive_0014_sync' # both moving, sample 104 # drive_id = '2011_09_26_drive_0015_sync' # both moving # drive_id = '2011_09_26_drive_0017_sync' # other only, traffic light, moving across # drive_id = '2011_09_26_drive_0018_sync' # other only, traffic light, forward # drive_id = '2011_09_26_drive_0019_sync' # both moving, some people at end, wonky gps # drive_id = '2011_09_26_drive_0020_sync' # gps drift # drive_id = '2011_09_26_drive_0022_sync' # ego mostly, then both, long drive_id = '2011_09_26_drive_0023_sync' # sample 169 (long) # drive_id = '2011_09_26_drive_0027_sync' # both moving, fast, straight # drive_id = '2011_09_26_drive_0028_sync' # both moving, opposite # drive_id = '2011_09_26_drive_0029_sync' # both moving, good gps # drive_id = '2011_09_26_drive_0032_sync' # both moving, following some cars # drive_id = '2011_09_26_drive_0035_sync' # # drive_id = '2011_09_26_drive_0036_sync' # (long) behind red truck # drive_id = '2011_09_26_drive_0039_sync' # ok, 1 moving # drive_id = '2011_09_26_drive_0046_sync' # (short) only 1 moving at start # drive_id = '2011_09_26_drive_0048_sync' # ok but short, no movement # drive_id = '2011_09_26_drive_0052_sync' # # drive_id = '2011_09_26_drive_0056_sync' # # drive_id = '2011_09_26_drive_0057_sync' # (gps sinking) # drive_id = '2011_09_26_drive_0059_sync' # # drive_id = '2011_09_26_drive_0060_sync' # # drive_id = '2011_09_26_drive_0061_sync' # ego only, ok, long, bumpy, some gps drift # drive_id = '2011_09_26_drive_0064_sync' # Smart car, sample 25 # drive_id = '2011_09_26_drive_0070_sync' # # drive_id = '2011_09_26_drive_0079_sync' # # drive_id = '2011_09_26_drive_0086_sync' # (medium) uphill # drive_id = '2011_09_26_drive_0087_sync' # # drive_id = '2011_09_26_drive_0091_sync' # # drive_id = '2011_09_26_drive_0093_sync' # Sample 50 (bad) # drive_id = '2011_09_26_drive_0106_sync' # Two cyclists on right # drive_id = '2011_09_26_drive_0113_sync' # # drive_id = '2011_09_26_drive_0117_sync' # (long) # drive_id = '2011_09_28_drive_0002_sync' # campus # drive_id = '2011_09_28_drive_0016_sync' # campus # drive_id = '2011_09_28_drive_0021_sync' # campus # drive_id = '2011_09_28_drive_0034_sync' # # drive_id = '2011_09_28_drive_0037_sync' # # drive_id = '2011_09_28_drive_0038_sync' # # drive_id = '2011_09_28_drive_0039_sync' # busy campus, bad gps # drive_id = '2011_09_28_drive_0043_sync' # # drive_id = '2011_09_28_drive_0045_sync' # # drive_id = '2011_09_28_drive_0179_sync' # # drive_id = '2011_09_29_drive_0026_sync' # # drive_id = '2011_09_29_drive_0071_sync' # # drive_id = '2011_09_30_drive_0020_sync' # # drive_id = '2011_09_30_drive_0027_sync' # (long) # drive_id = '2011_09_30_drive_0028_sync' # (long) bad gps # drive_id = '2011_09_30_drive_0033_sync' # # drive_id = '2011_09_30_drive_0034_sync' # # drive_id = '2011_10_03_drive_0042_sync' # # drive_id = '2011_10_03_drive_0047_sync' # raw_dir = os.path.expanduser('~/Kitti/raw') max_fps = 50.0 vtk_window_size = (1280, 720) show_pose = True point_cloud_source = 'lidar' buffer_size = 20 # Load raw data drive_date = drive_id[0:10] drive_num_str = drive_id[17:21] raw_data = pykitti.raw(raw_dir, drive_date, drive_num_str) # Check that velo length matches timestamps? if len(raw_data.velo_files) != len(raw_data.timestamps): raise ValueError('velo files and timestamps have different length!') frame_range = (0, len(raw_data.timestamps)) # frame_range = (0, 100) camera_viewpoint = 'front' camera_viewpoint = 'elevated' #################### # End of Options #################### min_loop_time = 1.0 / max_fps vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'Overlaid Point Cloud', vtk_window_size, vtk_renderer) vtk_interactor = vtk.vtkRenderWindowInteractor() vtk_interactor.SetRenderWindow(vtk_render_window) vtk_interactor.SetInteractorStyle( vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer)) vtk_interactor.Initialize() drive_date_dir = raw_dir + '/{}'.format(drive_date) # Setup iterators and paths if point_cloud_source != 'lidar': raise ValueError( 'Invalid point cloud source {}'.format(point_cloud_source)) cam_p = raw_data.calib.P_rect_20 # Load poses poses_path = drive_date_dir + '/{}/poses.mat'.format(drive_id) poses_mat = scipy.io.loadmat(poses_path) poses = np.asarray(poses_mat['pose'][0]) # Read calibrations tf_velo_calib_imu_calib = raw_data.calib.T_velo_imu tf_imu_calib_velo_calib = transform_utils.invert_tf( tf_velo_calib_imu_calib) tf_cam0_calib_velo_calib = raw_data.calib.T_cam0_velo tf_velo_calib_cam0_calib = transform_utils.invert_tf( tf_cam0_calib_velo_calib) # Create VtkAxes vtk_axes = vtk.vtkAxesActor() vtk_axes.SetTotalLength(2, 2, 2) vtk_renderer.AddActor(vtk_axes) # Create vtk actors vtk_pc_list = [] for i in range(buffer_size): # Point cloud actor wrapper vtk_pc = VtkPointCloudGlyph() vtk_pc.vtk_actor.GetProperty().SetPointSize(2) # all_vtk_actors.append(vtk_point_cloud.vtk_actor) vtk_renderer.AddActor(vtk_pc.vtk_actor) vtk_pc_list.append(vtk_pc) curr_actor_idx = -1 for frame_idx in range(*frame_range): loop_start_time = time.time() curr_actor_idx = (curr_actor_idx + 1) % buffer_size vtk_pc = vtk_pc_list[curr_actor_idx] for i in range(buffer_size): if i == curr_actor_idx: vtk_pc_list[i].vtk_actor.GetProperty().SetPointSize(3) else: vtk_pc_list[i].vtk_actor.GetProperty().SetPointSize(0.5) print('{} / {}'.format(frame_idx, len(raw_data.timestamps) - 1)) # Load next frame data load_start_time = time.time() # rgb_image = np.asarray(next(cam2_iterator)) rgb_image = np.asarray(raw_data.get_cam2(frame_idx)) bgr_image = rgb_image[..., ::-1] if point_cloud_source == 'lidar': velo_points = get_velo_points(raw_data, frame_idx) # Transform point cloud to cam_0 frame velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]], constant_values=1.0, mode='constant') cam0_curr_pc_all_padded = raw_data.calib.T_cam0_velo @ velo_curr_points_padded.T else: raise ValueError('Invalid point cloud source') print('load\t\t', time.time() - load_start_time) # Project velodyne points projection_start_time = time.time() if point_cloud_source == 'lidar': points_in_img = calib_utils.project_pc_to_image( cam0_curr_pc_all_padded[0:3], cam_p) points_in_img_int = np.round(points_in_img).astype(np.int32) image_filter = obj_utils.points_in_img_filter( points_in_img_int, bgr_image.shape) cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter] points_in_img_int_valid = points_in_img_int[:, image_filter] point_colours = bgr_image[points_in_img_int_valid[1], points_in_img_int_valid[0]] else: raise ValueError('Invalid point_cloud_source', point_cloud_source) print('projection\t', time.time() - projection_start_time) # Get calibration transformations tf_velo_calib_imu_calib = transform_utils.invert_tf( tf_imu_calib_velo_calib) tf_cam0_calib_imu_calib = tf_cam0_calib_velo_calib @ tf_velo_calib_imu_calib tf_imu_calib_cam0_calib = transform_utils.invert_tf( tf_cam0_calib_imu_calib) # Get poses imu_ref_pose = poses[frame_idx] tf_imu_ref_imu_curr = imu_ref_pose velo_curr_pc_padded = tf_velo_calib_cam0_calib @ cam0_curr_pc_padded imu_curr_pc_padded = tf_imu_calib_velo_calib @ velo_curr_pc_padded imu_ref_pc_padded = tf_imu_ref_imu_curr @ imu_curr_pc_padded # VtkPointCloud vtk_pc_start_time = time.time() vtk_pc.set_points(imu_ref_pc_padded[0:3].T, point_colours) print('vtk_pc\t\t', time.time() - vtk_pc_start_time) if show_pose: vtk_pc_pose = VtkPointCloudGlyph() vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5) vtk_pc_pose.set_points(np.reshape(imu_ref_pose[0:3, 3], [-1, 3])) vtk_renderer.AddActor(vtk_pc_pose.vtk_actor) # Move camera if camera_viewpoint == 'front': imu_curr_cam0_position = tf_imu_calib_cam0_calib @ [ 0.0, 0.0, 0.0, 1.0 ] elif camera_viewpoint == 'elevated': imu_curr_cam0_position = tf_imu_calib_cam0_calib.dot( [0.0, -5.0, -10.0, 1.0]) else: raise ValueError('Invalid camera_pos', camera_viewpoint) imu_ref_cam0_position = tf_imu_ref_imu_curr.dot(imu_curr_cam0_position) imu_curr_focal_point = tf_imu_calib_cam0_calib.dot( [0.0, 0.0, 20.0, 1.0]) imu_ref_focal_point = tf_imu_ref_imu_curr.dot(imu_curr_focal_point) current_cam = vtk_renderer.GetActiveCamera() vtk_renderer.ResetCamera() current_cam.SetViewUp(0, 0, 1) current_cam.SetPosition(imu_ref_cam0_position[0:3]) current_cam.SetFocalPoint(*imu_ref_focal_point[0:3]) current_cam.Zoom(0.5) # Reset the clipping range to show all points vtk_renderer.ResetCameraClippingRange() # Render render_start_time = time.time() vtk_render_window.Render() print('render\t\t', time.time() - render_start_time) # Pause to keep frame rate under max loop_run_time = time.time() - loop_start_time print('loop\t\t', loop_run_time) if loop_run_time < min_loop_time: time.sleep(min_loop_time - loop_run_time) print('---') print('Done') # Keep window open vtk_interactor.Start()
def main(): #################### # Options #################### odom_dir = os.path.expanduser('~/Kitti/odometry/dataset') sequence = '03' # max_fps = 10.0 # vtk_window_size = (1280, 720) vtk_window_size = (960, 540) save_images = False point_cloud_source = 'lidar' # point_cloud_source = 'fast' # point_cloud_source = 'multiscale' # first_frame_idx = None # first_frame_pose = None # Setup odometry dataset handler odom_dataset = pykitti.odometry(odom_dir, sequence) # # Check that velo length matches timestamps? # if len(odom_dataset.velo_files) != len(odom_dataset.timestamps): # raise ValueError('velo files and timestamps have different length!') frame_range = (0, len(odom_dataset.timestamps)) # frame_range = (0, 100) # frame_range = (1, 10) # frame_range = (20, 30) # frame_range = (440, 442) # frame_range = (441, 442) # frame_range = (440, 452) # frame_range = (440, 512) # frame_range = (500, 502) # frame_range = (500, 512) # for frame_idx in range(len(raw_data.timestamps)): # for frame_idx in range(457, 459): camera_viewpoint = 'front' camera_viewpoint = 'elevated' # camera_zoom = 2.2 # camera_viewpoint = (0.0, -5.0, -30.0) # camera_fp = (0.0, 1.0, 30.0) # viewpoint = 'front' # camera_zoom = 0.6 # camera_pos = (0.0, 0.0, 0.0) # camera_fp = (0.0, 0.0, 2000.0) # vtk_window_size = (1000, 500) # viewpoint = 'bev' # camera_zoom = 1.0 # # camera_pos = (0.0, -15.0, -25.0) # # camera_pos = (0.0, 0.0, 0.0) # camera_fp = (0.0, 1.0, 30.0) #################### # End of Options #################### # Setup output folder # drive_name = category + '_' + date + '_' + drive # images_out_dir = 'outputs/point_clouds/' + drive_name + '/' + point_cloud_source + '_' + viewpoint # os.makedirs(images_out_dir, exist_ok=True) # max_loop_time = 1.0 / max_fps vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'Overlaid Point Cloud', vtk_window_size, vtk_renderer) vtk_interactor = vtk.vtkRenderWindowInteractor() vtk_interactor.SetRenderWindow(vtk_render_window) vtk_interactor.SetInteractorStyle( vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer)) vtk_interactor.Initialize() cam_p2 = odom_dataset.calib.P_rect_20 # Load poses cam0_poses = odom_dataset.poses # Setup camera if camera_viewpoint == 'front': cam0_curr_vtk_cam_pos = [0.0, 0.0, 0.0, 1.0] cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0] elif camera_viewpoint == 'elevated': cam0_curr_vtk_cam_pos = [0.0, -5.0, -15.0, 1.0] cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0] else: raise ValueError('Invalid camera_pos', camera_viewpoint) # Create VtkAxes vtk_axes = vtk.vtkAxesActor() vtk_axes.SetTotalLength(2, 2, 2) vtk_renderer.AddActor(vtk_axes) for frame_idx in range(*frame_range): # Point cloud actor wrapper vtk_pc = VtkPointCloud() vtk_pc.vtk_actor.GetProperty().SetPointSize(2) # all_vtk_actors.append(vtk_point_cloud.vtk_actor) vtk_renderer.AddActor(vtk_pc.vtk_actor) print('{} / {}'.format(frame_idx, len(odom_dataset.timestamps) - 1)) # Load next frame data load_start_time = time.time() rgb_image = np.asarray(odom_dataset.get_cam2(frame_idx)) bgr_image = rgb_image[..., ::-1] if point_cloud_source == 'lidar': velo_points = get_velo_points(odom_dataset, frame_idx) # Transform point cloud to cam_0_curr frame velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]], constant_values=1.0, mode='constant') cam0_curr_pc_all_padded = odom_dataset.calib.T_cam0_velo @ velo_curr_points_padded.T elif point_cloud_source == 'multiscale': # depth_map_path = depth_map_dir + '/{:010d}.png'.format(frame_idx) # depth_map = depth_map_utils.read_depth_map(depth_map_path) # points_cam2 = depth_map_utils.get_depth_point_cloud(depth_map, cam_p).T raise NotImplementedError() else: raise ValueError('Invalid point cloud source') print('load\t\t', time.time() - load_start_time) # Project velodyne points projection_start_time = time.time() if point_cloud_source == 'lidar': # Project into image2 points_in_img2 = calib_utils.project_pc_to_image( cam0_curr_pc_all_padded[0:3], cam_p2) points_in_img2_int = np.round(points_in_img2).astype(np.int32) image_filter = obj_utils.points_in_img_filter( points_in_img2_int, bgr_image.shape) cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter] points_in_img_int_valid = points_in_img2_int[:, image_filter] point_colours = bgr_image[points_in_img_int_valid[1], points_in_img_int_valid[0]] else: raise ValueError('Invalid point_cloud_source', point_cloud_source) print('projection\t', time.time() - projection_start_time) # Get pose cam0_ref_pose = cam0_poses[frame_idx] tf_cam0_ref_cam0_curr = cam0_ref_pose # print('cam0_ref_pose\n', np.round(cam0_ref_pose, 3)) cam0_ref_pc_padded = tf_cam0_ref_cam0_curr @ cam0_curr_pc_padded # VtkPointCloud vtk_pc_start_time = time.time() vtk_pc.set_points(cam0_ref_pc_padded[0:3].T, point_colours) print('vtk_pc\t\t', time.time() - vtk_pc_start_time) # Display pose vtk_pc_pose = VtkPointCloud() vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5) vtk_pc_pose.set_points(np.reshape(cam0_ref_pose[0:3, 3], [-1, 3])) vtk_renderer.AddActor(vtk_pc_pose.vtk_actor) cam0_ref_vtk_cam_pos = tf_cam0_ref_cam0_curr.dot(cam0_curr_vtk_cam_pos) cam0_ref_vtk_focal_point = tf_cam0_ref_cam0_curr.dot( cam0_curr_vtk_focal_point) current_cam = vtk_renderer.GetActiveCamera() vtk_renderer.ResetCamera() current_cam.SetViewUp(0, -1, 0) current_cam.SetPosition(cam0_ref_vtk_cam_pos[0:3]) current_cam.SetFocalPoint(*cam0_ref_vtk_focal_point[0:3]) current_cam.Zoom(0.5) vtk_renderer.ResetCameraClippingRange() # Render render_start_time = time.time() vtk_render_window.Render() print('render\t\t', time.time() - render_start_time) print('---') print('Done') # Keep window open vtk_interactor.Start()
def main(): # set root_dir to the correct path to your dataset folder root_dir = core.data_dir( ) + '/datasets/argoverse/argoverse-tracking/sample/' vtk_window_size = (1280, 720) argoverse_loader = ArgoverseTrackingLoader(root_dir) print('Total number of logs:', len(argoverse_loader)) argoverse_loader.print_all() for i, argoverse_data in enumerate(argoverse_loader): if i >= 3: break print(argoverse_data) argoverse_data = argoverse_loader[0] print(argoverse_data) argoverse_data = argoverse_loader.get( 'c6911883-1843-3727-8eaa-41dc8cda8993') print(argoverse_data) log_id = 'c6911883-1843-3727-8eaa-41dc8cda8993' # argoverse_loader.log_list[55] frame_idx = 150 camera = argoverse_loader.CAMERA_LIST[0] argoverse_data = argoverse_loader.get(log_id) city_name = argoverse_data.city_name print('-------------------------------------------------------') print(f'Log: {log_id}, \n\tframe: {frame_idx}, camera: {camera}') print('-------------------------------------------------------\n') lidar_points = argoverse_data.get_lidar(frame_idx) img = argoverse_data.get_image_sync(frame_idx, camera=camera) objects = argoverse_data.get_label_object(frame_idx) calib = argoverse_data.get_calibration(camera) # TODO: Calculate point colours vtk_pc = VtkPointCloudGlyph() vtk_pc.set_points(lidar_points) vtk_pc.set_point_size(2) vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'Argoverse Demo', vtk_window_size, vtk_renderer) vtk_axes = vtk.vtkAxesActor() vtk_axes.SetTotalLength(2, 2, 2) vtk_renderer.AddActor(vtk_axes) vtk_renderer.AddActor(vtk_pc.vtk_actor) current_cam = vtk_renderer.GetActiveCamera() current_cam.SetViewUp(0, 0, 1) current_cam.SetPosition(-50, 0, 15) current_cam.SetFocalPoint(30, 0, 0) vtk_interactor = demo_utils.setup_vtk_interactor(vtk_render_window) vtk_interactor.Start()
def main(): ############################## # Options ############################## # sequence_id = '0013' # sequence_id = '0014' sequence_id = '0015' sequence_id = '0020' # raw_dir = os.path.expanduser('~/Kitti/raw') tracking_dir = os.path.expanduser('~/Kitti/tracking/training') tracking_data = pykitti.tracking(tracking_dir, sequence_id) max_fps = 30.0 vtk_window_size = (1280, 720) show_pose = True point_cloud_source = 'lidar' buffer_size = 5 oxts_path = tracking_data.base_path + '/oxts/{}.txt'.format(sequence_id) oxts_data = pykitti.utils.load_oxts_packets_and_poses([oxts_path]) poses = np.asarray([oxt.T_w_imu for oxt in oxts_data]) calib_path = tracking_data.base_path + '/calib/{}.txt'.format(sequence_id) calib_data = tracking_utils.load_calib(calib_path) camera_viewpoint = 'front' camera_viewpoint = 'elevated' load_images = True # load_images = False ############################## label_path = tracking_data.base_path + '/label_02/{}.txt'.format( sequence_id) gt_tracklets = tracking_utils.get_tracklets(label_path) min_loop_time = 1.0 / max_fps vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'KITTI Tracking Demo', vtk_window_size, vtk_renderer) vtk_interactor = vtk.vtkRenderWindowInteractor() vtk_interactor.SetRenderWindow(vtk_render_window) vtk_interactor.SetInteractorStyle( vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer)) vtk_interactor.Initialize() if point_cloud_source != 'lidar': raise ValueError( 'Invalid point cloud source {}'.format(point_cloud_source)) # cam_p = raw_data.calib.P_rect_20 cam_p = calib_data['P2'].reshape(3, 4) r_rect = calib_data['R_rect'].reshape(3, 3) # Read calibrations tf_velo_calib_imu_calib = calib_data['Tr_imu_velo'].reshape(3, 4) tf_velo_calib_imu_calib = np.vstack( [tf_velo_calib_imu_calib, np.asarray([0, 0, 0, 1])]) # tf_velo_calib_imu_calib = raw_data.calib.T_velo_imu tf_imu_calib_velo_calib = transform_utils.invert_tf( tf_velo_calib_imu_calib) tf_cam0_calib_velo_calib = r_rect @ calib_data['Tr_velo_cam'].reshape(3, 4) tf_cam0_calib_velo_calib = np.vstack( [tf_cam0_calib_velo_calib, np.asarray([0, 0, 0, 1])]) # tf_cam0_calib_velo_calib = raw_data.calib.T_cam0_velo tf_velo_calib_cam0_calib = transform_utils.invert_tf( tf_cam0_calib_velo_calib) # Create VtkAxes vtk_axes = vtk.vtkAxesActor() vtk_axes.SetTotalLength(2, 2, 2) vtk_renderer.AddActor(vtk_axes) # Create vtk actors vtk_pc_list = [] for i in range(buffer_size): # Point cloud actor wrapper vtk_pc = VtkPointCloudGlyph() vtk_pc.vtk_actor.GetProperty().SetPointSize(2) # all_vtk_actors.append(vtk_point_cloud.vtk_actor) vtk_renderer.AddActor(vtk_pc.vtk_actor) vtk_pc_list.append(vtk_pc) curr_actor_idx = -1 vtk_boxes = VtkBoxes() vtk_text_labels = VtkTextLabels() vtk_renderer.AddActor(vtk_boxes.vtk_actor) vtk_renderer.AddActor(vtk_text_labels.vtk_actor) for frame_idx in range(len(poses)): loop_start_time = time.time() curr_actor_idx = (curr_actor_idx + 1) % buffer_size vtk_pc = vtk_pc_list[curr_actor_idx] for i in range(buffer_size): if i == curr_actor_idx: vtk_pc_list[i].vtk_actor.GetProperty().SetPointSize(3) else: vtk_pc_list[i].vtk_actor.GetProperty().SetPointSize(0.5) print('{} / {}'.format(frame_idx, len(tracking_data.velo_files) - 1)) # Load next frame data load_start_time = time.time() if point_cloud_source == 'lidar': velo_points, velo_points_i = get_velo_points( tracking_data, frame_idx) # Transform point cloud to cam_0 frame velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]], constant_values=1.0, mode='constant') cam0_curr_pc_all_padded = tf_cam0_calib_velo_calib @ velo_curr_points_padded.T # cam0_curr_pc_all_padded = raw_data.calib.T_cam0_velo @ velo_curr_points_padded.T else: raise ValueError('Invalid point cloud source') print('load\t\t', time.time() - load_start_time) # Project velodyne points projection_start_time = time.time() if point_cloud_source == 'lidar': if load_images: rgb_image = np.asarray(tracking_data.get_cam2(frame_idx)) bgr_image = rgb_image[..., ::-1] points_in_img = calib_utils.project_pc_to_image( cam0_curr_pc_all_padded[0:3], cam_p) points_in_img_int = np.round(points_in_img).astype(np.int32) image_filter = obj_utils.points_in_img_filter( points_in_img_int, bgr_image.shape) cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter] points_in_img_int_valid = points_in_img_int[:, image_filter] point_colours = bgr_image[points_in_img_int_valid[1], points_in_img_int_valid[0]] else: cam0_curr_pc_padded = cam0_curr_pc_all_padded point_colours = np.repeat( (velo_points_i * 255).astype(np.uint8), 3).reshape(-1, 3) # point_colours = np.full(cam0_curr_pc_padded[0:3].T.shape, [255, 255, 255]) else: raise ValueError('Invalid point_cloud_source', point_cloud_source) print('projection\t', time.time() - projection_start_time) # Get calibration transformations tf_velo_calib_imu_calib = transform_utils.invert_tf( tf_imu_calib_velo_calib) tf_cam0_calib_imu_calib = tf_cam0_calib_velo_calib @ tf_velo_calib_imu_calib tf_imu_calib_cam0_calib = transform_utils.invert_tf( tf_cam0_calib_imu_calib) # Get poses imu_ref_pose = poses[frame_idx] tf_imu_ref_imu_curr = imu_ref_pose velo_curr_pc_padded = tf_velo_calib_cam0_calib @ cam0_curr_pc_padded imu_curr_pc_padded = tf_imu_calib_velo_calib @ velo_curr_pc_padded imu_ref_pc_padded = tf_imu_ref_imu_curr @ imu_curr_pc_padded # TODO: Show points in correct frame # VtkPointCloud vtk_pc_start_time = time.time() vtk_pc.set_points(cam0_curr_pc_padded[0:3].T, point_colours) # vtk_pc.set_points(imu_ref_pc_padded[0:3].T, point_colours) print('vtk_pc\t\t', time.time() - vtk_pc_start_time) # if show_pose: # vtk_pc_pose = VtkPointCloudGlyph() # vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5) # vtk_pc_pose.set_points(np.reshape(imu_ref_pose[0:3, 3], [-1, 3])) # vtk_renderer.AddActor(vtk_pc_pose.vtk_actor) # Tracklets frame_mask = [tracklet.frame == frame_idx for tracklet in gt_tracklets] tracklets_for_frame = gt_tracklets[frame_mask] obj_labels = [ tracking_utils.tracklet_to_obj_label(tracklet) for tracklet in tracklets_for_frame ] vtk_boxes.set_objects(obj_labels, colour_scheme=vtk_utils.COLOUR_SCHEME_KITTI) positions_3d = [obj_label.t for obj_label in obj_labels] text_labels = [str(tracklet.id) for tracklet in tracklets_for_frame] vtk_text_labels.set_text_labels(positions_3d, text_labels) # Move camera if camera_viewpoint == 'front': imu_curr_cam0_position = tf_imu_calib_cam0_calib @ [ 0.0, 0.0, 0.0, 1.0 ] elif camera_viewpoint == 'elevated': imu_curr_cam0_position = tf_imu_calib_cam0_calib.dot( [0.0, -5.0, -10.0, 1.0]) else: raise ValueError('Invalid camera_pos', camera_viewpoint) # imu_ref_cam0_position = tf_imu_ref_imu_curr.dot(imu_curr_cam0_position) # imu_curr_focal_point = tf_imu_calib_cam0_calib.dot([0.0, 0.0, 20.0, 1.0]) # imu_ref_focal_point = tf_imu_ref_imu_curr.dot(imu_curr_focal_point) current_cam = vtk_renderer.GetActiveCamera() vtk_renderer.ResetCamera() current_cam.SetViewUp(0, 0, 1) current_cam.SetPosition(0.0, -8.0, -20.0) # current_cam.SetPosition(imu_ref_cam0_position[0:3]) current_cam.SetFocalPoint(0.0, 0.0, 20.0) # current_cam.SetFocalPoint(*imu_ref_focal_point[0:3]) # current_cam.Zoom(0.5) # Reset the clipping range to show all points vtk_renderer.ResetCameraClippingRange() # Render render_start_time = time.time() vtk_render_window.Render() print('render\t\t', time.time() - render_start_time) # Pause to keep frame rate under max loop_run_time = time.time() - loop_start_time print('loop\t\t', loop_run_time) if loop_run_time < min_loop_time: time.sleep(min_loop_time - loop_run_time) print('---') print('Done') # Keep window open vtk_interactor.Start()
def main(): #################### # Options #################### # drive_id = '2011_09_26_drive_0001_sync' # No moving cars, depth completion sample 0 area # drive_id = '2011_09_26_drive_0002_sync' # depth completion sample 0 # drive_id = '2011_09_26_drive_0005_sync' # (gps bad) # drive_id = '2011_09_26_drive_0005_sync' # (moving) (gps bad) # drive_id = '2011_09_26_drive_0009_sync' # missing velo? # drive_id = '2011_09_26_drive_0011_sync' # both moving, then traffic light) # drive_id = '2011_09_26_drive_0013_sync' # both moving # drive_id = '2011_09_26_drive_0014_sync' # both moving, sample 104 (314) # drive_id = '2011_09_26_drive_0015_sync' # both moving # drive_id = '2011_09_26_drive_0017_sync' # other only, traffic light, moving across # drive_id = '2011_09_26_drive_0018_sync' # other only, traffic light, forward # drive_id = '2011_09_26_drive_0019_sync' # both moving, some people at end, wonky gps # drive_id = '2011_09_26_drive_0020_sync' # gps drift # drive_id = '2011_09_26_drive_0022_sync' # ego mostly, then both, long # drive_id = '2011_09_26_drive_0023_sync' # sample 169 (474) # drive_id = '2011_09_26_drive_0027_sync' # both moving, fast, straight # drive_id = '2011_09_26_drive_0028_sync' # both moving, opposite # drive_id = '2011_09_26_drive_0029_sync' # both moving, good gps (430) # drive_id = '2011_09_26_drive_0032_sync' # both moving, following some cars # drive_id = '2011_09_26_drive_0035_sync' # # drive_id = '2011_09_26_drive_0036_sync' # (long) behind red truck # drive_id = '2011_09_26_drive_0039_sync' # ok, 1 moving # drive_id = '2011_09_26_drive_0046_sync' # (short) only 1 moving at start # drive_id = '2011_09_26_drive_0048_sync' # ok but short, no movement # drive_id = '2011_09_26_drive_0052_sync' # # drive_id = '2011_09_26_drive_0056_sync' # # drive_id = '2011_09_26_drive_0057_sync' # (gps sinking) # drive_id = '2011_09_26_drive_0059_sync' # # drive_id = '2011_09_26_drive_0060_sync' # # drive_id = '2011_09_26_drive_0061_sync' # ego only, ok, long, bumpy, some gps drift # drive_id = '2011_09_26_drive_0064_sync' # Smart car, sample 25 # drive_id = '2011_09_26_drive_0070_sync' # # drive_id = '2011_09_26_drive_0079_sync' # drive_id = '2011_09_26_drive_0086_sync' # (medium) uphill # drive_id = '2011_09_26_drive_0087_sync' # # drive_id = '2011_09_26_drive_0091_sync' # # drive_id = '2011_09_26_drive_0093_sync' # Sample 50 (bad) # drive_id = '2011_09_26_drive_0106_sync' # Two cyclists on right # drive_id = '2011_09_26_drive_0113_sync' # # drive_id = '2011_09_26_drive_0117_sync' # (long) # drive_id = '2011_09_28_drive_0002_sync' # campus # drive_id = '2011_09_28_drive_0016_sync' # campus # drive_id = '2011_09_28_drive_0021_sync' # campus # drive_id = '2011_09_28_drive_0034_sync' # # drive_id = '2011_09_28_drive_0037_sync' # # drive_id = '2011_09_28_drive_0038_sync' # # drive_id = '2011_09_28_drive_0039_sync' # busy campus, bad gps # drive_id = '2011_09_28_drive_0043_sync' # # drive_id = '2011_09_28_drive_0045_sync' # # drive_id = '2011_09_28_drive_0179_sync' # # drive_id = '2011_09_29_drive_0026_sync' # # drive_id = '2011_09_29_drive_0071_sync' # # drive_id = '2011_09_30_drive_0020_sync' # # drive_id = '2011_09_30_drive_0027_sync' # (long) # drive_id = '2011_09_30_drive_0028_sync' # (long) bad gps # drive_id = '2011_09_30_drive_0033_sync' # # drive_id = '2011_09_30_drive_0034_sync' # # drive_id = '2011_10_03_drive_0042_sync' # # drive_id = '2011_10_03_drive_0047_sync' # raw_dir = os.path.expanduser('~/Kitti/raw') vtk_window_size = (900, 600) save_images = False point_cloud_source = 'lidar' # Load raw data drive_date = drive_id[0:10] drive_num_str = drive_id[17:21] raw_data = pykitti.raw(raw_dir, drive_date, drive_num_str) # Check that velo length matches timestamps? if len(raw_data.velo_files) != len(raw_data.timestamps): raise ValueError('velo files and timestamps have different length!') frame_range = (0, len(raw_data.timestamps)) # frame_range = (0, 100) camera_viewpoint = 'front' camera_viewpoint = 'elevated' #################### # End of Options #################### vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'Overlaid Point Cloud', vtk_window_size, vtk_renderer) vtk_interactor = vtk.vtkRenderWindowInteractor() vtk_interactor.SetRenderWindow(vtk_render_window) vtk_interactor.SetInteractorStyle( vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer)) vtk_interactor.Initialize() if point_cloud_source != 'lidar': raise ValueError( 'Invalid point cloud source {}'.format(point_cloud_source)) # Load poses from ORBSLAM2 poses = np.loadtxt(raw_dir + '/orbslam2_cam0_poses/{}/{}_cam0_poses.txt'.format( drive_date, drive_id)) poses = poses.reshape((-1, 3, 4)) poses = np.pad(poses, ((0, 0), (0, 1), (0, 0)), mode='constant', constant_values=0) poses[:, 3, 3] = 1.0 # Read calibrations tf_velo_calib_imu_calib = raw_data.calib.T_velo_imu tf_imu_calib_velo_calib = transform_utils.invert_tf( tf_velo_calib_imu_calib) tf_cam0_calib_velo_calib = raw_data.calib.T_cam0_velo # Create VtkAxes vtk_axes = vtk.vtkAxesActor() vtk_axes.SetTotalLength(1, 1, 1) vtk_renderer.AddActor(vtk_axes) for frame_idx in range(*frame_range): # Point cloud actor wrapper vtk_pc = VtkPointCloud() vtk_pc.vtk_actor.GetProperty().SetPointSize(2) # all_vtk_actors.append(vtk_point_cloud.vtk_actor) vtk_renderer.AddActor(vtk_pc.vtk_actor) print('{} / {}'.format(frame_idx, len(raw_data.timestamps) - 1)) # Load next frame data load_start_time = time.time() rgb_image = np.asarray(raw_data.get_cam2(frame_idx)) bgr_image = rgb_image[..., ::-1] if point_cloud_source == 'lidar': velo_points, velo_intensities = get_velo_points( raw_data, frame_idx) # Transform point cloud to cam_0 frame velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]], constant_values=1.0, mode='constant') else: raise ValueError('Invalid point cloud source') print('load\t\t', time.time() - load_start_time) # Project velodyne points projection_start_time = time.time() if point_cloud_source == 'lidar': cam0_curr_pc_padded = tf_cam0_calib_velo_calib @ velo_curr_points_padded.T point_colours = np.repeat( np.round(velo_intensities * 255).astype(np.uint8), 3).reshape(-1, 3) print('projection\t', time.time() - projection_start_time) cam0_ref_pose = poses[frame_idx] tf_cam0_ref_cam0_curr = cam0_ref_pose cam0_ref_pc_padded = tf_cam0_ref_cam0_curr @ cam0_curr_pc_padded # VtkPointCloud vtk_pc_start_time = time.time() vtk_pc.set_points(cam0_ref_pc_padded[0:3].T, point_colours) print('vtk_pc\t\t', time.time() - vtk_pc_start_time) vtk_pc_pose = VtkPointCloud() vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5) vtk_pc_pose.set_points(np.reshape(cam0_ref_pose[0:3, 3], [-1, 3])) # vtk_renderer.AddActor(vtk_pc_pose.vtk_actor) # Move camera if camera_viewpoint == 'front': cam0_curr_vtk_cam_position = [0.0, 0.0, 0.0, 1.0] elif camera_viewpoint == 'elevated': cam0_curr_vtk_cam_position = [0.0, -10.0, -25.0, 1.0] elif camera_viewpoint == '': pass else: raise ValueError('Invalid camera_pos', camera_viewpoint) cam0_ref_vtk_cam_pos = tf_cam0_ref_cam0_curr @ cam0_curr_vtk_cam_position cam0_curr_vtk_cam_fp = [0.0, 0.0, 20.0, 1.0] cam0_ref_vtk_cam_fp = tf_cam0_ref_cam0_curr @ cam0_curr_vtk_cam_fp current_cam = vtk_renderer.GetActiveCamera() # vtk_renderer.ResetCamera() current_cam.SetViewUp(0, -1, 0) current_cam.SetPosition(cam0_ref_vtk_cam_pos[0:3]) current_cam.SetFocalPoint(*cam0_ref_vtk_cam_fp[0:3]) # current_cam.Zoom(0.5) # Reset the clipping range to show all points vtk_renderer.ResetCameraClippingRange() # Render render_start_time = time.time() vtk_render_window.Render() print('render\t\t', time.time() - render_start_time) print('---') print('Done') # Keep window open vtk_interactor.Start()
def main(): #################### # Options #################### odom_dir = os.path.expanduser('~/Kitti/odometry/dataset') # sequence = '01' # sequence = '02' # sequence = '03' # sequence = '04' # sequence = '05' # sequence = '06' # sequence = '08' sequence = '09' # max_fps = 10.0 # vtk_window_size = (1280, 720) vtk_window_size = (960, 540) save_images = False point_cloud_source = 'lidar' # point_cloud_source = 'fast' # point_cloud_source = 'multiscale' # Setup odometry dataset handler odom_dataset = pykitti.odometry(odom_dir, sequence) # # Check that velo length matches timestamps? # if len(odom_dataset.velo_files) != len(odom_dataset.timestamps): # raise ValueError('velo files and timestamps have different length!') frame_range = (0, len(odom_dataset.timestamps)) # frame_range = (0, 200) camera_viewpoint = 'front' camera_viewpoint = 'elevated' render_interval = 4 actor_buffer = [] #################### # End of Options #################### vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'Sequence {}'.format(sequence), vtk_window_size, vtk_renderer) vtk_interactor = vtk.vtkRenderWindowInteractor() vtk_interactor.SetRenderWindow(vtk_render_window) vtk_interactor.SetInteractorStyle( vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer)) vtk_interactor.Initialize() cam_p2 = odom_dataset.calib.P_rect_20 # Load poses cam0_poses = odom_dataset.poses # tf_cam0_calib_velo_calib = odom_dataset.calib.T_cam0_velo # tf_velo_calib_cam0_calib = transform_utils.invert_tf(tf_cam0_calib_velo_calib) # Setup camera if camera_viewpoint == 'front': cam0_curr_vtk_cam_pos = [0.0, 0.0, 0.0, 1.0] cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0] elif camera_viewpoint == 'elevated': cam0_curr_vtk_cam_pos = [0.0, -5.0, -15.0, 1.0] cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0] else: raise ValueError('Invalid camera_pos', camera_viewpoint) # Create VtkAxes vtk_axes = vtk.vtkAxesActor() vtk_axes.SetTotalLength(1, 1, 1) vtk_renderer.AddActor(vtk_axes) all_render_times = [] buffer_points = [] buffer_colours = [] for frame_idx in range(*frame_range): print('{} / {}'.format(frame_idx, len(odom_dataset.timestamps) - 1)) # Load next frame data load_start_time = time.time() rgb_image = np.asarray(odom_dataset.get_cam2(frame_idx)) bgr_image = rgb_image[..., ::-1] if point_cloud_source == 'lidar': velo_points = get_velo_points(odom_dataset, frame_idx) # Transform point cloud to cam_0 frame velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]], constant_values=1.0, mode='constant') # Velo in cam0_curr cam0_curr_pc_all_padded = odom_dataset.calib.T_cam0_velo @ velo_curr_points_padded.T pass # cam0_curr_points = cam0_curr_pc.T elif point_cloud_source in ['fast', 'multiscale']: # depth_map_path = depth_map_dir + '/{:010d}.png'.format(frame_idx) # depth_map = depth_map_utils.read_depth_map(depth_map_path) # points_cam2 = depth_map_utils.get_depth_point_cloud(depth_map, cam_p).T raise NotImplementedError() else: raise ValueError('Invalid point cloud source') print('load\t\t', time.time() - load_start_time) # Project velodyne points projection_start_time = time.time() # points_in_img = calib_utils.project_pc_to_image(points_cam2.T, cam_p) if point_cloud_source == 'lidar': # Project into image2 points_in_img2 = calib_utils.project_pc_to_image( cam0_curr_pc_all_padded[0:3], cam_p2) points_in_img2_int = np.round(points_in_img2).astype(np.int32) image_filter = obj_utils.points_in_img_filter( points_in_img2_int, bgr_image.shape) # image_mask = (points_in_img_int[0] >= 0) & (points_in_img_int[0] < bgr_image.shape[1]) & \ # (points_in_img_int[1] >= 0) & (points_in_img_int[1] < bgr_image.shape[0]) cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter] # cam0_curr_points = cam0_curr_pc_padded[0:3].T # points = points_cam2[image_mask] points_in_img_int_valid = points_in_img2_int[:, image_filter] point_colours = bgr_image[points_in_img_int_valid[1], points_in_img_int_valid[0]] else: raise ValueError('Invalid point_cloud_source', point_cloud_source) print('projection\t', time.time() - projection_start_time) # Get pose cam0_ref_pose = cam0_poses[frame_idx] tf_cam0_ref_cam0_curr = cam0_ref_pose # print('cam0_ref_pose\n', np.round(cam0_ref_pose, 3)) cam0_ref_pc_padded = tf_cam0_ref_cam0_curr @ cam0_curr_pc_padded cam0_ref_points = cam0_ref_pc_padded[0:3].T if render_interval > 1: buffer_points.extend(cam0_ref_points) buffer_colours.extend(point_colours) if frame_idx % render_interval == 0 or frame_idx == (frame_range[1] - 1): vtk_pc = VtkPointCloudGlyph() vtk_pc.vtk_actor.GetProperty().SetPointSize(2) vtk_renderer.AddActor(vtk_pc.vtk_actor) vtk_pc_start_time = time.time() if render_interval > 1: vtk_pc.set_points(buffer_points, buffer_colours) buffer_points = [] buffer_colours = [] else: vtk_pc.set_points(cam0_ref_points, point_colours) print('vtk_pc\t\t', time.time() - vtk_pc_start_time) # Display pose vtk_pc_pose = VtkPointCloud() vtk_pc_pose.vtk_actor.GetProperty().SetPointSize(5) vtk_pc_pose.set_points(np.reshape(cam0_ref_pose[0:3, 3], [-1, 3])) vtk_renderer.AddActor(vtk_pc_pose.vtk_actor) cam0_ref_vtk_cam_pos = tf_cam0_ref_cam0_curr.dot( cam0_curr_vtk_cam_pos) cam0_ref_vtk_focal_point = tf_cam0_ref_cam0_curr.dot( cam0_curr_vtk_focal_point) current_cam = vtk_renderer.GetActiveCamera() vtk_renderer.ResetCamera() current_cam.SetViewUp(0, -1, 0) current_cam.SetPosition(cam0_ref_vtk_cam_pos[0:3]) current_cam.SetFocalPoint(*cam0_ref_vtk_focal_point[0:3]) current_cam.Zoom(0.5) vtk_renderer.ResetCameraClippingRange() # Render render_start_time = time.time() vtk_renderer.GetRenderWindow().Render() print('render\t\t', time.time() - render_start_time) all_render_times.append(time.time() - render_start_time) print('---') actor_buffer.append(vtk_pc.vtk_actor) if len(actor_buffer) > 50: if frame_idx % 10 != 0: actor_buffer[frame_idx - 50].SetVisibility(0) for vtk_actor in actor_buffer: vtk_actor.SetVisibility(1) print('Done') import matplotlib.pyplot as plt plt.plot(all_render_times) plt.show(block=True) # Keep window open vtk_interactor.Start()
def main(): """Comparison of ground truth and ORBSLAM2 poses https://github.com/raulmur/ORB_SLAM2 """ #################### # Options #################### odom_dataset_dir = os.path.expanduser('~/Kitti/odometry/dataset') # sequence = '02' # sequence = '03' # sequence = '08' sequence = '09' # sequence = '10' # sequence = '11' # sequence = '12' # vtk_window_size = (1280, 720) vtk_window_size = (960, 540) # vtk_window_size = (400, 300) point_cloud_source = 'lidar' # point_cloud_source = 'fast' # point_cloud_source = 'multiscale' # first_frame_idx = None # first_frame_pose = None # Setup odometry dataset handler odom_dataset = pykitti.odometry(odom_dataset_dir, sequence) # # Check that velo length matches timestamps? # if len(odom_dataset.velo_files) != len(odom_dataset.timestamps): # raise ValueError('velo files and timestamps have different length!') frame_range = (0, len(odom_dataset.timestamps)) # frame_range = (0, 100) # camera_viewpoint = 'front' camera_viewpoint = 'elevated' # camera_viewpoint = 'bev' buffer_size = 50 buffer_update = 10 save_screenshots = False ############################## vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'Overlaid Point Cloud', vtk_window_size, vtk_renderer) if save_screenshots: vtk_win_to_img_filter, vtk_png_writer = vtk_utils.setup_screenshots( vtk_render_window) vtk_interactor = vtk.vtkRenderWindowInteractor() vtk_interactor.SetRenderWindow(vtk_render_window) vtk_interactor.SetInteractorStyle( vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer)) vtk_interactor.Initialize() cam_p2 = odom_dataset.calib.P_rect_20 # Load poses cam0_ref_poses_orbslam = np.loadtxt( odom_dataset_dir + '/poses_orbslam2/{}.txt'.format(sequence)) cam0_ref_poses_orbslam = np.pad(cam0_ref_poses_orbslam, ((0, 0), (0, 4)), mode='constant') cam0_ref_poses_orbslam[:, -1] = 1 cam0_ref_poses_orbslam = cam0_ref_poses_orbslam.reshape((-1, 4, 4)) cam0_ref_poses_gt = np.asarray(odom_dataset.poses, np.float32) # Setup camera if camera_viewpoint == 'front': cam0_curr_vtk_cam_pos = [0.0, 0.0, 0.0, 1.0] cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0] elif camera_viewpoint == 'elevated': cam0_curr_vtk_cam_pos = [0.0, -5.0, -15.0, 1.0] cam0_curr_vtk_focal_point = [0.0, 0.0, 20.0, 1.0] elif camera_viewpoint == 'bev': # camera_zoom = 1.0 cam0_curr_vtk_cam_pos = [0.0, -50.0, 10.0, 1.0] # camera_pos = (0.0, 0.0, 0.0) cam0_curr_vtk_focal_point = [0.0, 0.0, 15.0, 1.0] else: raise ValueError('Invalid camera_pos', camera_viewpoint) # Create VtkAxes vtk_axes = vtk.vtkAxesActor() vtk_axes.SetTotalLength(2, 2, 2) vtk_renderer.AddActor(vtk_axes) vtk_pc_poses_orbslam = VtkPointCloudGlyph() vtk_pc_poses_gt = VtkPointCloudGlyph() vtk_pc_poses_orbslam.vtk_actor.GetProperty().SetPointSize(5) vtk_pc_poses_gt.vtk_actor.GetProperty().SetPointSize(5) actor_buffer = [] for frame_idx in range(*frame_range): print('{} / {}'.format(frame_idx, len(odom_dataset.timestamps) - 1)) # Load next frame data load_start_time = time.time() rgb_load_start = time.time() bgr_image = cv2.imread(odom_dataset.cam2_files[frame_idx]) print('rgb_load', time.time() - rgb_load_start) if point_cloud_source == 'lidar': velo_curr_points_padded = get_velo_points(odom_dataset, frame_idx) # Velo in cam0_curr cam0_curr_pc_all_padded = odom_dataset.calib.T_cam0_velo @ velo_curr_points_padded.T pass # cam0_curr_points = cam0_curr_pc.T elif point_cloud_source in ['fast', 'multiscale']: # depth_map_path = depth_map_dir + '/{:010d}.png'.format(frame_idx) # depth_map = depth_map_utils.read_depth_map(depth_map_path) # points_cam2 = depth_map_utils.get_depth_point_cloud(depth_map, cam_p).T raise NotImplementedError() else: raise ValueError('Invalid point cloud source') print('load\t\t', time.time() - load_start_time) # Project velodyne points projection_start_time = time.time() if point_cloud_source == 'lidar': # Project into image2 points_in_img2 = calib_utils.project_pc_to_image2( cam0_curr_pc_all_padded, cam_p2) points_in_img2_int = np.round(points_in_img2).astype(np.int32) image_filter = obj_utils.points_in_img_filter( points_in_img2_int, bgr_image.shape) cam0_curr_pc_padded = cam0_curr_pc_all_padded[:, image_filter] # points = points_cam2[image_mask] points_in_img_int_valid = points_in_img2_int[:, image_filter] point_colours = bgr_image[points_in_img_int_valid[1], points_in_img_int_valid[0]] else: raise ValueError('Invalid point_cloud_source', point_cloud_source) print('projection\t', time.time() - projection_start_time) # Get pose cam0_ref_pose_orbslam = cam0_ref_poses_orbslam[frame_idx] cam0_ref_pose_gt = cam0_ref_poses_gt[frame_idx] tf_cam0_ref_cam0_curr = cam0_ref_pose_orbslam cam0_ref_pc_padded = tf_cam0_ref_cam0_curr @ cam0_curr_pc_padded # VtkPointCloud vtk_pc = VtkPointCloudGlyph() vtk_pc.vtk_actor.GetProperty().SetPointSize(2) vtk_pc_start_time = time.time() vtk_pc.set_points(cam0_ref_pc_padded[0:3].T, point_colours) print('vtk_pc\t\t', time.time() - vtk_pc_start_time) # Display orbslam pose vtk_pc_poses_orbslam.set_points( cam0_ref_poses_orbslam[max(0, frame_idx - buffer_size):frame_idx + 1, 0:3, 3], np.tile([255, 0, 0], [buffer_size, 1])) # Display gt pose vtk_pc_poses_gt.vtk_actor.GetProperty().SetPointSize(5) vtk_pc_poses_gt.set_points( cam0_ref_poses_gt[max(0, frame_idx - buffer_size):frame_idx + 1, 0:3, 3], np.tile([0, 255, 0], [buffer_size, 1])) # Add vtk actors vtk_renderer.AddActor(vtk_pc.vtk_actor) vtk_renderer.AddActor(vtk_pc_poses_orbslam.vtk_actor) vtk_renderer.AddActor(vtk_pc_poses_gt.vtk_actor) cam0_ref_vtk_cam_pos = tf_cam0_ref_cam0_curr.dot(cam0_curr_vtk_cam_pos) cam0_ref_vtk_focal_point = tf_cam0_ref_cam0_curr.dot( cam0_curr_vtk_focal_point) current_cam = vtk_renderer.GetActiveCamera() vtk_renderer.ResetCamera() current_cam.SetViewUp(0, -1, 0) current_cam.SetPosition(cam0_ref_vtk_cam_pos[0:3]) current_cam.SetFocalPoint(*cam0_ref_vtk_focal_point[0:3]) current_cam.Zoom(0.5) vtk_renderer.ResetCameraClippingRange() # Render render_start_time = time.time() vtk_render_window.Render() print('render\t\t', time.time() - render_start_time) actor_buffer.append(vtk_pc.vtk_actor) if len(actor_buffer) > buffer_size: if frame_idx % buffer_update != 0: actor_buffer[frame_idx - buffer_size].SetVisibility(0) if save_screenshots: screenshot_start_time = time.time() screenshot_path = core.data_dir() + '/temp/{:010d}.png'.format( frame_idx) vtk_utils.save_screenshot(screenshot_path, vtk_win_to_img_filter, vtk_png_writer) print('screenshot\t', time.time() - screenshot_start_time) print('---') for vtk_actor in actor_buffer: vtk_actor.SetVisibility(1) print('Done') # Keep window open vtk_interactor.Start()
def main(): ############################## # Options ############################## """Note: Run scripts/depth_completion/save_depth_maps_raw.py first. Demo to show depth completed point clouds """ raw_dir = os.path.expanduser('~/Kitti/raw') drive_id = '2011_09_26_drive_0023_sync' vtk_window_size = (1280, 720) max_fps = 30.0 # point_cloud_source = 'lidar' # fill_type = None point_cloud_source = 'depth' fill_type = 'multiscale' # Load raw data drive_date = drive_id[0:10] drive_num_str = drive_id[17:21] raw_data = pykitti.raw(raw_dir, drive_date, drive_num_str) # Check that velo length matches timestamps? if len(raw_data.velo_files) != len(raw_data.timestamps): raise ValueError('velo files and timestamps have different length!') frame_range = (0, len(raw_data.timestamps)) ############################## min_loop_time = 1.0 / max_fps vtk_renderer = demo_utils.setup_vtk_renderer() vtk_render_window = demo_utils.setup_vtk_render_window( 'Overlaid Point Cloud', vtk_window_size, vtk_renderer) # Setup camera current_cam = vtk_renderer.GetActiveCamera() current_cam.SetViewUp(0, 0, 1) current_cam.SetPosition(0.0, -8.0, -15.0) current_cam.SetFocalPoint(0.0, 0.0, 20.0) current_cam.Zoom(0.7) # Create VtkAxes vtk_axes = vtk.vtkAxesActor() vtk_axes.SetTotalLength(2, 2, 2) # Setup interactor vtk_interactor = vtk.vtkRenderWindowInteractor() vtk_interactor.SetRenderWindow(vtk_render_window) vtk_interactor.SetInteractorStyle( vtk_utils.ToggleActorsInteractorStyle(None, vtk_renderer, current_cam, vtk_axes)) vtk_interactor.Initialize() if point_cloud_source not in ['lidar', 'depth']: raise ValueError( 'Invalid point cloud source {}'.format(point_cloud_source)) cam_p = raw_data.calib.P_rect_20 # Point cloud vtk_pc = VtkPointCloudGlyph() # Add actors vtk_renderer.AddActor(vtk_axes) vtk_renderer.AddActor(vtk_pc.vtk_actor) for frame_idx in range(*frame_range): loop_start_time = time.time() print('{} / {}'.format(frame_idx, len(raw_data.timestamps) - 1)) # Load next frame data load_start_time = time.time() rgb_image = np.asarray(raw_data.get_cam2(frame_idx)) bgr_image = rgb_image[..., ::-1] if point_cloud_source == 'lidar': velo_points = get_velo_points(raw_data, frame_idx) # Transform point cloud to cam_0 frame velo_curr_points_padded = np.pad(velo_points, [[0, 0], [0, 1]], constant_values=1.0, mode='constant') cam0_curr_pc_all_padded = raw_data.calib.T_cam0_velo @ velo_curr_points_padded.T # Project velodyne points projection_start_time = time.time() points_in_img = calib_utils.project_pc_to_image( cam0_curr_pc_all_padded[0:3], cam_p) points_in_img_int = np.round(points_in_img).astype(np.int32) print('projection\t', time.time() - projection_start_time) image_filter = obj_utils.points_in_img_filter( points_in_img_int, bgr_image.shape) cam0_curr_pc = cam0_curr_pc_all_padded[0:3, image_filter] points_in_img_int_valid = points_in_img_int[:, image_filter] point_colours = bgr_image[points_in_img_int_valid[1], points_in_img_int_valid[0]] elif point_cloud_source == 'depth': depth_maps_dir = core.data_dir() + \ '/depth_completion/raw/{}/depth_02_{}'.format(drive_id, fill_type) depth_map_path = depth_maps_dir + '/{:010d}.png'.format(frame_idx) depth_map = depth_map_utils.read_depth_map(depth_map_path) cam0_curr_pc = depth_map_utils.get_depth_point_cloud( depth_map, cam_p) point_colours = bgr_image.reshape(-1, 3) # Mask to valid points valid_mask = (cam0_curr_pc[2] != 0) cam0_curr_pc = cam0_curr_pc[:, valid_mask] point_colours = point_colours[valid_mask] else: raise ValueError('Invalid point cloud source') print('load\t\t', time.time() - load_start_time) # VtkPointCloud vtk_pc_start_time = time.time() vtk_pc.set_points(cam0_curr_pc.T, point_colours) print('vtk_pc\t\t', time.time() - vtk_pc_start_time) # Reset the clipping range to show all points vtk_renderer.ResetCameraClippingRange() # Render render_start_time = time.time() vtk_render_window.Render() print('render\t\t', time.time() - render_start_time) # Pause to keep frame rate under max loop_run_time = time.time() - loop_start_time print('loop\t\t', loop_run_time) if loop_run_time < min_loop_time: time.sleep(min_loop_time - loop_run_time) print('---') print('Done') # Keep window open vtk_interactor.Start()