基于PCL的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矩阵。假定第一次计算的矩阵为$R_0$和$T_0$。PCL中ICP的初始矩阵为

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

也就是：
$\left[\begin{array}{cccc}1& & & 0\\ & 1& & 0\\ & & 1& 0\\ & & & 1\end{array}\right]$
公式如下：
$p_i$=$R_0$*$q_i$+$T_0$
其中：

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

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

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

PCL迭代部分的代码如下：

 // Estimate the transform
    transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);

    // Transform the data
    transformCloud (*input_transformed, *input_transformed, transformation_);

    // Obtain the final transformation
    final_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自适应算法。