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

38# examples/Python/ReconstructionSystem/refine_registration.py
39def multiscale_icp(source,
40                   target,
41                   voxel_size,
42                   max_iter,
43                   config,
44                   init_transformation=np.identity(4)):
45    current_transformation = init_transformation
46    for i, scale in enumerate(range(len(max_iter))):  # multi-scale approach
47        iter = max_iter[scale]
48        distance_threshold = config["voxel_size"] * 1.4
49        print("voxel_size %f" % voxel_size[scale])
50        source_down = source.voxel_down_sample(voxel_size[scale])
51        target_down = target.voxel_down_sample(voxel_size[scale])
52        if config["icp_method"] == "point_to_point":
53            result_icp = o3d.registration.registration_icp(
54                source_down, target_down, distance_threshold,
55                current_transformation,
56                o3d.registration.TransformationEstimationPointToPoint(),
57                o3d.registration.ICPConvergenceCriteria(max_iteration=iter))
58        else:
59            source_down.estimate_normals(
60                o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
61                                                     2.0,
62                                                     max_nn=30))
63            target_down.estimate_normals(
64                o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
65                                                     2.0,
66                                                     max_nn=30))
67            if config["icp_method"] == "point_to_plane":
68                result_icp = o3d.registration.registration_icp(
69                    source_down, target_down, distance_threshold,
70                    current_transformation,
71                    o3d.registration.TransformationEstimationPointToPlane(),
72                    o3d.registration.ICPConvergenceCriteria(max_iteration=iter))
73            if config["icp_method"] == "color":
74                result_icp = o3d.registration.registration_colored_icp(
75                    source_down, target_down, voxel_size[scale],
76                    current_transformation,
77                    o3d.registration.ICPConvergenceCriteria(
78                        relative_fitness=1e-6,
79                        relative_rmse=1e-6,
80                        max_iteration=iter))
81        current_transformation = result_icp.transformation
82        if i == len(max_iter) - 1:
83            information_matrix = o3d.registration.get_information_matrix_from_point_clouds(
84                source_down, target_down, voxel_size[scale] * 1.4,
85                result_icp.transformation)
86
87    if config["debug_mode"]:
88        draw_registration_result_original_color(source, target,
89                                                result_icp.transformation)
90    return (result_icp.transformation, information_matrix)

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

16# examples/Python/ReconstructionSystem/refine_registration.py
17def update_posegrph_for_scene(s, t, transformation, information, odometry,
18                              pose_graph):
19    if t == s + 1:  # odometry case
20        odometry = np.dot(transformation, odometry)
21        odometry_inv = np.linalg.inv(odometry)
22        pose_graph.nodes.append(o3d.registration.PoseGraphNode(odometry_inv))
23        pose_graph.edges.append(
24            o3d.registration.PoseGraphEdge(s,
25                                           t,
26                                           transformation,
27                                           information,
28                                           uncertain=False))
29    else:  # loop closure case
30        pose_graph.edges.append(
31            o3d.registration.PoseGraphEdge(s,
32                                           t,
33                                           transformation,
34                                           information,
35                                           uncertain=True))
36    return (odometry, pose_graph)

This script uses the technique demonstrated in Multiway registration. Function update_posegrph_for_refined_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.

52# examples/Python/ReconstructionSystem/optimize_posegraph.py
53def optimize_posegraph_for_refined_scene(path_dataset, config):
54    pose_graph_name = join(path_dataset, config["template_refined_posegraph"])
55    pose_graph_optimized_name = join(
56        path_dataset, config["template_refined_posegraph_optimized"])
57    run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
58            max_correspondence_distance = config["voxel_size"] * 1.4,
59            preference_loop_closure = \
60            config["preference_loop_closure_registration"])

Main registration loop

The function make_posegraph_for_refined_scene below calls all the functions introduced above.

129# examples/Python/ReconstructionSystem/refine_registration.py
130def make_posegraph_for_refined_scene(ply_file_names, config):
131    pose_graph = o3d.io.read_pose_graph(
132        join(config["path_dataset"],
133             config["template_global_posegraph_optimized"]))
134
135    n_files = len(ply_file_names)
136    matching_results = {}
137    for edge in pose_graph.edges:
138        s = edge.source_node_id
139        t = edge.target_node_id
140        matching_results[s * n_files + t] = \
141                matching_result(s, t, edge.transformation)
142
143    if config["python_multi_threading"]:
144        from joblib import Parallel, delayed
145        import multiprocessing
146        import subprocess
147        MAX_THREAD = min(multiprocessing.cpu_count(),
148                         max(len(pose_graph.edges), 1))
149        results = Parallel(n_jobs=MAX_THREAD)(
150            delayed(register_point_cloud_pair)(
151                ply_file_names, matching_results[r].s, matching_results[r].t,
152                matching_results[r].transformation, config)
153            for r in matching_results)
154        for i, r in enumerate(matching_results):
155            matching_results[r].transformation = results[i][0]
156            matching_results[r].information = results[i][1]
157    else:
158        for r in matching_results:
159            (matching_results[r].transformation,
160                    matching_results[r].information) = \
161                    register_point_cloud_pair(ply_file_names,
162                    matching_results[r].s, matching_results[r].t,
163                    matching_results[r].transformation, config)
164
165    pose_graph_new = o3d.registration.PoseGraph()
166    odometry = np.identity(4)
167    pose_graph_new.nodes.append(o3d.registration.PoseGraphNode(odometry))
168    for r in matching_results:
169        (odometry, pose_graph_new) = update_posegrph_for_scene(
170            matching_results[r].s, matching_results[r].t,
171            matching_results[r].transformation, matching_results[r].information,
172            odometry, pose_graph_new)
173    print(pose_graph_new)
174    o3d.io.write_pose_graph(
175        join(config["path_dataset"], config["template_refined_posegraph"]),
176        pose_graph_new)

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

Results

The following is messages from pose graph optimization.

[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 iteration, 11 edges are detected to be false positive. After they are pruned, pose graph optimization runs again to achieve tight alignment.