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 /tutorial/pipelines/rgbd_odometry.ipynb#read-camera-intrinsic for details). If it is not given, the PrimeSense factory setting is used instead.

Register RGBD image pairs

58# examples/python/reconstruction_system/make_fragments.py
59
60def register_one_rgbd_pair(s, t, color_files, depth_files, intrinsic,
61                           with_opencv, config):
62    source_rgbd_image = read_rgbd_image(color_files[s], depth_files[s], True,
63                                        config)
64    target_rgbd_image = read_rgbd_image(color_files[t], depth_files[t], True,
65                                        config)
66
67    option = o3d.pipelines.odometry.OdometryOption()
68    option.max_depth_diff = config["max_depth_diff"]
69    if abs(s - t) != 1:
70        if with_opencv:
71            success_5pt, odo_init = pose_estimation(source_rgbd_image,
72                                                    target_rgbd_image,
73                                                    intrinsic, False)
74            if success_5pt:
75                [success, trans, info
76                ] = o3d.pipelines.odometry.compute_rgbd_odometry(
77                    source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
78                    o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(),
79                    option)
80                return [success, trans, info]
81        return [False, np.identity(4), np.identity(6)]
82    else:
83        odo_init = np.identity(4)
84        [success, trans, info] = o3d.pipelines.odometry.compute_rgbd_odometry(
85            source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
86            o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
87        return [success, trans, info]
88

The function reads a pair of RGBD images and registers the source_rgbd_image to the target_rgbd_image. The Open3D function compute_rgbd_odometry is called to align the RGBD images. For adjacent RGBD images, an identity matrix is used as the initialization. For non-adjacent RGBD images, wide baseline matching is used as the initialization. In particular, the 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

 88# examples/python/reconstruction_system/make_fragments.py
 89
 90def make_posegraph_for_fragment(path_dataset, sid, eid, color_files,
 91                                depth_files, fragment_id, n_fragments,
 92                                intrinsic, with_opencv, config):
 93    o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Error)
 94    pose_graph = o3d.pipelines.registration.PoseGraph()
 95    trans_odometry = np.identity(4)
 96    pose_graph.nodes.append(
 97        o3d.pipelines.registration.PoseGraphNode(trans_odometry))
 98    for s in range(sid, eid):
 99        for t in range(s + 1, eid):
100            # odometry
101            if t == s + 1:
102                print(
103                    "Fragment %03d / %03d :: RGBD matching between frame : %d and %d"
104                    % (fragment_id, n_fragments - 1, s, t))
105                [success, trans,
106                 info] = register_one_rgbd_pair(s, t, color_files, depth_files,
107                                                intrinsic, with_opencv, config)
108                trans_odometry = np.dot(trans, trans_odometry)
109                trans_odometry_inv = np.linalg.inv(trans_odometry)
110                pose_graph.nodes.append(
111                    o3d.pipelines.registration.PoseGraphNode(
112                        trans_odometry_inv))
113                pose_graph.edges.append(
114                    o3d.pipelines.registration.PoseGraphEdge(s - sid,
115                                                             t - sid,
116                                                             trans,
117                                                             info,
118                                                             uncertain=False))
119
120            # keyframe loop closure
121            if s % config['n_keyframes_per_n_frame'] == 0 \
122                    and t % config['n_keyframes_per_n_frame'] == 0:
123                print(
124                    "Fragment %03d / %03d :: RGBD matching between frame : %d and %d"
125                    % (fragment_id, n_fragments - 1, s, t))
126                [success, trans,
127                 info] = register_one_rgbd_pair(s, t, color_files, depth_files,
128                                                intrinsic, with_opencv, config)
129                if success:
130                    pose_graph.edges.append(
131                        o3d.pipelines.registration.PoseGraphEdge(
132                            s - sid, t - sid, trans, info, uncertain=True))
133    o3d.io.write_pose_graph(
134        join(path_dataset, config["template_fragment_posegraph"] % fragment_id),
135        pose_graph)
136

This script uses the technique demonstrated in /tutorial/pipelines/multiway_registration.ipynb. The 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 the function optimize_posegraph_for_fragment.

51# examples/python/reconstruction_system/optimize_posegraph.py
52
53def optimize_posegraph_for_fragment(path_dataset, fragment_id, config):
54    pose_graph_name = join(path_dataset,
55                           config["template_fragment_posegraph"] % fragment_id)
56    pose_graph_optimized_name = join(
57        path_dataset,
58        config["template_fragment_posegraph_optimized"] % fragment_id)
59    run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
60            max_correspondence_distance = config["max_depth_diff"],
61            preference_loop_closure = \
62            config["preference_loop_closure_odometry"])
63
64

This function calls global_optimization to estimate poses of the RGBD images.

Make a fragment

136# examples/python/reconstruction_system/make_fragments.py
137
138def integrate_rgb_frames_for_fragment(color_files, depth_files, fragment_id,
139                                      n_fragments, pose_graph_name, intrinsic,
140                                      config):
141    pose_graph = o3d.io.read_pose_graph(pose_graph_name)
142    volume = o3d.pipelines.integration.ScalableTSDFVolume(
143        voxel_length=config["tsdf_cubic_size"] / 512.0,
144        sdf_trunc=0.04,
145        color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8)
146    for i in range(len(pose_graph.nodes)):
147        i_abs = fragment_id * config['n_frames_per_fragment'] + i
148        print(
149            "Fragment %03d / %03d :: integrate rgbd frame %d (%d of %d)." %
150            (fragment_id, n_fragments - 1, i_abs, i + 1, len(pose_graph.nodes)))
151        rgbd = read_rgbd_image(color_files[i_abs], depth_files[i_abs], False,
152                               config)
153        pose = pose_graph.nodes[i].pose
154        volume.integrate(rgbd, intrinsic, np.linalg.inv(pose))
155    mesh = volume.extract_triangle_mesh()
156    mesh.compute_vertex_normals()
157    return mesh
158

Once the poses are estimates, /tutorial/pipelines/rgbd_integration.ipynb is used to reconstruct a colored fragment from each RGBD sequence.

Batch processing

193# examples/python/reconstruction_system/make_fragments.py
194
195def run(config):
196
197    print("making fragments from RGBD sequence.")
198    make_clean_folder(join(config["path_dataset"], config["folder_fragment"]))
199
200    [color_files, depth_files] = get_rgbd_file_lists(config["path_dataset"])
201    n_files = len(color_files)
202    n_fragments = int(
203        math.ceil(float(n_files) / config['n_frames_per_fragment']))
204
205    if config["python_multi_threading"] is True:
206        from joblib import Parallel, delayed
207        import multiprocessing
208        import subprocess
209        MAX_THREAD = min(multiprocessing.cpu_count(), n_fragments)
210        Parallel(n_jobs=MAX_THREAD)(delayed(process_single_fragment)(
211            fragment_id, color_files, depth_files, n_files, n_fragments, config)
212                                    for fragment_id in range(n_fragments))
213    else:
214        for fragment_id in range(n_fragments):
215            process_single_fragment(fragment_id, color_files, depth_files,
216                                    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.

../../_images/fragment_0.png ../../_images/fragment_1.png ../../_images/fragment_2.png ../../_images/fragment_3.png