Multiway registration¶
Multiway registration is the process of aligning multiple pieces of geometry in a global space. Typically, the input is a set of geometries (e.g., point clouds or RGBD images) \(\{\mathbf{P}_{i}\}\). The output is a set of rigid transformations \(\{\mathbf{T}_{i}\}\), so that the transformed point clouds \(\{\mathbf{T}_{i}\mathbf{P}_{i}\}\) are aligned in the global space.
Open3D implements multiway registration via pose graph optimization. The backend implements the technique presented in [Choi2015].
Input¶
The first part of the tutorial code reads three point clouds from files. The point clouds are downsampled and visualized together. They are misaligned.
[2]:
def load_point_clouds(voxel_size=0.0):
pcds = []
for i in range(3):
pcd = o3d.io.read_point_cloud("../../test_data/ICP/cloud_bin_%d.pcd" %
i)
pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
pcds.append(pcd_down)
return pcds
[3]:
voxel_size = 0.02
pcds_down = load_point_clouds(voxel_size)
o3d.visualization.draw_geometries(pcds_down,
zoom=0.3412,
front=[0.4257, -0.2125, -0.8795],
lookat=[2.6172, 2.0475, 1.532],
up=[-0.0694, -0.9768, 0.2024])
Pose graph¶
A pose graph has two key elements: nodes and edges. A node is a piece of geometry \(\mathbf{P}_{i}\) associated with a pose matrix \(\mathbf{T}_{i}\) which transforms \(\mathbf{P}_{i}\) into the global space. The set \(\{\mathbf{T}_{i}\}\) are the unknown variables to be optimized. PoseGraph.nodes is a list of PoseGraphNode. We set the global space to be the space of \(\mathbf{P}_{0}\). Thus \(\mathbf{T}_{0}\) is the identity matrix. The other pose matrices are
initialized by accumulating transformation between neighboring nodes. The neighboring nodes usually have large overlap and can be registered with Point-to-plane ICP.
A pose graph edge connects two nodes (pieces of geometry) that overlap. Each edge contains a transformation matrix \(\mathbf{T}_{i,j}\) that aligns the source geometry \(\mathbf{P}_{i}\) to the target geometry \(\mathbf{P}_{j}\). This tutorial uses Point-to-plane ICP to estimate the transformation. In more complicated cases, this pairwise registration problem should be solved via Global registration.
[Choi2015] has observed that pairwise registration is error-prone. False pairwise alignments can outnumber correctly aligned pairs. Thus, they partition pose graph edges into two classes. Odometry edges connect temporally close, neighboring nodes. A local registration algorithm such as ICP can reliably align them. Loop closure edges connect any non-neighboring nodes. The alignment is found by global registration and is less reliable. In Open3D, these two
classes of edges are distinguished by the uncertain parameter in the initializer of PoseGraphEdge.
In addition to the transformation matrix \(\mathbf{T}_{i}\), the user can set an information matrix \(\mathbf{\Lambda}_{i}\) for each edge. If \(\mathbf{\Lambda}_{i}\) is set using function get_information_matrix_from_point_clouds, the loss on this pose graph edge approximates the RMSE of the corresponding sets between the two nodes, with a line process weight. Refer to Eq (3) to (9) in [Choi2015] and the Redwood registration
benchmark for details.
The script creates a pose graph with three nodes and three edges. Among the edges, two of them are odometry edges (uncertain = False) and one is a loop closure edge (uncertain = True).
[4]:
def pairwise_registration(source, target):
print("Apply point-to-plane ICP")
icp_coarse = o3d.pipelines.registration.registration_icp(
source, target, max_correspondence_distance_coarse, np.identity(4),
o3d.pipelines.registration.TransformationEstimationPointToPlane())
icp_fine = o3d.pipelines.registration.registration_icp(
source, target, max_correspondence_distance_fine,
icp_coarse.transformation,
o3d.pipelines.registration.TransformationEstimationPointToPlane())
transformation_icp = icp_fine.transformation
information_icp = o3d.pipelines.registration.get_information_matrix_from_point_clouds(
source, target, max_correspondence_distance_fine,
icp_fine.transformation)
return transformation_icp, information_icp
def full_registration(pcds, max_correspondence_distance_coarse,
max_correspondence_distance_fine):
pose_graph = o3d.pipelines.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(odometry))
n_pcds = len(pcds)
for source_id in range(n_pcds):
for target_id in range(source_id + 1, n_pcds):
transformation_icp, information_icp = pairwise_registration(
pcds[source_id], pcds[target_id])
print("Build o3d.pipelines.registration.PoseGraph")
if target_id == source_id + 1: # odometry case
odometry = np.dot(transformation_icp, odometry)
pose_graph.nodes.append(
o3d.pipelines.registration.PoseGraphNode(
np.linalg.inv(odometry)))
pose_graph.edges.append(
o3d.pipelines.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=False))
else: # loop closure case
pose_graph.edges.append(
o3d.pipelines.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=True))
return pose_graph
[5]:
print("Full registration ...")
max_correspondence_distance_coarse = voxel_size * 15
max_correspondence_distance_fine = voxel_size * 1.5
with o3d.utility.VerbosityContextManager(
o3d.utility.VerbosityLevel.Debug) as cm:
pose_graph = full_registration(pcds_down,
max_correspondence_distance_coarse,
max_correspondence_distance_fine)
Full registration ...
Apply point-to-plane ICP
[Open3D DEBUG] ICP Iteration #0: Fitness 0.6258, RMSE 0.1566
[Open3D DEBUG] Residual : 1.85e-02 (# of elements : 17029)
[Open3D DEBUG] ICP Iteration #1: Fitness 0.6873, RMSE 0.1427
[Open3D DEBUG] Residual : 1.34e-02 (# of elements : 18703)
[Open3D DEBUG] ICP Iteration #2: Fitness 0.7258, RMSE 0.1364
[Open3D DEBUG] Residual : 1.11e-02 (# of elements : 19751)
[Open3D DEBUG] ICP Iteration #3: Fitness 0.7514, RMSE 0.1330
[Open3D DEBUG] Residual : 1.02e-02 (# of elements : 20446)
[Open3D DEBUG] ICP Iteration #4: Fitness 0.7743, RMSE 0.1296
[Open3D DEBUG] Residual : 9.36e-03 (# of elements : 21070)
[Open3D DEBUG] ICP Iteration #5: Fitness 0.7881, RMSE 0.1214
[Open3D DEBUG] Residual : 7.82e-03 (# of elements : 21444)
[Open3D DEBUG] ICP Iteration #6: Fitness 0.7959, RMSE 0.1142
[Open3D DEBUG] Residual : 6.81e-03 (# of elements : 21657)
[Open3D DEBUG] ICP Iteration #7: Fitness 0.8024, RMSE 0.1127
[Open3D DEBUG] Residual : 6.56e-03 (# of elements : 21833)
[Open3D DEBUG] ICP Iteration #8: Fitness 0.8026, RMSE 0.1108
[Open3D DEBUG] Residual : 6.34e-03 (# of elements : 21840)
[Open3D DEBUG] ICP Iteration #9: Fitness 0.7959, RMSE 0.1050
[Open3D DEBUG] Residual : 5.79e-03 (# of elements : 21658)
[Open3D DEBUG] ICP Iteration #10: Fitness 0.7816, RMSE 0.0914
[Open3D DEBUG] Residual : 4.30e-03 (# of elements : 21268)
[Open3D DEBUG] ICP Iteration #11: Fitness 0.7676, RMSE 0.0758
[Open3D DEBUG] Residual : 2.52e-03 (# of elements : 20887)
[Open3D DEBUG] ICP Iteration #12: Fitness 0.7573, RMSE 0.0682
[Open3D DEBUG] Residual : 1.73e-03 (# of elements : 20606)
[Open3D DEBUG] ICP Iteration #13: Fitness 0.7525, RMSE 0.0656
[Open3D DEBUG] Residual : 1.48e-03 (# of elements : 20475)
[Open3D DEBUG] ICP Iteration #14: Fitness 0.7500, RMSE 0.0645
[Open3D DEBUG] Residual : 1.40e-03 (# of elements : 20407)
[Open3D DEBUG] ICP Iteration #15: Fitness 0.7489, RMSE 0.0639
[Open3D DEBUG] Residual : 1.36e-03 (# of elements : 20377)
[Open3D DEBUG] ICP Iteration #16: Fitness 0.7482, RMSE 0.0635
[Open3D DEBUG] Residual : 1.32e-03 (# of elements : 20358)
[Open3D DEBUG] ICP Iteration #17: Fitness 0.7477, RMSE 0.0632
[Open3D DEBUG] Residual : 1.31e-03 (# of elements : 20347)
[Open3D DEBUG] ICP Iteration #18: Fitness 0.7476, RMSE 0.0631
[Open3D DEBUG] Residual : 1.30e-03 (# of elements : 20342)
[Open3D DEBUG] ICP Iteration #19: Fitness 0.7475, RMSE 0.0631
[Open3D DEBUG] Residual : 1.29e-03 (# of elements : 20341)
[Open3D DEBUG] ICP Iteration #20: Fitness 0.7475, RMSE 0.0630
[Open3D DEBUG] Residual : 1.29e-03 (# of elements : 20339)
[Open3D DEBUG] ICP Iteration #21: Fitness 0.7473, RMSE 0.0629
[Open3D DEBUG] Residual : 1.29e-03 (# of elements : 20335)
[Open3D DEBUG] ICP Iteration #22: Fitness 0.7473, RMSE 0.0629
[Open3D DEBUG] Residual : 1.28e-03 (# of elements : 20334)
[Open3D DEBUG] ICP Iteration #23: Fitness 0.7472, RMSE 0.0628
[Open3D DEBUG] Residual : 1.28e-03 (# of elements : 20331)
[Open3D DEBUG] ICP Iteration #24: Fitness 0.7471, RMSE 0.0627
[Open3D DEBUG] Residual : 1.28e-03 (# of elements : 20329)
[Open3D DEBUG] ICP Iteration #25: Fitness 0.7470, RMSE 0.0627
[Open3D DEBUG] Residual : 1.27e-03 (# of elements : 20326)
[Open3D DEBUG] ICP Iteration #26: Fitness 0.7469, RMSE 0.0626
[Open3D DEBUG] Residual : 1.27e-03 (# of elements : 20325)
[Open3D DEBUG] ICP Iteration #27: Fitness 0.7469, RMSE 0.0626
[Open3D DEBUG] Residual : 1.27e-03 (# of elements : 20324)
[Open3D DEBUG] ICP Iteration #28: Fitness 0.7469, RMSE 0.0626
[Open3D DEBUG] Residual : 1.26e-03 (# of elements : 20323)
[Open3D DEBUG] ICP Iteration #29: Fitness 0.7469, RMSE 0.0626
[Open3D DEBUG] Residual : 1.26e-03 (# of elements : 20323)
[Open3D DEBUG] ICP Iteration #0: Fitness 0.5852, RMSE 0.0139
[Open3D DEBUG] Residual : 1.17e-04 (# of elements : 15924)
[Open3D DEBUG] ICP Iteration #1: Fitness 0.6369, RMSE 0.0121
[Open3D DEBUG] Residual : 7.15e-05 (# of elements : 17332)
[Open3D DEBUG] ICP Iteration #2: Fitness 0.6395, RMSE 0.0102
[Open3D DEBUG] Residual : 3.27e-05 (# of elements : 17402)
[Open3D DEBUG] ICP Iteration #3: Fitness 0.6390, RMSE 0.0101
[Open3D DEBUG] Residual : 3.13e-05 (# of elements : 17388)
[Open3D DEBUG] ICP Iteration #4: Fitness 0.6392, RMSE 0.0101
[Open3D DEBUG] Residual : 3.13e-05 (# of elements : 17392)
[Open3D DEBUG] ICP Iteration #5: Fitness 0.6391, RMSE 0.0101
[Open3D DEBUG] Residual : 3.13e-05 (# of elements : 17390)
Build o3d.pipelines.registration.PoseGraph
Apply point-to-plane ICP
[Open3D DEBUG] ICP Iteration #0: Fitness 0.5669, RMSE 0.1655
[Open3D DEBUG] Residual : 1.78e-02 (# of elements : 15427)
[Open3D DEBUG] ICP Iteration #1: Fitness 0.6107, RMSE 0.1603
[Open3D DEBUG] Residual : 1.72e-02 (# of elements : 16617)
[Open3D DEBUG] ICP Iteration #2: Fitness 0.6706, RMSE 0.1484
[Open3D DEBUG] Residual : 1.47e-02 (# of elements : 18249)
[Open3D DEBUG] ICP Iteration #3: Fitness 0.7393, RMSE 0.1341
[Open3D DEBUG] Residual : 9.62e-03 (# of elements : 20117)
[Open3D DEBUG] ICP Iteration #4: Fitness 0.8061, RMSE 0.1313
[Open3D DEBUG] Residual : 7.39e-03 (# of elements : 21934)
[Open3D DEBUG] ICP Iteration #5: Fitness 0.8341, RMSE 0.1120
[Open3D DEBUG] Residual : 5.13e-03 (# of elements : 22697)
[Open3D DEBUG] ICP Iteration #6: Fitness 0.8784, RMSE 0.0993
[Open3D DEBUG] Residual : 3.11e-03 (# of elements : 23901)
[Open3D DEBUG] ICP Iteration #7: Fitness 0.9177, RMSE 0.0846
[Open3D DEBUG] Residual : 1.47e-03 (# of elements : 24972)
[Open3D DEBUG] ICP Iteration #8: Fitness 0.9376, RMSE 0.0795
[Open3D DEBUG] Residual : 9.79e-04 (# of elements : 25512)
[Open3D DEBUG] ICP Iteration #9: Fitness 0.9463, RMSE 0.0789
[Open3D DEBUG] Residual : 9.53e-04 (# of elements : 25750)
[Open3D DEBUG] ICP Iteration #10: Fitness 0.9491, RMSE 0.0788
[Open3D DEBUG] Residual : 9.87e-04 (# of elements : 25827)
[Open3D DEBUG] ICP Iteration #11: Fitness 0.9499, RMSE 0.0787
[Open3D DEBUG] Residual : 1.00e-03 (# of elements : 25849)
[Open3D DEBUG] ICP Iteration #12: Fitness 0.9503, RMSE 0.0787
[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25858)
[Open3D DEBUG] ICP Iteration #13: Fitness 0.9504, RMSE 0.0788
[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25862)
[Open3D DEBUG] ICP Iteration #14: Fitness 0.9505, RMSE 0.0788
[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25863)
[Open3D DEBUG] ICP Iteration #15: Fitness 0.9505, RMSE 0.0787
[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25863)
[Open3D DEBUG] ICP Iteration #16: Fitness 0.9505, RMSE 0.0788
[Open3D DEBUG] Residual : 1.01e-03 (# of elements : 25863)
[Open3D DEBUG] ICP Iteration #0: Fitness 0.6932, RMSE 0.0152
[Open3D DEBUG] Residual : 1.42e-04 (# of elements : 18863)
[Open3D DEBUG] ICP Iteration #1: Fitness 0.7057, RMSE 0.0106
[Open3D DEBUG] Residual : 3.39e-05 (# of elements : 19203)
[Open3D DEBUG] ICP Iteration #2: Fitness 0.7052, RMSE 0.0104
[Open3D DEBUG] Residual : 3.06e-05 (# of elements : 19190)
[Open3D DEBUG] ICP Iteration #3: Fitness 0.7054, RMSE 0.0104
[Open3D DEBUG] Residual : 3.07e-05 (# of elements : 19195)
[Open3D DEBUG] ICP Iteration #4: Fitness 0.7055, RMSE 0.0105
[Open3D DEBUG] Residual : 3.07e-05 (# of elements : 19198)
[Open3D DEBUG] ICP Iteration #5: Fitness 0.7055, RMSE 0.0105
[Open3D DEBUG] Residual : 3.07e-05 (# of elements : 19197)
Build o3d.pipelines.registration.PoseGraph
Apply point-to-plane ICP
[Open3D DEBUG] ICP Iteration #0: Fitness 0.7980, RMSE 0.1231
[Open3D DEBUG] Residual : 1.23e-02 (# of elements : 15272)
[Open3D DEBUG] ICP Iteration #1: Fitness 0.9232, RMSE 0.1132
[Open3D DEBUG] Residual : 9.84e-03 (# of elements : 17668)
[Open3D DEBUG] ICP Iteration #2: Fitness 0.9638, RMSE 0.0957
[Open3D DEBUG] Residual : 4.76e-03 (# of elements : 18445)
[Open3D DEBUG] ICP Iteration #3: Fitness 0.9648, RMSE 0.0865
[Open3D DEBUG] Residual : 2.63e-03 (# of elements : 18464)
[Open3D DEBUG] ICP Iteration #4: Fitness 0.9576, RMSE 0.0784
[Open3D DEBUG] Residual : 1.34e-03 (# of elements : 18326)
[Open3D DEBUG] ICP Iteration #5: Fitness 0.9506, RMSE 0.0755
[Open3D DEBUG] Residual : 8.08e-04 (# of elements : 18191)
[Open3D DEBUG] ICP Iteration #6: Fitness 0.9446, RMSE 0.0746
[Open3D DEBUG] Residual : 5.87e-04 (# of elements : 18077)
[Open3D DEBUG] ICP Iteration #7: Fitness 0.9400, RMSE 0.0740
[Open3D DEBUG] Residual : 4.83e-04 (# of elements : 17989)
[Open3D DEBUG] ICP Iteration #8: Fitness 0.9381, RMSE 0.0739
[Open3D DEBUG] Residual : 4.41e-04 (# of elements : 17953)
[Open3D DEBUG] ICP Iteration #9: Fitness 0.9373, RMSE 0.0737
[Open3D DEBUG] Residual : 4.30e-04 (# of elements : 17937)
[Open3D DEBUG] ICP Iteration #10: Fitness 0.9372, RMSE 0.0738
[Open3D DEBUG] Residual : 4.29e-04 (# of elements : 17936)
[Open3D DEBUG] ICP Iteration #11: Fitness 0.9372, RMSE 0.0739
[Open3D DEBUG] Residual : 4.29e-04 (# of elements : 17936)
[Open3D DEBUG] ICP Iteration #0: Fitness 0.7654, RMSE 0.0129
[Open3D DEBUG] Residual : 9.10e-05 (# of elements : 14648)
[Open3D DEBUG] ICP Iteration #1: Fitness 0.7605, RMSE 0.0109
[Open3D DEBUG] Residual : 4.28e-05 (# of elements : 14553)
[Open3D DEBUG] ICP Iteration #2: Fitness 0.7594, RMSE 0.0108
[Open3D DEBUG] Residual : 4.23e-05 (# of elements : 14533)
[Open3D DEBUG] ICP Iteration #3: Fitness 0.7592, RMSE 0.0108
[Open3D DEBUG] Residual : 4.24e-05 (# of elements : 14528)
Build o3d.pipelines.registration.PoseGraph
Open3D uses the function global_optimization to perform pose graph optimization. Two types of optimization methods can be chosen: GlobalOptimizationGaussNewton or GlobalOptimizationLevenbergMarquardt. The latter is recommended since it has better convergence property. Class GlobalOptimizationConvergenceCriteria can be used to set the maximum number of iterations and various optimization parameters.
Class GlobalOptimizationOption defines a couple of options. max_correspondence_distance decides the correspondence threshold. edge_prune_threshold is a threshold for pruning outlier edges. reference_node is the node id that is considered to be the global space.
[6]:
print("Optimizing PoseGraph ...")
option = o3d.pipelines.registration.GlobalOptimizationOption(
max_correspondence_distance=max_correspondence_distance_fine,
edge_prune_threshold=0.25,
reference_node=0)
with o3d.utility.VerbosityContextManager(
o3d.utility.VerbosityLevel.Debug) as cm:
o3d.pipelines.registration.global_optimization(
pose_graph,
o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
option)
Optimizing PoseGraph ...
[Open3D DEBUG] Validating PoseGraph - finished.
[Open3D DEBUG] [GlobalOptimizationLM] Optimizing PoseGraph having 3 nodes and 3 edges.
[Open3D DEBUG] Line process weight : 15.334500
[Open3D DEBUG] [Initial ] residual : 1.037615e+00, lambda : 2.960068e+00
[Open3D DEBUG] [Iteration 00] residual : 2.034300e-01, valid edges : 1, time : 0.000 sec.
[Open3D DEBUG] [Iteration 01] residual : 1.455356e-01, valid edges : 1, time : 0.000 sec.
[Open3D DEBUG] Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)
[Open3D DEBUG] [GlobalOptimizationLM] total time : 0.000 sec.
[Open3D DEBUG] [GlobalOptimizationLM] Optimizing PoseGraph having 3 nodes and 3 edges.
[Open3D DEBUG] Line process weight : 15.334500
[Open3D DEBUG] [Initial ] residual : 1.455332e-01, lambda : 3.051329e+00
[Open3D DEBUG] Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)
[Open3D DEBUG] [GlobalOptimizationLM] total time : 0.000 sec.
[Open3D DEBUG] CompensateReferencePoseGraphNode : reference : 0
The global optimization performs twice on the pose graph. The first pass optimizes poses for the original pose graph taking all edges into account and does its best to distinguish false alignments among uncertain edges. These false alignments have small line process weights, and they are pruned after the first pass. The second pass runs without them and produces a tight global alignment. In this example, all the edges are considered as true alignments, hence the second pass terminates immediately.
Visualize optimization¶
The transformed point clouds are listed and visualized using draw_geometries.
[7]:
print("Transform points and display")
for point_id in range(len(pcds_down)):
print(pose_graph.nodes[point_id].pose)
pcds_down[point_id].transform(pose_graph.nodes[point_id].pose)
o3d.visualization.draw_geometries(pcds_down,
zoom=0.3412,
front=[0.4257, -0.2125, -0.8795],
lookat=[2.6172, 2.0475, 1.532],
up=[-0.0694, -0.9768, 0.2024])
Transform points and display
[[ 1.00000000e+00 1.41136865e-19 1.08420217e-19 -1.73472348e-18]
[-7.57035697e-20 1.00000000e+00 1.08420217e-19 -8.67361738e-19]
[ 0.00000000e+00 1.08420217e-19 1.00000000e+00 0.00000000e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 0.84016209 -0.14644296 0.52218973 0.34786264]
[ 0.0061365 0.96535921 0.26085233 -0.39421642]
[-0.54230065 -0.21595382 0.81195686 1.73016434]
[ 0. 0. 0. 1. ]]
[[ 0.9627171 -0.07182071 0.26080179 0.37666506]
[-0.00193493 0.96225765 0.27213318 -0.48961689]
[-0.27050331 -0.2624919 0.92624293 1.29777557]
[ 0. 0. 0. 1. ]]
Make a combined point cloud¶
PointCloud has a convenience operator + that can merge two point clouds into a single one. In the code below, the points are uniformly resampled using voxel_down_sample after merging. This is recommended post-processing after merging point clouds since it can relieve duplicated or over-densified points.
[8]:
pcds = load_point_clouds(voxel_size)
pcd_combined = o3d.geometry.PointCloud()
for point_id in range(len(pcds)):
pcds[point_id].transform(pose_graph.nodes[point_id].pose)
pcd_combined += pcds[point_id]
pcd_combined_down = pcd_combined.voxel_down_sample(voxel_size=voxel_size)
o3d.io.write_point_cloud("multiway_registration.pcd", pcd_combined_down)
o3d.visualization.draw_geometries([pcd_combined_down],
zoom=0.3412,
front=[0.4257, -0.2125, -0.8795],
lookat=[2.6172, 2.0475, 1.532],
up=[-0.0694, -0.9768, 0.2024])
Note:
Although this tutorial demonstrates multiway registration for point clouds, the same procedure can be applied to RGBD images. See Make fragments for an example.