Python Open3D点云配准 ICP(Iterative Closest Point）

这篇博客将介绍 迭代最近点配准算法(Iterative Closest Point, ICP) 。多年来，它一直是研究和工业中几何注册的支柱。输入是两个点云和一个初始变换，该变换大致将源点云与目标点云对齐。输出是一个精确的变换，它将两个点云紧密对齐。

• 将展示俩种ICP：点对点ICP（PointToPoint）和点对面ICP（PointToPlane）。
• 函数 draw_registration_result 在icp过程中可视化对齐效果。目标点云和源点云分别用青色和黄色绘制。两个点云彼此重叠得越多越紧密，对齐结果越好。
• 函数 evaluate_registration 计算两个主要指标：
  • fitness：其测量重叠区域（内部对应的数量/目标中的点的数量），越高越好。
  • inlier_rmse，其测量所有inlier对应的rmse相似性，越低越好。

1. 效果图

原始点云黄色 VS 原始点云青色：

初始对齐 Point to Point ICP 匹配74056个点，效果图如下：

Apply point-to-point ICP
RegistrationResult with fitness=3.724495e-01, inlier_rmse=7.760179e-03, and correspondence_set size of 74056
Access transformation to get result.
Transformation is:
[[ 0.83924644 0.01006041 -0.54390867 0.64639961]
[-0.15102344 0.9
6521988 -0.21491604 0.75166079]
[ 0.52191123 0.2616952 0.81146378 -1.50303533]
[ 0. 0. 0. 1. ]]

Point to Plane ICP 匹配123471个点效果图如下：
可以看到点对面ICP更快达到收敛。fitness更高更好，inlier_rmse也小一些。

Apply point-to-plane ICP
RegistrationResult with fitness=6.209722e-01, inlier_rmse=6.581453e-03, and correspondence_set size of 123471
Access transformation to get result.
Transformation is:
[[ 0.84023324 0.00618369 -0.54244126 0.64720943]
[-0.14752342 0.96523919 -0.21724508 0.81018928]
[ 0.52132423 0.26174429 0.81182576 -1.48366001]
[ 0. 0. 0. 1. ]]

Initial alignment
RegistrationResult with fitness=1.747228e-01, inlier_rmse=1.177106e-02, and correspondence_set size of 34741
Access transformation to get result. 

Apply point-to-point ICP
RegistrationResult with fitness=3.724495e-01, inlier_rmse=7.760179e-03, and correspondence_set size of 74056
Access transformation to get result.
Transformation is:
[[ 0.83924644  0.01006041 -0.54390867  0.64639961]
 [-0.15102344  0.9
 6521988 -0.21491604  0.75166079]
 [ 0.52191123  0.2616952   0.81146378 -1.50303533]
 [ 0.          0.          0.          1.        ]] 

Apply point-to-plane ICP
RegistrationResult with fitness=6.209722e-01, inlier_rmse=6.581453e-03, and correspondence_set size of 123471
Access transformation to get result.
Transformation is:
[[ 0.84023324  0.00618369 -0.54244126  0.64720943]
 [-0.14752342  0.96523919 -0.21724508  0.81018928]
 [ 0.52132423  0.26174429  0.81182576 -1.48366001]
 [ 0.          0.          0.          1.        ]] 

2. 安装及原理

pip install open3d -i http://mirrors.aliyun.com/pypi/simple/

点云配准本质上是将点云从一个坐标系变换到另一个坐标系。通常会需要用到两个点云数据，第一类点云数据称为原始点云，用S（source）来表示，黄色。第二类点云数据称为目标点云，用T（Target）来表示，青色。

点云配准的主要任务是计算出旋转矩阵R和平移矩阵T。

2.1 点对点ICP

通常，ICP算法迭代两个步骤：

• 查找对应集K={（p，q）} 从目标点云P和用当前变换矩阵T变换的源点云Q中提取并更新转换T
• 通过最小化在对应集K上定义的目标函数E（T）
  ICP的不同变体使用不同的目标函数E（T）：

$$\mathbf{E}(\mathbf{T})=\sum _{(\mathbf{p},\mathbf{q})ϵ\kappa }\mid \mid \mathbf{p}-{\mathbf{T}}_{\mathbf{q}}\mid {\mid }^2$$

TransformationEstimationPointToPoint类提供了计算点对点ICP目标的残差和转换矩阵的函数。函数registration_icp将其作为参数并运行点对点icp以获得结果。

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

最终对准是紧密的。fitness分数提高到0.621123。inlier_rmse降低到0.006583。

2.2 点对面ICP

点对面ICP使用不同的目标函数E（T）：
$$\mathbf{E}(\mathbf{T})=\sum _{(\mathbf{p},\mathbf{q})ϵ\kappa }((\mathbf{p}-{\mathbf{T}}_{\mathbf{q}})\cdot {\mathbf{n}}_{\mathbf{p}}{)}^2$$

其中 $n_p$ 是点p的法线，表明点对平面ICP算法比点对点ICP算法具有更快的收敛速度。
使用不同的参数TransformationEstimationPointToPlane调用registration_icp。在内部，此类实现了计算点到平面ICP目标的残差和转换矩阵的函数。

3. 源码

# 使用官方demo点云对俩个原始点云进行点对点icp，点对面icp配准，并进行可视化，原始点云黄色，目标点云青色。
# python icp_registration.py
import copy

import numpy as np
import open3d as o3d


def draw_registration_result(source, target, transformation, title):
    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([source_temp, target_temp], title="Open3D " + title)


def point_to_point_icp(source, target, threshold, trans_init):
    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, "\n")
    draw_registration_result(source, target, reg_p2p.transformation, 'point-to-point ICP')


def point_to_plane_icp(source, target, threshold, trans_init):
    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, "\n")
    draw_registration_result(source, target, reg_p2l.transformation, 'point-to-plane ICP')


if __name__ == "__main__":
    # 本地没有会自动下载demo点云数据 https://github.com/isl-org/open3d_downloads/releases/download/20220301-data/DemoICPPointClouds.zip
    pcd_data = o3d.data.DemoICPPointClouds()
    source = o3d.io.read_point_cloud(pcd_data.paths[0])
    target = o3d.io.read_point_cloud(pcd_data.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, 'origin')

    print("Initial alignment")
    # 函数evaluate_registration计算两个主要指标：
    # fitness：其测量重叠区域（内部对应的数量/目标中的点的数量），越高越好。
    # inlier_rmse，其测量所有inlier对应的rmse相似性，越低越好。
    evaluation = o3d.pipelines.registration.evaluate_registration(
        source, target, threshold, trans_init)
    print(evaluation, "\n")

    point_to_point_icp(source, target, threshold, trans_init)
    point_to_plane_icp(source, target, threshold, trans_init)

参考

• http://www.open3d.org/docs/release/tutorial/pipelines/icp_registration.html
• http://www.open3d.org/docs/release/python_example/pipelines/index.html#colored-icp-registration-py
• https://github.com/seminar2012/Open3D
• http://www.open3d.org/docs/release/getting_started.html
• https://blog.csdn.net/suiyingy/article/details/124336448