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()