Refine registration

Input arguments

This script runs with python run_system.py [config] --refine. In [config], ["path_dataset"] should have subfolders fragments which stores fragments in .ply files and a pose graph in a .json file.

The main function runs local_refinement and optimize_posegraph_for_scene. The first function performs pairwise registration on the pairs detected by Register fragments. The second function performs multiway registration.

Fine-grained registration

 63# examples/python/reconstruction_system/refine_registration.py
 64
 65def multiscale_icp(source,
 66                   target,
 67                   voxel_size,
 68                   max_iter,
 69                   config,
 70                   init_transformation=np.identity(4)):
 71    current_transformation = init_transformation
 72    for i, scale in enumerate(range(len(max_iter))):  # multi-scale approach
 73        iter = max_iter[scale]
 74        distance_threshold = config["voxel_size"] * 1.4
 75        print("voxel_size {}".format(voxel_size[scale]))
 76        source_down = source.voxel_down_sample(voxel_size[scale])
 77        target_down = target.voxel_down_sample(voxel_size[scale])
 78        if config["icp_method"] == "point_to_point":
 79            result_icp = o3d.pipelines.registration.registration_icp(
 80                source_down, target_down, distance_threshold,
 81                current_transformation,
 82                o3d.pipelines.registration.TransformationEstimationPointToPoint(
 83                ),
 84                o3d.pipelines.registration.ICPConvergenceCriteria(
 85                    max_iteration=iter))
 86        else:
 87            source_down.estimate_normals(
 88                o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
 89                                                     2.0,
 90                                                     max_nn=30))
 91            target_down.estimate_normals(
 92                o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
 93                                                     2.0,
 94                                                     max_nn=30))
 95            if config["icp_method"] == "point_to_plane":
 96                result_icp = o3d.pipelines.registration.registration_icp(
 97                    source_down, target_down, distance_threshold,
 98                    current_transformation,
 99                    o3d.pipelines.registration.
100                    TransformationEstimationPointToPlane(),
101                    o3d.pipelines.registration.ICPConvergenceCriteria(
102                        max_iteration=iter))
103            if config["icp_method"] == "color":
104                # Colored ICP is sensitive to threshold.
105                # Fallback to preset distance threshold that works better.
106                # TODO: make it adjustable in the upgraded system.
107                result_icp = o3d.pipelines.registration.registration_colored_icp(
108                    source_down, target_down, voxel_size[scale],
109                    current_transformation,
110                    o3d.pipelines.registration.
111                    TransformationEstimationForColoredICP(),
112                    o3d.pipelines.registration.ICPConvergenceCriteria(
113                        relative_fitness=1e-6,
114                        relative_rmse=1e-6,
115                        max_iteration=iter))
116            if config["icp_method"] == "generalized":
117                result_icp = o3d.pipelines.registration.registration_generalized_icp(
118                    source_down, target_down, distance_threshold,
119                    current_transformation,
120                    o3d.pipelines.registration.
121                    TransformationEstimationForGeneralizedICP(),
122                    o3d.pipelines.registration.ICPConvergenceCriteria(
123                        relative_fitness=1e-6,
124                        relative_rmse=1e-6,
125                        max_iteration=iter))
126        current_transformation = result_icp.transformation
127        if i == len(max_iter) - 1:
128            information_matrix = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
129                source_down, target_down, voxel_size[scale] * 1.4,
130                result_icp.transformation)
131
132    if config["debug_mode"]:
133        draw_registration_result_original_color(source, target,
134                                                result_icp.transformation)
135    return (result_icp.transformation, information_matrix)
136

Two options are given for the fine-grained registration. The color option is recommended since it uses color information to prevent drift. See [Park2017] for details.

Multiway registration

40# examples/python/reconstruction_system/refine_registration.py
41
42def update_posegraph_for_scene(s, t, transformation, information, odometry,
43                               pose_graph):
44    if t == s + 1:  # odometry case
45        odometry = np.dot(transformation, odometry)
46        odometry_inv = np.linalg.inv(odometry)
47        pose_graph.nodes.append(
48            o3d.pipelines.registration.PoseGraphNode(odometry_inv))
49        pose_graph.edges.append(
50            o3d.pipelines.registration.PoseGraphEdge(s,
51                                                     t,
52                                                     transformation,
53                                                     information,
54                                                     uncertain=False))
55    else:  # loop closure case
56        pose_graph.edges.append(
57            o3d.pipelines.registration.PoseGraphEdge(s,
58                                                     t,
59                                                     transformation,
60                                                     information,
61                                                     uncertain=True))
62    return (odometry, pose_graph)
63

