1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
|
# ----------------------------------------------------------------------------
# - Open3D: www.open3d.org -
# ----------------------------------------------------------------------------
# Copyright (c) 2018-2024 www.open3d.org
# SPDX-License-Identifier: MIT
# ----------------------------------------------------------------------------
# examples/python/t_reconstruction_system/dense_slam.py
# P.S. This example is used in documentation, so, please ensure the changes are
# synchronized.
import os
import numpy as np
import open3d as o3d
import time
from config import ConfigParser
from common import (get_default_dataset, load_rgbd_file_names, save_poses,
load_intrinsic, extract_trianglemesh, extract_rgbd_frames)
def slam(depth_file_names, color_file_names, intrinsic, config):
n_files = len(color_file_names)
device = o3d.core.Device(config.device)
T_frame_to_model = o3d.core.Tensor(np.identity(4))
model = o3d.t.pipelines.slam.Model(config.voxel_size, 16,
config.block_count, T_frame_to_model,
device)
depth_ref = o3d.t.io.read_image(depth_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)
poses = []
for i in range(n_files):
start = time.time()
depth = o3d.t.io.read_image(depth_file_names[i]).to(device)
color = o3d.t.io.read_image(color_file_names[i]).to(device)
input_frame.set_data_from_image('depth', depth)
input_frame.set_data_from_image('color', color)
if i > 0:
result = model.track_frame_to_model(input_frame, raycast_frame,
config.depth_scale,
config.depth_max,
config.odometry_distance_thr)
T_frame_to_model = T_frame_to_model @ result.transformation
poses.append(T_frame_to_model.cpu().numpy())
model.update_frame_pose(i, T_frame_to_model)
model.integrate(input_frame, config.depth_scale, config.depth_max,
config.trunc_voxel_multiplier)
model.synthesize_model_frame(raycast_frame, config.depth_scale,
config.depth_min, config.depth_max,
config.trunc_voxel_multiplier, 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.')
parser.add('--default_dataset',
help='Default dataset is used when config file is not provided. '
'Default dataset may be selected from the following options: '
'[lounge, bedroom, jack_jack]',
default='lounge')
parser.add('--path_npz',
help='path to the npz file that stores voxel block grid.',
default='output.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)
depth_file_names, color_file_names = load_rgbd_file_names(config)
intrinsic = load_intrinsic(config)
if not os.path.exists(config.path_npz):
volume, poses = slam(depth_file_names, color_file_names, intrinsic,
config)
print('Saving to {}...'.format(config.path_npz))
volume.save(config.path_npz)
save_poses('output.log', poses)
print('Saving finished')
else:
volume = o3d.t.geometry.VoxelBlockGrid.load(config.path_npz)
mesh = extract_trianglemesh(volume, config, 'output.ply')
o3d.visualization.draw([mesh])
|
