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#
27def preprocess_point_cloud(pcd, config):
28 voxel_size = config["voxel_size"]
29 pcd_down = pcd.voxel_down_sample(voxel_size)
30 pcd_down.estimate_normals(
31 o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
32 max_nn=30))
33 pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
34 pcd_down,
35 o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,
36 max_nn=100))
37 return (pcd_down, pcd_fpfh)
This function downsamples a point cloud to make it 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#
71def compute_initial_registration(s, t, source_down, target_down, source_fpfh,
72 target_fpfh, path_dataset, config):
73
74 if t == s + 1: # odometry case
75 print("Using RGBD odometry")
76 pose_graph_frag = o3d.io.read_pose_graph(
77 join(path_dataset,
78 config["template_fragment_posegraph_optimized"] % s))
79 n_nodes = len(pose_graph_frag.nodes)
80 transformation_init = np.linalg.inv(pose_graph_frag.nodes[n_nodes -
81 1].pose)
82 (transformation, information) = \
83 multiscale_icp(source_down, target_down,
84 [config["voxel_size"]], [50], config, transformation_init)
85 else: # loop closure case
86 (success, transformation,
87 information) = register_point_cloud_fpfh(source_down, target_down,
88 source_fpfh, target_fpfh,
89 config)
90 if not success:
91 print("No reasonable solution. Skip this pair")
92 return (False, np.identity(4), np.zeros((6, 6)))
93 print(transformation)
94
95 if config["debug_mode"]:
96 draw_registration_result(source_down, target_down, transformation)
97 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#
40def register_point_cloud_fpfh(source, target, source_fpfh, target_fpfh, config):
41 o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
42 distance_threshold = config["voxel_size"] * 1.4
43 if config["global_registration"] == "fgr":
44 result = o3d.pipelines.registration.registration_fgr_based_on_feature_matching(
45 source, target, source_fpfh, target_fpfh,
46 o3d.pipelines.registration.FastGlobalRegistrationOption(
47 maximum_correspondence_distance=distance_threshold))
48 if config["global_registration"] == "ransac":
49 # Fallback to preset parameters that works better
50 result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
51 source, target, source_fpfh, target_fpfh, False, distance_threshold,
52 o3d.pipelines.registration.TransformationEstimationPointToPoint(
53 False), 4,
54 [
55 o3d.pipelines.registration.
56 CorrespondenceCheckerBasedOnEdgeLength(0.9),
57 o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
58 distance_threshold)
59 ],
60 o3d.pipelines.registration.RANSACConvergenceCriteria(
61 1000000, 0.999))
62 if (result.transformation.trace() == 4.0):
63 return (False, np.identity(4), np.zeros((6, 6)))
64 information = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
65 source, target, distance_threshold, result.transformation)
66 if information[5, 5] / min(len(source.points), len(target.points)) < 0.3:
67 return (False, np.identity(4), np.zeros((6, 6)))
68 return (True, result.transformation, information)
This function uses RANSAC or Fast global registration for pairwise global registration.
Multiway registration#
100def update_posegraph_for_scene(s, t, transformation, information, odometry,
101 pose_graph):
102 if t == s + 1: # odometry case
103 odometry = np.dot(transformation, odometry)
104 odometry_inv = np.linalg.inv(odometry)
105 pose_graph.nodes.append(
106 o3d.pipelines.registration.PoseGraphNode(odometry_inv))
107 pose_graph.edges.append(
108 o3d.pipelines.registration.PoseGraphEdge(s,
109 t,
110 transformation,
111 information,
112 uncertain=False))
113 else: # loop closure case
114 pose_graph.edges.append(
115 o3d.pipelines.registration.PoseGraphEdge(s,
116 t,
117 transformation,
118 information,
119 uncertain=True))
120 return (odometry, pose_graph)
This script uses the technique demonstrated in
Multiway registration. The 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, the function optimize_posegraph_for_scene
is
called for multiway registration.
46def optimize_posegraph_for_scene(path_dataset, config):
47 pose_graph_name = join(path_dataset, config["template_global_posegraph"])
48 pose_graph_optimized_name = join(
49 path_dataset, config["template_global_posegraph_optimized"])
50 run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
51 max_correspondence_distance = config["voxel_size"] * 1.4,
52 preference_loop_closure = \
53 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.
153def make_posegraph_for_scene(ply_file_names, config):
154 pose_graph = o3d.pipelines.registration.PoseGraph()
155 odometry = np.identity(4)
156 pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))
157
158 n_files = len(ply_file_names)
159 matching_results = {}
160 for s in range(n_files):
161 for t in range(s + 1, n_files):
162 matching_results[s * n_files + t] = matching_result(s, t)
163
164 if config["python_multi_threading"] is True:
165 os.environ['OMP_NUM_THREADS'] = '1'
166 max_workers = max(
167 1, min(multiprocessing.cpu_count() - 1, len(matching_results)))
168 mp_context = multiprocessing.get_context('spawn')
169 with mp_context.Pool(processes=max_workers) as pool:
170 args = [(ply_file_names, v.s, v.t, config)
171 for k, v in matching_results.items()]
172 results = pool.starmap(register_point_cloud_pair, args)
173
174 for i, r in enumerate(matching_results):
175 matching_results[r].success = results[i][0]
176 matching_results[r].transformation = results[i][1]
177 matching_results[r].information = results[i][2]
178 else:
179 for r in matching_results:
180 (matching_results[r].success, matching_results[r].transformation,
181 matching_results[r].information) = \
182 register_point_cloud_pair(ply_file_names,
183 matching_results[r].s, matching_results[r].t, config)
184
185 for r in matching_results:
186 if matching_results[r].success:
187 (odometry, pose_graph) = update_posegraph_for_scene(
188 matching_results[r].s, matching_results[r].t,
189 matching_results[r].transformation,
190 matching_results[r].information, odometry, pose_graph)
191 o3d.io.write_pose_graph(
192 join(config["path_dataset"], config["template_global_posegraph"]),
193 pose_graph)
Results#
The pose graph optimization outputs the following messages:
[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 iterations, 11 edges are detected to be false positives. After they are pruned, pose graph optimization runs again to achieve tight alignment.