Register fragments
Once the fragments of the scene are created, the next step is to align them in a global space.
Input arguments
This script runs with python run_system.py [config] --register
. 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 make_posegraph_for_scene
and optimize_posegraph_for_scene
. The first function performs pairwise registration. The second function performs multiway registration.
Preprocess point cloud
17# examples/Python/ReconstructionSystem/register_fragments.py
18def preprocess_point_cloud(pcd, config):
19 voxel_size = config["voxel_size"]
20 pcd_down = pcd.voxel_down_sample(voxel_size)
21 pcd_down.estimate_normals(
22 o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
23 max_nn=30))
24 pcd_fpfh = o3d.registration.compute_fpfh_feature(
25 pcd_down,
26 o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,
27 max_nn=100))
28 return (pcd_down, pcd_fpfh)
This function downsamples point cloud to make a point cloud sparser and regularly distributed. Normals and FPFH feature are precomputed. See Voxel downsampling, Vertex normal estimation, and Extract geometric feature for more details.
Compute initial registration
54# examples/Python/ReconstructionSystem/register_fragments.py
55def compute_initial_registration(s, t, source_down, target_down, source_fpfh,
56 target_fpfh, path_dataset, config):
57
58 if t == s + 1: # odometry case
59 print("Using RGBD odometry")
60 pose_graph_frag = o3d.io.read_pose_graph(
61 join(path_dataset,
62 config["template_fragment_posegraph_optimized"] % s))
63 n_nodes = len(pose_graph_frag.nodes)
64 transformation_init = np.linalg.inv(pose_graph_frag.nodes[n_nodes -
65 1].pose)
66 (transformation, information) = \
67 multiscale_icp(source_down, target_down,
68 [config["voxel_size"]], [50], config, transformation_init)
69 else: # loop closure case
70 (success, transformation,
71 information) = register_point_cloud_fpfh(source_down, target_down,
72 source_fpfh, target_fpfh,
73 config)
74 if not success:
75 print("No resonable solution. Skip this pair")
76 return (False, np.identity(4), np.zeros((6, 6)))
77 print(transformation)
78
79 if config["debug_mode"]:
80 draw_registration_result(source_down, target_down, transformation)
81 return (True, transformation, information)
This function computes a rough alignment between two fragments. If the fragments are neighboring fragments, the rough alignment is determined by an aggregating RGBD odometry obtained from Make fragments. Otherwise, register_point_cloud_fpfh
is called to perform global registration. Note that global registration is less reliable according to [Choi2015].
Pairwise global registration
30# examples/Python/ReconstructionSystem/register_fragments.py
31def register_point_cloud_fpfh(source, target, source_fpfh, target_fpfh, config):
32 distance_threshold = config["voxel_size"] * 1.4
33 if config["global_registration"] == "fgr":
34 result = o3d.registration.registration_fast_based_on_feature_matching(
35 source, target, source_fpfh, target_fpfh,
36 o3d.registration.FastGlobalRegistrationOption(
37 maximum_correspondence_distance=distance_threshold))
38 if config["global_registration"] == "ransac":
39 result = o3d.registration.registration_ransac_based_on_feature_matching(
40 source, target, source_fpfh, target_fpfh, distance_threshold,
41 o3d.registration.TransformationEstimationPointToPoint(False), 4, [
42 o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
43 o3d.registration.CorrespondenceCheckerBasedOnDistance(
44 distance_threshold)
45 ], o3d.registration.RANSACConvergenceCriteria(4000000, 500))
46 if (result.transformation.trace() == 4.0):
47 return (False, np.identity(4), np.zeros((6, 6)))
48 information = o3d.registration.get_information_matrix_from_point_clouds(
49 source, target, distance_threshold, result.transformation)
50 if information[5, 5] / min(len(source.points), len(target.points)) < 0.3:
51 return (False, np.identity(4), np.zeros((6, 6)))
52 return (True, result.transformation, information)
This function uses RANSAC or Fast global registration for pairwise global registration.
Multiway registration
83# examples/Python/ReconstructionSystem/register_fragments.py
84def update_posegrph_for_scene(s, t, transformation, information, odometry,
85 pose_graph):
86 if t == s + 1: # odometry case
87 odometry = np.dot(transformation, odometry)
88 odometry_inv = np.linalg.inv(odometry)
89 pose_graph.nodes.append(o3d.registration.PoseGraphNode(odometry_inv))
90 pose_graph.edges.append(
91 o3d.registration.PoseGraphEdge(s,
92 t,
93 transformation,
94 information,
95 uncertain=False))
96 else: # loop closure case
97 pose_graph.edges.append(
98 o3d.registration.PoseGraphEdge(s,
99 t,
100 transformation,
101 information,
102 uncertain=True))
103 return (odometry, pose_graph)
This script uses the technique demonstrated in Multiway registration. Function update_posegrph_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.
42# examples/Python/ReconstructionSystem/optimize_posegraph.py
43def optimize_posegraph_for_scene(path_dataset, config):
44 pose_graph_name = join(path_dataset, config["template_global_posegraph"])
45 pose_graph_optimized_name = join(
46 path_dataset, config["template_global_posegraph_optimized"])
47 run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
48 max_correspondence_distance = config["voxel_size"] * 1.4,
49 preference_loop_closure = \
50 config["preference_loop_closure_registration"])
Main registration loop
The function make_posegraph_for_scene
below calls all the functions introduced above. The main workflow is: pairwise global registration -> multiway registration.
135# examples/Python/ReconstructionSystem/register_fragments.py
136def make_posegraph_for_scene(ply_file_names, config):
137 pose_graph = o3d.registration.PoseGraph()
138 odometry = np.identity(4)
139 pose_graph.nodes.append(o3d.registration.PoseGraphNode(odometry))
140
141 n_files = len(ply_file_names)
142 matching_results = {}
143 for s in range(n_files):
144 for t in range(s + 1, n_files):
145 matching_results[s * n_files + t] = matching_result(s, t)
146
147 if config["python_multi_threading"]:
148 from joblib import Parallel, delayed
149 import multiprocessing
150 import subprocess
151 MAX_THREAD = min(multiprocessing.cpu_count(),
152 max(len(matching_results), 1))
153 results = Parallel(n_jobs=MAX_THREAD)(delayed(
154 register_point_cloud_pair)(ply_file_names, matching_results[r].s,
155 matching_results[r].t, config)
156 for r in matching_results)
157 for i, r in enumerate(matching_results):
158 matching_results[r].success = results[i][0]
159 matching_results[r].transformation = results[i][1]
160 matching_results[r].information = results[i][2]
161 else:
162 for r in matching_results:
163 (matching_results[r].success, matching_results[r].transformation,
164 matching_results[r].information) = \
165 register_point_cloud_pair(ply_file_names,
166 matching_results[r].s, matching_results[r].t, config)
167
168 for r in matching_results:
169 if matching_results[r].success:
170 (odometry, pose_graph) = update_posegrph_for_scene(
171 matching_results[r].s, matching_results[r].t,
172 matching_results[r].transformation,
173 matching_results[r].information, odometry, pose_graph)
174 o3d.io.write_pose_graph(
175 join(config["path_dataset"], config["template_global_posegraph"]),
176 pose_graph)
Results
The following is messages from pose graph optimization.
[GlobalOptimizationLM] Optimizing PoseGraph having 14 nodes and 42 edges.
Line process weight : 55.885667
[Initial ] residual : 7.791139e+04, lambda : 1.205976e+00
[Iteration 00] residual : 6.094275e+02, valid edges : 22, time : 0.001 sec.
[Iteration 01] residual : 4.526879e+02, valid edges : 22, time : 0.000 sec.
[Iteration 02] residual : 4.515039e+02, valid edges : 22, time : 0.000 sec.
[Iteration 03] residual : 4.514832e+02, valid edges : 22, time : 0.000 sec.
[Iteration 04] residual : 4.514825e+02, valid edges : 22, time : 0.000 sec.
Current_residual - new_residual < 1.000000e-06 * current_residual
[GlobalOptimizationLM] total time : 0.003 sec.
[GlobalOptimizationLM] Optimizing PoseGraph having 14 nodes and 35 edges.
Line process weight : 60.762800
[Initial ] residual : 6.336097e+01, lambda : 1.324043e+00
[Iteration 00] residual : 6.334147e+01, valid edges : 22, time : 0.000 sec.
[Iteration 01] residual : 6.334138e+01, valid edges : 22, time : 0.000 sec.
Current_residual - new_residual < 1.000000e-06 * current_residual
[GlobalOptimizationLM] total time : 0.001 sec.
CompensateReferencePoseGraphNode : reference : 0
There are 14 fragments and 52 valid matching pairs among the 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.