icp算法的实现_【点云精配准】Iterative Closest Point（ICP）

1. ICP 算法简介

通常，点云配准分为两步，先做粗配准，再做精配准：

• 粗配准（Coarse Global Registeration）：基于局部几何特征
• 精配准（Fine Local Registeration）：需要初始位姿（initial alignment）

本文所讲的 ICP 就是最常用的点云精配准算法。

ICP 的经典论文：P.J. Besl, A method for registration of 3-D shapes, 1992.

发表在 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）求解 translation

对

求偏导，可得：

令

，求得：

定义质心：

因而，

将

代入

可得：

定义去质心坐标：

因此，目标函数变为：

（2）求解 rotation

是标量，对于任意标量

，都满足

，因此，

将其代入目标函数，可得：

因而，

由上图可知，

其中，

和

为

维矩阵。

定义协方差矩阵

，对

做 SVD 分解：

因而，求解问题变为使下式最大化：

令

，其是正交阵，其列向量

是 orthonormal vectors，即

。因此， 对

的所有元素都有

。

当

时，

最大；

又是正交阵，因此

必为单位阵。

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

void 

2. PCL 库中的 ICP 算法

PCL 库中提供了以下 ICP 的接口及其变种：

• 点到点：pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >
• 点到面：pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >
• 面到面：pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >
• ……

其中，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)

基本用法：

IterativeClosestPoint

Demo

以下两个demo 来自 PCL Tutorial

（1）Interactive ICP（交互式 ICP）

Point Cloud Library (PCL)​pointclouds.org

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

#include 

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

How to incrementally register pairs of clouds​pointclouds.org

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

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

#include 

参考

1. 高翔，视觉 SLAM 十四讲
2. Least-Squares Rigid Motion Using SVD
3. ICP（迭代最近点）算法
4. PCL点云库：ICP算法
5. Interactive Iterative Closest Point
6. How to incrementally register pairs of clouds