This script uses the technique demonstrated in /tutorial/pipelines/multiway_registration.ipynb. Function update_posegraph_for_scene builds a pose graph for multiway registration of all fragments. Each graph node represents a fragment and its pose which transforms the geometry to the global space.

Once a pose graph is built, function optimize_posegraph_for_scene is called for multiway registration.

63# examples/python/reconstruction_system/optimize_posegraph.py
64
65def optimize_posegraph_for_scene(path_dataset, config):
66    pose_graph_name = join(path_dataset, config["template_global_posegraph"])
67    pose_graph_optimized_name = join(
68        path_dataset, config["template_global_posegraph_optimized"])
69    run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
70            max_correspondence_distance = config["voxel_size"] * 1.4,
71            preference_loop_closure = \
72            config["preference_loop_closure_registration"])
73

Main registration loop

The function make_posegraph_for_refined_scene below calls all the functions

introduced above.

173# examples/python/reconstruction_system/refine_registration.py
174
175def make_posegraph_for_refined_scene(ply_file_names, config):
176    pose_graph = o3d.io.read_pose_graph(
177        join(config["path_dataset"],
178             config["template_global_posegraph_optimized"]))
179
180    n_files = len(ply_file_names)
181    matching_results = {}
182    for edge in pose_graph.edges:
183        s = edge.source_node_id
184        t = edge.target_node_id
185        matching_results[s * n_files + t] = \
186                matching_result(s, t, edge.transformation)
187
188    if config["python_multi_threading"] == True:
189        from joblib import Parallel, delayed
190        import multiprocessing
191        import subprocess
192        MAX_THREAD = min(multiprocessing.cpu_count(),
193                         max(len(pose_graph.edges), 1))
194        results = Parallel(n_jobs=MAX_THREAD)(
195            delayed(register_point_cloud_pair)(
196                ply_file_names, matching_results[r].s, matching_results[r].t,
197                matching_results[r].transformation, config)
198            for r in matching_results)
199        for i, r in enumerate(matching_results):
200            matching_results[r].transformation = results[i][0]
201            matching_results[r].information = results[i][1]
202    else:
203        for r in matching_results:
204            (matching_results[r].transformation,
205                    matching_results[r].information) = \
206                    register_point_cloud_pair(ply_file_names,
207                    matching_results[r].s, matching_results[r].t,
208                    matching_results[r].transformation, config)
209
210    pose_graph_new = o3d.pipelines.registration.PoseGraph()
211    odometry = np.identity(4)
212    pose_graph_new.nodes.append(
213        o3d.pipelines.registration.PoseGraphNode(odometry))
214    for r in matching_results:
215        (odometry, pose_graph_new) = update_posegraph_for_scene(
216            matching_results[r].s, matching_results[r].t,
217            matching_results[r].transformation, matching_results[r].information,
218            odometry, pose_graph_new)
219    print(pose_graph_new)
220    o3d.io.write_pose_graph(
221        join(config["path_dataset"], config["template_refined_posegraph"]),
222        pose_graph_new)
223

The main workflow is: pairwise local refinement -> multiway registration.

Results

The pose graph optimization outputs the following messages:

[GlobalOptimizationLM] Optimizing PoseGraph having 14 nodes and 35 edges.
Line process weight : 789.730200
[Initial     ] residual : 1.208286e+04, lambda : 1.706306e+01
[Iteration 00] residual : 2.410383e+03, valid edges : 22, time : 0.000 sec.
[Iteration 01] residual : 8.127856e+01, valid edges : 22, time : 0.000 sec.
[Iteration 02] residual : 8.031355e+01, valid edges : 22, time : 0.000 sec.
Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)
[GlobalOptimizationLM] total time : 0.001 sec.
[GlobalOptimizationLM] Optimizing PoseGraph having 14 nodes and 35 edges.
Line process weight : 789.730200
[Initial     ] residual : 8.031355e+01, lambda : 1.716826e+01
Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)
[GlobalOptimizationLM] total time : 0.000 sec.
CompensateReferencePoseGraphNode : reference : 0

There are 14 fragments and 52 valid matching pairs between fragments. After 23 iterations, 11 edges are detected to be false positives. After they are pruned, pose graph optimization runs again to achieve tight alignment.