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.