def main(args): # if args.dataset == 'kitti': dataset = KITTIOdometry(args.path) # elif args.dataset == 'vkitti': # dataset = VKITTIOdometry(args.vkitti_path) # else: # return ValueError() params = ParamsKITTI() cam = Camera( dataset.cam.fx, dataset.cam.fy, dataset.cam.cx, dataset.cam.cy, dataset.cam.width, dataset.cam.height, params.frustum_near, params.frustum_far, dataset.cam.baseline) tracker = Tracker(params, cam) viewer = MapViewer(tracker, params) if args.view_map: viewer.load_points() return timer = RunningAverageTimer() durations = [] try: for i in range(len(dataset)): j = i + 800 # Data loading takes 0.036s left = dataset.left[j] right = dataset.right[j] print('track') tracker.update(i, left, right, timestamp=dataset.timestamps[j]) print('update') # Viewer update takes 0.002s viewer.update(refresh=False) print(f'Frame {j}') # input("Press any key to continue...") # import time # time.sleep(2) except: pass finally: viewer.stop() viewer.save_points()
parser.add_argument('--output', type=str, help='storing output values') args = parser.parse_args() if args.dataset.lower() == 'kitti': params = ParamsKITTI() dataset = KITTIOdometry(args.path) elif args.dataset.lower() == 'euroc': params = ParamsEuroc() dataset = EuRoCDataset(args.path) sptam = SPTAM(params) visualize = not args.no_viz if visualize: from viewer import MapViewer viewer = MapViewer(sptam, params) cam = Camera( dataset.cam.fx, dataset.cam.fy, dataset.cam.cx, dataset.cam.cy, dataset.cam.width, dataset.cam.height, params.frustum_near, params.frustum_far, dataset.cam.baseline) durations = [] for i in range(len(dataset)): featurel = ImageFeature(dataset.left[i], params) featurer = ImageFeature(dataset.right[i], params) timestamp = dataset.timestamps[i]
def main(args): if args.presort_poses: presort(args.cameras_file_path) if args.refined_depth_dir is not None: if os.path.exists(args.refined_depth_dir): shutil.rmtree(args.refined_depth_dir) shutil.copytree(args.depth_dir, args.refined_depth_dir) # read camera calibration cam = load_calibration(args.intrinsic) # cameras and depths paths cameras_file_path = args.cameras_file_path depth_file_paths = sorted(glob.glob(os.path.join(args.depth_dir, '*.depth.bin'))) if not depth_file_paths: depth_file_paths = sorted(glob.glob(os.path.join(args.depth_dir, '*.bin'))) # image file paths image_names = sorted(os.listdir(args.image_dir)) image_file_paths = [os.path.join(args.image_dir, name) for name in image_names] n_images = len(image_file_paths) assert len(depth_file_paths) == n_images # depth images, these are user provided depth images to show in UI. By default None. depth_image_file_paths = None if args.depth_image_dir: depth_image_names = sorted(os.listdir(args.depth_image_dir)) depth_image_file_paths = [os.path.join(args.depth_image_dir, name) for name in depth_image_names] # initialize system and map viewer system = MySystem(local_window_size=args.local_window_size) if args.use_viewer: print('use_viewer = True, import MapViewer') from viewer import MapViewer viewer = MapViewer(system, w=cam.width, h=cam.height) current_frame_id = -1 count_kf = 0 start_time = time.time() pure_computing_time = 0.0 file = open(cameras_file_path, 'r') for i in range(n_images): print('[{:3d}/{:3d}] '.format(i+1,n_images), end='') # 1) ------- Load Relative Camera pose if current_frame_id < i: m = file.readline() m = m.split(' ') if len(m) < 5: print("End of Sequence!") if args.refined_depth_dir is not None: write_all_keyframes(system, args.refined_depth_dir) break if len(m) > 12: current_frame_id = int(m[12]) if current_frame_id > i: print('') continue matrix = np.identity(4) matrix[0, :] = m[0:4] matrix[1, :] = m[4:8] matrix[2, :] = m[8:12] if system.current_keyframe is not None: relative_pose_matrix = np.matmul(matrix, np.linalg.inv(system.current_keyframe.pose_matrix)) else: relative_pose_matrix = matrix # 2) ------- Load Depth Map depth_file_path = depth_file_paths[i] _, depth = load_depth( depth_file_path, cam.height, cam.width, fx=cam.fx, fy=cam.fy, cx=cam.cx, cy=cam.cy ) # 3) ------- Load Image if args.use_viewer and image_file_paths is not None: image = Image.open(image_file_paths[i]) try: image = image.resize((depth.shape[1],depth.shape[0])) except: image_np = np.array(image) image = Image.fromarray(image_np) image = image.resize((depth.shape[1],depth.shape[0])) image = np.array(image) else: image = None # 4) ------- Load Depth Image if args.use_viewer and depth_image_file_paths is not None: depth_image = Image.open(depth_image_file_paths[i]) try: depth_image = depth_image.resize((depth.shape[1],depth.shape[0])) except: depth_image_np = np.array(depth_image) depth_image = Image.fromarray(depth_image_np) depth_image = depth_image.resize((depth.shape[1],depth.shape[0])) depth_image = np.array(depth_image) depth_image = depth_image[:,:,:3] else: depth_image = None # 5) ------- Instantiate keyframe keyframe = Frame( idx=i, cam=cam, depth=depth, relative_pose_matrix=relative_pose_matrix, preceding_keyframe=system.current_keyframe, image=image, depth_image=depth_image, name=os.path.basename(depth_file_path) ) # 6) ------- Add Keyframe, Do Windowed Averaging pure_computing_time += system.add_keyframe(keyframe, refined_depth_dir=args.refined_depth_dir) count_kf += 1 if args.use_viewer: system.refresh_display_content_window(add_pose=True) viewer.update() if i==len(depth_file_paths)-1 and args.refined_depth_dir is not None: write_all_keyframes(system, args.refined_depth_dir) time_elapsed = time.time() - start_time print('Total Time: {}'.format(time_elapsed)) print('{:.2f}ms per frame, {:.2f} frames per second'.format(time_elapsed*1000/len(depth_file_paths), len(depth_file_paths)/time_elapsed)) print('{:.2f}ms per keyframe, {:.2f} keyframes per second'.format(time_elapsed*1000/count_kf, count_kf/time_elapsed)) print('Pure Computing Time: {}'.format(pure_computing_time)) print('{:.2f}ms per frame, {:.2f} frames per second'.format(pure_computing_time*1000/len(depth_file_paths), len(depth_file_paths)/pure_computing_time)) print('{:.2f}ms per keyframe, {:.2f} keyframes per second'.format(pure_computing_time*1000/count_kf, count_kf/pure_computing_time)) file.close()