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.96521988 -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）：

E(T)=∑(p,q)ϵκ∣∣p−Tq∣∣2\bold {E(T)=\sum_{(p,q)\epsilon\kappa} \mid\mid p-T_q \mid \mid ^2 }E(T)=(p,q)ϵκ∑∣∣p−Tq∣∣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）：
E(T)=∑(p,q)ϵκ((p−Tq)⋅np)2\bold {E(T)=\sum_{(p,q)\epsilon\kappa} ((p-T_q) \cdot n_p)^2} E(T)=(p,q)ϵκ∑((p−Tq)⋅np)2

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

3. 源码

# 使用官方demo点云对俩个原始点云进行点对点icp，点对面icp配准，并进行可视化，原始点云黄色，目标点云青色。
# python icp_registration.py
import copyimport numpy as np
import open3d as o3ddef 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.zippcd_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.02trans_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

Python Open3D点云配准点对点，点对面ICP(Iterative Closest Point）相关推荐

点云旋转平移（三）—python open3d点云旋转
点云旋转平移介绍,请参考上一节:点云旋转平移(一)-基础知识介绍_Coding的叶子的博客-CSDN博客. 本节所使用的示例pcd点云文件请参考:pcd格式点云样例文件-深度学习文档类资源-CSDN下 ...
点云旋转平移（二）—python open3d点云平移
点云旋转平移介绍,请参考上一节:点云旋转平移(一)-基础知识介绍_Coding的叶子的博客-CSDN博客. 本节所使用的示例pcd点云文件请参考:pcd格式点云样例文件-深度学习文档类资源-CSDN下 ...
点云配准的传统算法ICP与NDT概述
公众号致力于分享点云处理,SLAM,三维视觉,高精地图相关的文章与技术,欢迎各位加入我们,一起交流一起进步,有兴趣的可联系微信:920177957.本文来自点云PCL博主的分享,未经作者允许请勿转载, ...
python open3d点云可视化（本节会根据实际所用持续更新）
本文为博主原创文章,未经博主允许不得转载. 本文为专栏<python三维点云从基础到深度学习>系列文章,地址为"https://blog.csdn.net/suiyingy/ar ...
点云配准 3- icp-交互式ICP点云配准
一.声明本人作为初学者,才开始接触点云配准这一块,如有错误地方,望大家指出,我将及时修改,共同进步.其次这一部分主要是点云数据配准的第一个程序,也是官方代码比较直观的一次icp配准实验. 二.数据来 ...
点云配准（二）— python open3d ICP方法
上一节中介绍了点云配准的基础知识.本节将采用python open3d来进行点云配准. open3d安装和点云配准介绍,请参考: Open3d读写ply点云文件_Coding的叶子的博客-CSDN博客 ...
教程：Python Open3d 完成 ICP 点云配准
Python Open3d 完成 ICP 点云配准关于Open3d Open3D 是一个在Python和C++平台上的三维数据处理与可视化库.它由 Qian-Yi Zhou,Jaesik Park, ...
Open3d系列 | 1. Open3d实现点云数据读写、点云配准、点云法向量计算
如有错误,恳请指出. 从这一篇博客开始,开始利用Open3d来处理点云数据.之后将围绕点云数据的多种处理方式来记录笔记.本篇博客的内容包括点云的文件格式介绍,点云数据的读取,以及点云的配准与点云的法向 ...
python 点云配准,python点云滤波点云配准用python怎
点云配准用python怎么样点云配准过程,就是分享一个两个点云之间的旋转平移矩阵(rigid transform or euclidean transform 刚性变换或欧式变换),将源点云(sou ...
两种常见的点云配准方法ICPNDT
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:于凡 https://zhuanlan.zhihu.com/p/96908474 本文转载自知乎 ...

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