一分钟详解点云配准ICP方法

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

作者：丁洪凯

链接：https://zhuanlan.zhihu.com/p/107218828

本文转载自知乎，作者已授权，未经许可请勿二次转载。

1. ICP 算法简介

ICP 的经典论文：https://ieeexplore.ieee.org/document/121791

发表在 IEEE Transactions on Pattern Analysis and Machine Intelligence，此刊在工程技术大类属于1区，算是神刊了。

【大类】工程技术（1区）;
【小类】COMPUTER SCIENCE, ARTIFICIAL INTELLIGENCE（1区）；ENGINEERING, ELECTRICAL & ELECTRONIC（1区）;
【2016-2018年平均IF】11.838；
【2017-2018年总被引】102333

进入正题......

ICP 算法的第一步就是找到 Source 点云与 Target 点云中的对应点（corresponding point sets），然后针对对应点，通过最小二乘法构建目标函数，进行迭代优化。

1.1 估计对应点（Correspondences estimation）

ICP 称为 Iterative Closest Point，顾名思义，是通过最近邻法来估计对应点的。

对 Source 点云中的一点，求解其与 Target 点云中距离最近的那个点，作为其对应点。

当然，这样操作的时间复杂度很高，为了加速计算，我们不需要计算 Target 点云中每个点到 Source 点云中一点的距离。可以设定一个阈值，当距离小于阈值时，就将其作为对应点。

还有一些其他加速求解对应点的方法，如 ANN(Approximate Nearest Neighbor)。

1.2 迭代优化

找到对应点后，我们就构建了两组对应的点集:

求欧式变换使得：

ICP 算法基于最小二乘法进行迭代计算，使得误差平方和达到极小值：

可通过以下三个步骤进行求解：

（1）定义两组点的质心，简化目标函数

其中，交叉项部分在求和之后零，因此目标函数简化为：

第一项只与旋转矩阵有关，只要获得，令第二项为零就能求得。

（2）计算每个点的去质心坐标，计算旋转矩阵

其中，第二项与无关，只有第三项与有关。因而，目标函数变为：

通过 Singular Value Decomposition (SVD) 来进行求解，先定义矩阵：

是一个矩阵，对进行 SVD 分解，得：

其中，为奇异值组成的对角矩阵，当满秩时，为：

（3）计算平移矩阵

以下求解 ICP 的代码来自：高翔，视觉SLAM十四讲。

2. PCL 库中的 ICP 算法

基本使用

PCL 库中提供了如下几种点云配置算法：

pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >
pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >
pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >
pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar >
pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >
pcl::registration::IncrementalICP< PointT, Scalar >

其中，IterativeClosestPoint 模板类是 ICP 算法的一个基本实现，其优化求解方法基于 Singular Value Decomposition (SVD)，算法迭代结束条件包括：

最大迭代次数：Number of iterations has reached the maximum user imposed number of iterations (via setMaximumIterations)
两次变换矩阵之间的差值：The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via setTransformationEpsilon)
均方误差：The sum of Euclidean squared errors is smaller than a user defined threshold (via setEuclideanFitnessEpsilon)

基本用法：

原理解析

PCL 中的 ICP 是通过Registeration 模板类实现的，其继承关系如下图所示。

ICP 点云配准的关键概念如下：

Keypoints 关键点：A keypoint is an interest point that has a "special property" in the scene. PCL 中的 NARF，SIFT，FAST 可以作为关键点，也可以把每一个点或部分点作为关键点；
Feature descriptors 特征描述子：每个关键点对应着一个特征描述子，相似的关键点有着相似的特征描述子，通常用一个向量表示；
Correspondences estimation 对应点估计：ICP 算法的第一步就是要找到对应点，即找到两个点云中 overlapping 的部分，分为 point matching 和 feature matching；
Correspondences rejection 对应点去除：去除不好的对应点可通过 RANSAC 实现，若模型点云中的一个点对应着场景点云中的若干点，则只取距离最小的那个；
Transformation estimation 位姿变换估计：定义目标函数，迭代优化直到收敛。

主要流程如下图所示：

定义关键点，能够最好地表示场景（from a set of points, identify interest points (i.e., keypoints) that best represent the scene in both datasets）；
针对每个关键点，计算特征描述子（at each keypoint, compute a feature descriptor）；
基于相似的特征和位置，估计一系列对应点（from the set of feature descriptors together with their XYZ positions in the two datasets, estimate a set of correspondences, based on the similarities between features and positions）；
假设数据是有噪声的，则并不是所有的对应点都是有效的，所以需要去除掉一些不好的对应点（given that the data is assumed to be noisy, not all correspondences are valid, so reject those bad correspondences that contribute negatively to the registration process）；
基于好的对应点，估计位姿变换（from the remaining set of good correspondences, estimate a motion transformation）。

http://pointclouds.org/documentation/tutorials/registration_api.php#registration-api

3. PCL Demo

以下两个demo 来自 PCL Tutorial

(1)Interactive ICP（交互式 ICP）http://pointclouds.org/documentation/tutorials/interactive_icp.php#id1

效果：按一下空格键，迭代一次

(2)Incrementally register pairs of clouds（多幅点云配准）

http://pointclouds.org/documentation/tutorials/pairwise_incremental_registration.php#pairwise-incremental-registration

点云模型：https://github.com/PointCloudLibrary/data/tree/master/tutorials/pairwise

效果：按 q 键进行迭代配准，同时生成配准后的点云

参考

高翔，视觉 SLAM 十四讲

ICP（迭代最近点）算法

PCL点云库：ICP算法

Interactive Iterative Closest Point

How to incrementally register pairs of clouds

推荐阅读：

吐血整理|3D视觉系统化学习路线

那些精贵的3D视觉系统学习资源总结（附书籍、网址与视频教程）

超全的3D视觉数据集汇总

大盘点|6D姿态估计算法汇总(上)

大盘点|6D姿态估计算法汇总（下）

机器人抓取汇总|涉及目标检测、分割、姿态识别、抓取点检测、路径规划

汇总|3D点云目标检测算法

汇总|3D人脸重建算法

那些年，我们一起刷过的计算机视觉比赛

总结|深度学习实现缺陷检测

深度学习在3-D环境重建中的应用

汇总|医学图像分析领域论文

大盘点|OCR算法汇总

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿微信交流群，旨在交流顶会（ICRA/IROS/ROBIO/CVPR/ICCV/ECCV等）、顶刊（IJCV/TPAMI/TIP等）、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近1000+星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题