文章目录

前言
一、ICP算法基础
- 1.1 提取待匹配点对
- 1.2 计算旋转平移矩阵
- 1.3 计算变换后的点和目标点之间的偏差
二、ICP算法变种
- 2.1 PLICP
- 2.2 PointToPlane ICP
- 2.3 NICP
- 2.4 LM_ICP
三、程序示例
- 1. 传统方法
- 2. PointToPlane ICP
总结

前言

ICP（Iterative Closest Point，最近邻点迭代）是应用最广泛的三维点云配准算法之一，网上讲ICP算法原理的非常多，这里列举几个个人觉得讲的比较好的。
三维点云配准 – ICP 算法原理及推导
ICP（迭代最近点）算法
PCL学习笔记二：Registration (ICP算法)

原始的ICP算法的问题在于点对之间只分析距离之间的关系从而引起迭代次数高，最终导致的计算时间过长，所以很多学者提出了各种各样的改进算法，如：PLICP，PointToPlane ICP，NICP，LM_ICP算法等。
本文对各种ICP算法的原理以及其简单的应用进行分析。

一、ICP算法基础

ICP算法的基本逻辑是先通过对应关系估计、关键点检测等方法找到源点云和目标点云的待匹配点对，然后计算旋转和平移矩阵，对source点云进行旋转平移到target点云上，根据设置的阈值进行判断，如果不满足阈值要求就重复以上过程继续计算，最终达到最大迭代次数或者满足设定的均方差阈值才能停止迭代。
可以用一个公式表示：

1.1 提取待匹配点对

首先按需要进行blob。
然后进行Correspondences estimation（对应关系估计），得到共同部分的点云。

/** \brief A CorrespondenceEstimation object, used to estimate correspondences between the source and the target cloud. */CorrespondenceEstimationPtr correspondence_estimation_;/** \brief The minimum number of correspondences that the algorithm needs before attempting to estimate the * transformation. The default value is 3.*/int min_number_correspondences_;correspondence_estimation_->setInputTarget (target_);if (correspondence_estimation_->requiresTargetNormals ())correspondence_estimation_->setTargetNormals (target_blob);

具体源码自行查看，下面列出Correspondences estimation的计算代码,里面包含了两种计算方法，分别为determineCorrespondences和determineReciprocalCorrespondences 。
determineCorrespondences会计算所有点的对应关系。
determineReciprocalCorrespondences计算两个点云共同部分的对应关系。

最后进行一个CorrespondenceRejector，去除错误的估计。

 for (std::size_t i = 0; i < correspondence_rejectors_.size (); ++i){registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];if (rej->requiresTargetPoints ())rej->setTargetPoints (target_blob);if (rej->requiresTargetNormals () && target_has_normals_)rej->setTargetNormals (target_blob);}

1.2 计算旋转平移矩阵

我们默认变换为刚性变换，通过空间中两点间的变换关系计算R和T矩阵。假定第一次计算的矩阵为R0R_0R0和T0T_0T0。PCL中ICP的初始矩阵为

Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
Eigen::Matrix4f tr = transform.template cast<float> ()

也就是：
[1010101]\begin{bmatrix} 1&&&0\\ &1&&0\\&&1&0\\&&&1\end{bmatrix}⎣⎢⎢⎡1110001⎦⎥⎥⎤
公式如下：
pip_ipi=R0R_0R0*qiq_iqi+T0T_0T0
其中：

T = [txtytz]\begin{bmatrix} tx&ty&tz\end{bmatrix}[txtytz]
具体的计算采用奇异值分解的方法，具体计算过程前言部分推荐的文章有写。

1.3 计算变换后的点和目标点之间的偏差

对点集p进行变换，计算变换后的点集p1p_1p1和q的距离值。
Distance=∑i=1n∣∣p1\displaystyle\sum_{i=1}^{n}||p_1i=1∑n∣∣p1-q ||2^22
Distance和设定的阈值进行对比，如果大于，则跳到第一步重新开始循环迭代，如果达到最大迭代次数还没有满足阈值要求，也会停止迭代，保留最近一次的迭代结果。
PCL中还有一个收敛条件设置setTransformationEpsilon，意思是上一个变换矩阵和当前变换矩阵的差异值，如果差异值小于阈值，也认为达到收敛。

PCL迭代部分的代码如下：

 // Estimate the transformtransformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);// Transform the datatransformCloud (*input_transformed, *input_transformed, transformation_);// Obtain the final transformationfinal_transformation_ = transformation_ * final_transformation_;++nr_iterations_;

二、ICP算法变种

因为计算点对间的最小距离的方法过于耗时和低效，所以出现了很多加速方法，例如先提取点云特征，然后进行特征间的匹配，可以极大减少匹配时间；或者对计算源点云中点到目标点云对应点线或者面的最小距离，可以降低。

2.1 PLICP

PLICP计算当前帧中的点到参考帧中最近邻两点连线的最小距离值，在slam中应用较多，可以或者更小的迭代次数和更高的精度。
原理可以参考论文：
A. Censi, “An ICP variant using a point-to-line metric,” 2008 IEEE International Conference on Robotics and Automation, Pasadena, CA, 2008, pp. 19-25, doi: 10.1109/ROBOT.2008.4543181

