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        max_workers = max(
166            1, min(multiprocessing.cpu_count() - 1, len(matching_results)))
167        mp_context = multiprocessing.get_context('spawn')
168        # Run Open3D single-threaded per worker to avoid CPU oversubscription.
169        with mp_context.Pool(processes=max_workers,
170                             initializer=limit_open3d_threads) as pool:
171            args = [(ply_file_names, v.s, v.t, config)
172                    for k, v in matching_results.items()]
173            results = pool.starmap(register_point_cloud_pair, args)
174
175        for i, r in enumerate(matching_results):
176            matching_results[r].success = results[i][0]
177            matching_results[r].transformation = results[i][1]
178            matching_results[r].information = results[i][2]
179    else:
180        for r in matching_results:
181            (matching_results[r].success, matching_results[r].transformation,
182             matching_results[r].information) = \
183                register_point_cloud_pair(ply_file_names,
184                                          matching_results[r].s, matching_results[r].t, config)
185
186    for r in matching_results:
187        if matching_results[r].success:
188            (odometry, pose_graph) = update_posegraph_for_scene(
189                matching_results[r].s, matching_results[r].t,
190                matching_results[r].transformation,
191                matching_results[r].information, odometry, pose_graph)
192    o3d.io.write_pose_graph(
193        join(config["path_dataset"], config["template_global_posegraph"]),
194        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.