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)
default='vbg.npz')
config = parser.get_config()
if config.path_dataset == '':
config = get_default_dataset(config)
# Extract RGB-D frames and intrinsic from bag file.
if config.path_dataset.endswith(".bag"):
assert os.path.isfile(
config.path_dataset), (f"File {config.path_dataset} not found.")
print("Extracting frames from RGBD video file")
config.path_dataset, config.path_intrinsic, config.depth_scale = extract_rgbd_frames(
config.path_dataset)
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
intrinsic = load_intrinsic(config)
extrinsics = load_extrinsics(config.path_trajectory, config)
vbg = integrate(depth_file_names, color_file_names, intrinsic, extrinsics,
config)
mesh = vbg.extract_triangle_mesh()
o3d.visualization.draw([mesh.to_legacy()])
pcd = vbg.extract_point_cloud()
o3d.visualization.draw([pcd])