Make fragments
The first step of the scene reconstruction system is to create fragments from short RGBD sequences.
Input arguments
The script runs with python run_system.py [config] --make
. In [config]
, ["path_dataset"]
should have subfolders image
and depth
to store the color images and depth images respectively. We assume the color images and the depth images are synchronized and registered. In [config]
, the optional argument ["path_intrinsic"]
specifies the path to a json file that stores the camera intrinsic matrix (See Read camera intrinsic for details). If it is not given, the PrimeSense factory setting is used instead.
Register RGBD image pairs
33# examples/Python/ReconstructionSystem/make_fragments.py
34def register_one_rgbd_pair(s, t, color_files, depth_files, intrinsic,
35 with_opencv, config):
36 source_rgbd_image = read_rgbd_image(color_files[s], depth_files[s], True,
37 config)
38 target_rgbd_image = read_rgbd_image(color_files[t], depth_files[t], True,
39 config)
40
41 option = o3d.odometry.OdometryOption()
42 option.max_depth_diff = config["max_depth_diff"]
43 if abs(s - t) is not 1:
44 if with_opencv:
45 success_5pt, odo_init = pose_estimation(source_rgbd_image,
46 target_rgbd_image,
47 intrinsic, False)
48 if success_5pt:
49 [success, trans, info] = o3d.odometry.compute_rgbd_odometry(
50 source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
51 o3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
52 return [success, trans, info]
53 return [False, np.identity(4), np.identity(6)]
54 else:
55 odo_init = np.identity(4)
56 [success, trans, info] = o3d.odometry.compute_rgbd_odometry(
57 source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
58 o3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
59 return [success, trans, info]
The function reads a pair of RGBD images and registers the source_rgbd_image
to the target_rgbd_image
. Open3D function compute_rgbd_odometry
is called to align the RGBD images. For adjacent RGBD images, an identity matrix is used as initialization. For non-adjacent RGBD images, wide baseline matching is used as an initialization. In particular, function pose_estimation
computes OpenCV ORB feature to match sparse features over wide baseline images, then performs 5-point RANSAC to estimate a rough alignment, which is used as the initialization of compute_rgbd_odometry
.
Multiway registration
61# examples/Python/ReconstructionSystem/make_fragments.py
62def make_posegraph_for_fragment(path_dataset, sid, eid, color_files,
63 depth_files, fragment_id, n_fragments,
64 intrinsic, with_opencv, config):
65 o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Error)
66 pose_graph = o3d.registration.PoseGraph()
67 trans_odometry = np.identity(4)
68 pose_graph.nodes.append(o3d.registration.PoseGraphNode(trans_odometry))
69 for s in range(sid, eid):
70 for t in range(s + 1, eid):
71 # odometry
72 if t == s + 1:
73 print(
74 "Fragment %03d / %03d :: RGBD matching between frame : %d and %d"
75 % (fragment_id, n_fragments - 1, s, t))
76 [success, trans,
77 info] = register_one_rgbd_pair(s, t, color_files, depth_files,
78 intrinsic, with_opencv, config)
79 trans_odometry = np.dot(trans, trans_odometry)
80 trans_odometry_inv = np.linalg.inv(trans_odometry)
81 pose_graph.nodes.append(
82 o3d.registration.PoseGraphNode(trans_odometry_inv))
83 pose_graph.edges.append(
84 o3d.registration.PoseGraphEdge(s - sid,
85 t - sid,
86 trans,
87 info,
88 uncertain=False))
89
90 # keyframe loop closure
91 if s % config['n_keyframes_per_n_frame'] == 0 \
92 and t % config['n_keyframes_per_n_frame'] == 0:
93 print(
94 "Fragment %03d / %03d :: RGBD matching between frame : %d and %d"
95 % (fragment_id, n_fragments - 1, s, t))
96 [success, trans,
97 info] = register_one_rgbd_pair(s, t, color_files, depth_files,
98 intrinsic, with_opencv, config)
99 if success:
100 pose_graph.edges.append(
101 o3d.registration.PoseGraphEdge(s - sid,
102 t - sid,
103 trans,
104 info,
105 uncertain=True))
106 o3d.io.write_pose_graph(
107 join(path_dataset, config["template_fragment_posegraph"] % fragment_id),
108 pose_graph)
This script uses the technique demonstrated in Multiway registration. Function make_posegraph_for_fragment
builds a pose graph for multiway registration of all RGBD images in this sequence. Each graph node represents an RGBD image and its pose which transforms the geometry to the global fragment space. For efficiency, only key frames are used.
Once a pose graph is created, multiway registration is performed by calling function optimize_posegraph_for_fragment
.
12# examples/Python/ReconstructionSystem/optimize_posegraph.py
13def run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
14 max_correspondence_distance,
15 preference_loop_closure):
16 # to display messages from o3d.registration.global_optimization
17 o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
18 method = o3d.registration.GlobalOptimizationLevenbergMarquardt()
19 criteria = o3d.registration.GlobalOptimizationConvergenceCriteria()
20 option = o3d.registration.GlobalOptimizationOption(
21 max_correspondence_distance=max_correspondence_distance,
22 edge_prune_threshold=0.25,
23 preference_loop_closure=preference_loop_closure,
24 reference_node=0)
25 pose_graph = o3d.io.read_pose_graph(pose_graph_name)
26 o3d.registration.global_optimization(pose_graph, method, criteria, option)
27 o3d.io.write_pose_graph(pose_graph_optimized_name, pose_graph)
28 o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Error)
29
30
31def optimize_posegraph_for_fragment(path_dataset, fragment_id, config):
32 pose_graph_name = join(path_dataset,
33 config["template_fragment_posegraph"] % fragment_id)
34 pose_graph_optimized_name = join(
35 path_dataset,
36 config["template_fragment_posegraph_optimized"] % fragment_id)
37 run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
38 max_correspondence_distance = config["max_depth_diff"],
39 preference_loop_closure = \
40 config["preference_loop_closure_odometry"])
This function calls global_optimization
to estimate poses of the RGBD images.
Make a fragment
110# examples/Python/ReconstructionSystem/make_fragments.py
111def integrate_rgb_frames_for_fragment(color_files, depth_files, fragment_id,
112 n_fragments, pose_graph_name, intrinsic,
113 config):
114 pose_graph = o3d.io.read_pose_graph(pose_graph_name)
115 volume = o3d.integration.ScalableTSDFVolume(
116 voxel_length=config["tsdf_cubic_size"] / 512.0,
117 sdf_trunc=0.04,
118 color_type=o3d.integration.TSDFVolumeColorType.RGB8)
119 for i in range(len(pose_graph.nodes)):
120 i_abs = fragment_id * config['n_frames_per_fragment'] + i
121 print(
122 "Fragment %03d / %03d :: integrate rgbd frame %d (%d of %d)." %
123 (fragment_id, n_fragments - 1, i_abs, i + 1, len(pose_graph.nodes)))
124 rgbd = read_rgbd_image(color_files[i_abs], depth_files[i_abs], False,
125 config)
126 pose = pose_graph.nodes[i].pose
127 volume.integrate(rgbd, intrinsic, np.linalg.inv(pose))
128 mesh = volume.extract_triangle_mesh()
129 mesh.compute_vertex_normals()
130 return mesh
131
132
133def make_pointcloud_for_fragment(path_dataset, color_files, depth_files,
134 fragment_id, n_fragments, intrinsic, config):
135 mesh = integrate_rgb_frames_for_fragment(
136 color_files, depth_files, fragment_id, n_fragments,
137 join(path_dataset,
138 config["template_fragment_posegraph_optimized"] % fragment_id),
139 intrinsic, config)
140 pcd = o3d.geometry.PointCloud()
141 pcd.points = mesh.vertices
142 pcd.colors = mesh.vertex_colors
143 pcd_name = join(path_dataset,
144 config["template_fragment_pointcloud"] % fragment_id)
145 o3d.io.write_point_cloud(pcd_name, pcd, False, True)
Once the poses are estimates, RGBD integration is used to reconstruct a colored fragment from each RGBD sequence.
Batch processing
167# examples/Python/ReconstructionSystem/make_fragments.py
168def run(config):
169 print("making fragments from RGBD sequence.")
170 make_clean_folder(join(config["path_dataset"], config["folder_fragment"]))
171 [color_files, depth_files] = get_rgbd_file_lists(config["path_dataset"])
172 n_files = len(color_files)
173 n_fragments = int(math.ceil(float(n_files) / \
174 config['n_frames_per_fragment']))
175
176 if config["python_multi_threading"]:
177 from joblib import Parallel, delayed
178 import multiprocessing
179 import subprocess
180 MAX_THREAD = min(multiprocessing.cpu_count(), n_fragments)
181 Parallel(n_jobs=MAX_THREAD)(delayed(process_single_fragment)(
182 fragment_id, color_files, depth_files, n_files, n_fragments, config)
183 for fragment_id in range(n_fragments))
184 else:
185 for fragment_id in range(n_fragments):
186 process_single_fragment(fragment_id, color_files, depth_files,
187 n_files, n_fragments, config)
The main function calls each individual function explained above.
Results
Fragment 000 / 013 :: RGBD matching between frame : 0 and 1
Fragment 000 / 013 :: RGBD matching between frame : 0 and 5
Fragment 000 / 013 :: RGBD matching between frame : 0 and 10
Fragment 000 / 013 :: RGBD matching between frame : 0 and 15
Fragment 000 / 013 :: RGBD matching between frame : 0 and 20
:
Fragment 000 / 013 :: RGBD matching between frame : 95 and 96
Fragment 000 / 013 :: RGBD matching between frame : 96 and 97
Fragment 000 / 013 :: RGBD matching between frame : 97 and 98
Fragment 000 / 013 :: RGBD matching between frame : 98 and 99
The following is a log from optimize_posegraph_for_fragment
.
[GlobalOptimizationLM] Optimizing PoseGraph having 100 nodes and 195 edges.
Line process weight : 389.309502
[Initial ] residual : 3.223357e+05, lambda : 1.771814e+02
[Iteration 00] residual : 1.721845e+04, valid edges : 157, time : 0.022 sec.
[Iteration 01] residual : 1.350251e+04, valid edges : 168, time : 0.017 sec.
:
[Iteration 32] residual : 9.779118e+03, valid edges : 179, time : 0.013 sec.
Current_residual - new_residual < 1.000000e-06 * current_residual
[GlobalOptimizationLM] total time : 0.519 sec.
[GlobalOptimizationLM] Optimizing PoseGraph having 100 nodes and 179 edges.
Line process weight : 398.292104
[Initial ] residual : 5.120047e+03, lambda : 2.565362e+02
[Iteration 00] residual : 5.064539e+03, valid edges : 179, time : 0.014 sec.
[Iteration 01] residual : 5.037665e+03, valid edges : 178, time : 0.015 sec.
:
[Iteration 11] residual : 5.017307e+03, valid edges : 177, time : 0.013 sec.
Current_residual - new_residual < 1.000000e-06 * current_residual
[GlobalOptimizationLM] total time : 0.197 sec.
CompensateReferencePoseGraphNode : reference : 0
The following is a log from integrate_rgb_frames_for_fragment
.
Fragment 000 / 013 :: integrate rgbd frame 0 (1 of 100).
Fragment 000 / 013 :: integrate rgbd frame 1 (2 of 100).
Fragment 000 / 013 :: integrate rgbd frame 2 (3 of 100).
:
Fragment 000 / 013 :: integrate rgbd frame 97 (98 of 100).
Fragment 000 / 013 :: integrate rgbd frame 98 (99 of 100).
Fragment 000 / 013 :: integrate rgbd frame 99 (100 of 100).
The following images show some of the fragments made by this script.



