model.update_frame_pose(i, T_frame_to_model)
model.integrate(input_frame, config.depth_scale, config.depth_max)
model.synthesize_model_frame(raycast_frame, config.depth_scale,
config.depth_min, config.depth_max, False)
stop = time.time()
print('{:04d}/{:04d} slam takes {:.4}s'.format(i, n_files,
stop - start))
return model.voxel_grid, poses
if __name__ == '__main__':
parser = ConfigParser()
parser.add(
'--config',
is_config_file=True,
help='YAML config file path. Please refer to default_config.yml as a '
'reference. It overrides the default config file, but will be '
'overridden by other command line inputs.')
config = parser.get_config()
depth_file_names, color_file_names = load_image_file_names(config)
intrinsic = load_intrinsic(config)
volume, poses = slam(depth_file_names, color_file_names, intrinsic, config)
save_poses('output.log', poses)
save_poses('output.json', poses)
mesh = extract_trianglemesh(volume, config, 'output.ply')
o3d.visualization.draw([mesh])
def update_main(self):
depth_file_names, color_file_names = load_rgbd_file_names(self.config)
intrinsic = load_intrinsic(self.config)
n_files = len(color_file_names)
device = o3d.core.Device(config.device)
T_frame_to_model = o3c.Tensor(np.identity(4))
depth_ref = o3d.t.io.read_image(depth_file_names[0])
color_ref = o3d.t.io.read_image(color_file_names[0])
input_frame = o3d.t.pipelines.slam.Frame(depth_ref.rows,
depth_ref.columns, intrinsic,
device)
raycast_frame = o3d.t.pipelines.slam.Frame(depth_ref.rows,
depth_ref.columns,
intrinsic, device)
input_frame.set_data_from_image('depth', depth_ref)
input_frame.set_data_from_image('color', color_ref)
raycast_frame.set_data_from_image('depth', depth_ref)
raycast_frame.set_data_from_image('color', color_ref)
gui.Application.instance.post_to_main_thread(
self.window, lambda: self.init_render(depth_ref, color_ref))
fps_interval_len = 30
self.idx = 0
pcd = None
start = time.time()
while not self.is_done:
if not self.is_started or not self.is_running:
time.sleep(0.05)
continue
depth = o3d.t.io.read_image(depth_file_names[self.idx]).to(device)
color = o3d.t.io.read_image(color_file_names[self.idx]).to(device)
input_frame.set_data_from_image('depth', depth)
input_frame.set_data_from_image('color', color)
if self.idx > 0:
result = self.model.track_frame_to_model(
input_frame,
raycast_frame,
float(self.scale_slider.int_value),
self.max_slider.double_value,
)
T_frame_to_model = T_frame_to_model @ result.transformation
self.poses.append(T_frame_to_model.cpu().numpy())
self.model.update_frame_pose(self.idx, T_frame_to_model)
self.model.integrate(input_frame,
float(self.scale_slider.int_value),
self.max_slider.double_value,
self.trunc_multiplier_slider.double_value)
self.model.synthesize_model_frame(
raycast_frame, float(self.scale_slider.int_value),
config.depth_min, self.max_slider.double_value,
self.trunc_multiplier_slider.double_value,
self.raycast_box.checked)
if (self.idx % self.interval_slider.int_value == 0 and
self.update_box.checked) \
or (self.idx == 3) \
or (self.idx == n_files - 1):
pcd = self.model.voxel_grid.extract_point_cloud(
3.0, self.est_point_count_slider.int_value).to(
o3d.core.Device('CPU:0'))
self.is_scene_updated = True
else:
self.is_scene_updated = False
frustum = o3d.geometry.LineSet.create_camera_visualization(
color.columns, color.rows, intrinsic.numpy(),
np.linalg.inv(T_frame_to_model.cpu().numpy()), 0.2)
frustum.paint_uniform_color([0.961, 0.475, 0.000])
# Output FPS
if (self.idx % fps_interval_len == 0):
end = time.time()
elapsed = end - start
start = time.time()
self.output_fps.text = 'FPS: {:.3f}'.format(fps_interval_len /
elapsed)
# Output info
info = 'Frame {}/{}\n\n'.format(self.idx, n_files)
info += 'Transformation:\n{}\n'.format(
np.array2string(T_frame_to_model.numpy(),
precision=3,
max_line_width=40,
suppress_small=True))
info += 'Active voxel blocks: {}/{}\n'.format(
self.model.voxel_grid.hashmap().size(),
self.model.voxel_grid.hashmap().capacity())
info += 'Surface points: {}/{}\n'.format(
0 if pcd is None else pcd.point['positions'].shape[0],
self.est_point_count_slider.int_value)
self.output_info.text = info
gui.Application.instance.post_to_main_thread(
self.window, lambda: self.update_render(
input_frame.get_data_as_image('depth'),
input_frame.get_data_as_image('color'),
raycast_frame.get_data_as_image('depth'),
raycast_frame.get_data_as_image('color'), pcd, frustum))
self.idx += 1
self.is_done = self.is_done | (self.idx >= n_files)
time.sleep(0.5)
'overridden by other command line inputs.')
parser.add('--integrate_color', action='store_true')
parser.add('--path_trajectory',
help='path to the trajectory .log or .json file.')
parser.add('--path_npz',
help='path to the npz file that stores voxel block grid.',
default='vbg.npz')
config = parser.get_config()
if config.path_dataset == '':
config = get_default_testdata(config)
if config.integrate_color:
depth_file_names, color_file_names = load_rgbd_file_names(config)
else:
depth_file_names = load_depth_file_names(config)
color_file_names = None
depth_intrinsic = load_intrinsic(config)
color_intrinsic = load_intrinsic(config, 'color')
extrinsics = load_extrinsics(config.path_trajectory, config)
vbg = integrate(depth_file_names, color_file_names, depth_intrinsic,
color_intrinsic, extrinsics, config)
pcd = vbg.extract_point_cloud()
o3d.visualization.draw([pcd])
mesh = vbg.extract_triangle_mesh()
o3d.visualization.draw([mesh.to_legacy()])