• 本教程演示了ICP（Iterative Closest Point）迭代最近点配准算法。多年来，它一直是研究和工业中几何配准的中流砥柱。输入是两个点云和一个初始变换，该变换将源点云大致对齐到目标点云。输出是一个精细的变换，可紧密对齐两个点云。辅助函数draw_registration_result在配准过程中可视化对齐。在本教程中，我们展示了两个ICP变体，即点到点ICP和点到面ICP[Rusinkiewicz2001]

辅助可视化函数

• 下面的函数可视化目标点云和通过对齐变换变换的源点云。目标点云和源点云分别涂有青色和黄色。两个点云之间的重叠越多、越紧密，对齐结果就越好。

def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp],
                                      zoom=0.4459,
                                      front=[0.9288, -0.2951, -0.2242],
                                      lookat=[1.6784, 2.0612, 1.4451],
                                      up=[-0.3402, -0.9189, -0.1996])

注意：由于函数transform和paint_uniform_color改变点云，我们调用copy.deepcopy来制作副本并保护原始点云。

输入

• 下面的代码从两个文件中读取源点云和目标点云。给出了一个粗略的变换。

注意：初试对齐通常通过全局配准算法获得。有关实例，请参阅全局配准。

demo_icp_pcds = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])
target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])
threshold = 0.02
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
                         [-0.139, 0.967, -0.215, 0.7],
                         [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
draw_registration_result(source, target, trans_init)

• 该函数evaluate_registration计算两个主要指标：
• fitness，用于测量重叠区域（内嵌对应关系的数目/目标中的点数）。越高越好。
• inlier_rmse，它测量所有内嵌对应关系的RMSE。越低越好。

print("Initial alignment")
evaluation = o3d.pipelines.registration.evaluate_registration(
    source, target, threshold, trans_init)
print(evaluation)

点到点ICP

• 通常，ICP算法循环迭代两个步骤：
• 1、从目标点云P和用当前变换矩阵T变换源点云Q中求出对应集K={(p, q)}。
• 2、通过最小化对应集K上定义的目标函数E(T)来更新变换T。
• ICP的不同变体使用不同的目标函数E(T)[BeslAndMcKay1992][ChenAndMedioni1992][Park2017]。
• 我们首先展示了一个点到点ICP算法[BeslAndMcKay1992]，使用目标

• 类TransformationEstimationPointToPoint提供了用于计算点到点ICP目标的残差和雅克比矩阵的函数。该函数registration_icp将其作为参数，运行点到点ICP以获得结果。

print("Apply point-to-point ICP")
reg_p2p = o3d.pipelines.registration.registration_icp(
    source, target, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)

• fitness分数从0.174723增加到0.372450。inlier_rmse从0.011771降低到0.007760。默认情况下，registration_icp运行直到收敛或达到最大迭代次数（默认为30次）。可以更改它以允许更多的计算时间并进一步改进结果。

reg_p2p = o3d.pipelines.registration.registration_icp(
    source, target, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint(),
    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=2000))
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)

• 最后的对齐是紧密的。fitness提高到0.621123。inlier_rmse减少到0.006583。

点到面ICP

• 点到面ICP算法[ChenAndMedioni1992]使用不同的目标函数

• 其中np是点p的法向量。[Rusinkiewicz2001]已经证明，点到面ICP算法比点到点ICP算法具有更快的收敛速度。
• registration_icp使用不同的参数TransformationEstimationPointToPlane调用。在内部，该类实现了函数来计算点到面ICP目标的残差和雅克比矩阵。

print("Apply point-to-plane ICP")
reg_p2l = o3d.pipelines.registration.registration_icp(
    source, target, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPlane())
print(reg_p2l)
print("Transformation is:")
print(reg_p2l.transformation)
draw_registration_result(source, target, reg_p2l.transformation)

• 点到面ICP在30次迭代中达到紧密对齐（fitness得分为0.620972，inlier_rmse得分为0.006581）。

点到面ICP算法使用法向量。在本教程中，我们将从文件加载法向量。如果未给出法向量，则可以使用顶点法向量估计来计算他们。