2.2 PointToPlane ICP

计算Source点云中点到目标点云对应点形成的面的最小距离值。

这里ni为qi的法线,minE为最小欧式距离。

2.3 NICP

NICP与传统ICP的不同之处在于NICP会多考虑曲率和法线的方向一致问题，如果对应点的曲率和法线方向超过设定阈值，不会建立对应点的匹配。所以在计算的时候需要计算点云的法线信息，然后进行匹配时需要额外对对应点对的法线和曲率限定阈值。

2.4 LM_ICP

LM_ICP在计算最小距离的时候采用LM模型来进行，算法原理可以查看论文：
结合Kinect的双目视觉场景三维重建。

三、程序示例

1. 传统方法

传统方法计算全部点云的对应关系导致计算时间非常长。

icp.setInputSource(source);
icp.setInputTarget(target);
icp.setTransformationEpsilon(1e-8);
icp.setMaxCorrespondenceDistance(1); //添加一个最大距离的阈值，超过最大距离的点不作为对应点。
icp.setEuclideanFitnessEpsilon(0.00005);
icp.setMaximumIterations(150);
icp.setUseReciprocalCorrespondences(true);

迭代1次：

迭代134次：

2. PointToPlane ICP

PointToPlane ICP的核心类是IterativeClosestPointWithNormals，默认情况下，此实现使用传统的点对面目标，并使用目标点云的法线计算点对面距离。
另外在计算法线的时候采用OpenMP来进行多线程加速。

pcl::IterativeClosestPointWithNormals<pcl::PointNormal, pcl::PointNormal>ptoplane_icp;ptoplane_icp.setInputSource(source_with_normals);
ptoplane_icp.setInputTarget(target_with_normals);
ptoplane_icp.setTransformationEpsilon(1e-8);
ptoplane_icp.setMaxCorrespondenceDistance(1);
ptoplane_icp.setEuclideanFitnessEpsilon(0.0001);
ptoplane_icp.setMaximumIterations(150);

可见只进行了7次迭代，用时1.6s.

总结

ICP算法功能强大，算法种类也很多，在实际使用时需要根据实际应用场景开发适合的ICP自适应算法。

基于PCL的ICP及其变种算法实现相关推荐

基于PCL库的通过ICP匹配多幅点云方法
基于PCL库的通过ICP匹配多幅点云方法前言 Code Result 前言 PCL库中有很多配准的方式,主要都是基于ICP ICP算法最初由Besl和Mckey提出,是一种基于轮廓特征的点配准方法. ...
PCL：超详细的基于法向量和曲率的区域生长算法原理以及源码解读
---------原理介绍: (1)首先计算出来各点的曲率值,将曲率值按照从小到大的顺序进行排序. (2)设置一空的种子点序列和一个空的聚类数组. (3)选取曲率最小的点放入上述种子点序列中. (4) ...
29 基于PCL的点云平面分割拟合算法技术路线（针对有噪声的点云数据）
0 引言最近项目中用到了基于PCL开发的基于平面的点云和CAD模型的配准算法,点云平面提取采用的算法如下. 1 基于PCL的点云平面分割拟合算法 2 参数及其意义介绍 (1)点云下采样 1. 参数: ...
vs2019 基于pcl和opencv的体积检测算法 zed版本+安卓端新手版（二）
vs2019 基于pcl和opencv的体积检测算法 zed版本+安卓端新手版(二) Pcl库安卓端的环境配置和使用jni实现c++算法在安卓端的实现 Pcl库的编译安装ubuntu系统编译pcl ...
GICP：基于体素泛化ICP方式的准确快速点云配准方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达标题: Voxelized GICP for Fast and Accurate 3D Point C ...
【PCL】ICP 源码分析
目录&索引一.ICP 介绍二.demo 示例 2.1 代码 2.2 输出结果三.源码分析 3.1 align() 函数 3.1.1 computeTransformation() 函数 ...
基于深度学习的目标检测算法综述（一）
基于深度学习的目标检测算法综述(一) 基于深度学习的目标检测算法综述(二) 基于深度学习的目标检测算法综述(三) 本文内容原创,作者:美图云视觉技术部检测团队,转载请注明出处目标检测(Object ...
python最优调配问题_Python实现的基于优先等级分配糖果问题算法示例
Python实现的基于优先等级分配糖果问题算法示例本文实例讲述了Python实现的基于优先等级分配糖果问题算法.分享给大家供大家参考,具体如下: 问题: 有n个人,每个人有一定的优先等级,等级高的人 ...
PCL之点云分割算法概述
PCL中提供了点云分割的基础数据结构和部分通用算法,目前实现的算法主要是基于聚类分割思想和基于随机采样一致性的分割算法,以下对这两种方法的原理进行介绍: 1) 基于聚类分割算法在聚类方法中每个点都与 ...

基于PCL的ICP及其变种算法实现