点云（刚性）配准--icp

前言

提到配准算法，icp认第二，没哪种算法敢认第一，可见，icp这道坎是绕不过去的，在本文中，会重点介绍icp的原理与实现方案，同时，也会结合pcl中的代码进行详细介绍。

一、icp原理与步骤

1.1 目标

从本质上讲，icp类算法的基本原理是：对于两组点云P和Q，计算旋转矩阵R与旋转矩阵T，使目标函数E最小：
E = $\frac{1}{2}$ $\sum$ $||q-R\ast p-T|{|}^2$
其中，$q\in Q$， $p\in P$。

1.2 原理

icp Iterated Closest Point中文翻译未迭代最近点算法，其核心在迭代，通过不断迭代使点云数据达到收敛：首先，需要计算Q与P中的对应点对，计算对应点对的方法比较多，按照原理可以分为多种变体，有Point to Point，Point to plane等，同时，为了提高对应点计算的速度，还可以合理的使用kd tree，octree等数据结构；然后，根据计算的点对关系计算转换矩阵（旋转与平移）。转换矩阵的求解大体分为两种：利用线性代数的求解（SVD奇异值分解），及利用非线性优化方式的方式求解（类似于BA, bundle Adjustment），其中奇异值分解的方法因为准确、稳定以及高效的特点被广泛运用（在Eigen中已经提供SVD分解方法，本文不做过多研究）。

1.3步骤

实现icp的主要步骤有：
步骤1：点云归一化即将两个点云的某一特征点（通常会使用质心，也可以选取重心或其他合理的点）平移至共同坐标原点，其目的是为了方便计算对应点对；
步骤2：确定对应点对即在关系模糊的两个点云中，更合理的预判对应关系，经典icp中是根据point to point的欧式距离进行判断，假设两片点云中欧式距离最近的点作为对应点。该方法具有一定的普适性，在后续优化中，也出现了以point to plane作为对应点的计算规则，该对应关系的查找占用大量资源，故pcl中利用kd_tree的数据结构实现查找的加速，当然，也可以通过cuda实现加速。
步骤3：矩阵计算对步骤2中提取的有序点对进行矩阵计算，主要方法有SVD、BA的方法（具体方法参考1.4），获得R和T;
步骤4：变换点云：将计算获取的R和T作用于正片点云；
步骤5：计算目标函数以点对间的欧式距离和作为目标函数；
步骤6：判断目标函数与阈值的关系，决定是否需要进行下一轮的迭代。

1.4核心计算方法

首先计算两组点云Q和P的质心，分别表示为${\mu }^q$与${\mu }^p$，计算公式为：
$$\{\begin{array}{l}{\mu }^q=\frac{1}{N}{\sum }_i{q}_i,q_i\in Q\\ {\mu }^p=\frac{1}{N}{\sum }_i{p}_i,p_i\in P\end{array}$$
其次，对于P与Q中的数据进行归一化处理，即将点云移至中心点，具体计算公式为：
$$\{\begin{array}{l}{q}_i=q_i-{\mu }^q\\ p_i=p_i-{\mu }^p\end{array}$$
然后，构建矩阵C（该矩阵为3*3）:
$$C=\sum _{i=1}^N{p}_i{q}_i^T$$
最后，对矩阵C进行SVD分解：
$C=U\sum V^T$
得到R和T:
$$\{\begin{array}{l}R=V{U}^T\\ T={\mu }^q-R{\mu }^p\end{array}$$

1.5算法流程

根据自己的理解以及程序实现中的感悟，在这里画一个蹩脚的流程图，简要说明下算法逻辑，如果有错误，烦请指正：

画的有点丑，先凑合看下吧，在这个流程中，有两个环节是比较重要的：计算对应点对与计算对应点间的旋转矩阵，其中计算对应点对是至关重要的，这就牵扯到如何树立一种标准，才能使两片点云重合的比较完美，因此有很多人在这个过程上做工作，才有了n个icp变种，例如，point to point 、point to plane、plane to plane等；另一个计算对应点间的旋转矩阵也很重要，但是老实讲，个人认为这已经成为一步死棋，不管是利用SVD还是unmerga的方法，偏差总会有的。

二、代码解读

2.1 C++实现

为保持页面整洁性，故将相关代码以资源形式上传，连接为:
https://download.csdn.net/download/weixin_42823098/13703315

2.2 PCL实现与解析

2.2.1程序调用

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(source_cloud);
	icp.setInputTarget(target_cloud);
	icp.setMaxCorrespondenceDistance(30.0);//点之间最大距离,影响大,鞋面是4.0,5.0
	icp.setTransformationEpsilon(1e-10);
	icp.setEuclideanFitnessEpsilon(0.00001);
	icp.setMaximumIterations(2500);
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_out(new         pcl::PointCloud<pcl::PointXYZ>);
	icp.align(*source_out);//Final是source_cloud变换后的点云

	float score = icp.getFitnessScore();
	//std::cout << "计算误差 = " << score << std::endl;

	Eigen::Matrix4f trans = icp.getFinalTransformation();

2.2.2源码解析–求解矩阵

对源码进行解析，在计算对应点之间的旋转+平移矩阵时，主要解法有两种，其一为，采用Umeyama算法，其二为采用SVD分解的方式，主要调用关系（盗图参考）与部分核心源码如下:
下列源码为采用Umeyama算法(最小二乘估计)计算对应点对关系：

    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_src (3, npts);
    Eigen::Matrix<Scalar, 3, Eigen::Dynamic> cloud_tgt (3, npts);
    //cloud_src与cloud_tgt是点对关系
    for (int i = 0; i < npts; ++i)
    {
      cloud_src (0, i) = source_it->x;
      cloud_src (1, i) = source_it->y;
      cloud_src (2, i) = source_it->z;
      ++source_it;

      cloud_tgt (0, i) = target_it->x;
      cloud_tgt (1, i) = target_it->y;
      cloud_tgt (2, i) = target_it->z;
      ++target_it;
    }
    // Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
    //计算cloud_src与cloud_tat的旋转矩阵
    transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);

下列源码为采用SVD算法计算对应点对关系：

source_it.reset (); target_it.reset ();
    // <cloud_src,cloud_src> is the source dataset
    transformation_matrix.setIdentity ();

    Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
    // Estimate the centroids of source, target
    compute3DCentroid (source_it, centroid_src);
    compute3DCentroid (target_it, centroid_tgt);
    source_it.reset (); target_it.reset ();

    // Subtract the centroids from source, target
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
    demeanPointCloud (source_it, centroid_src, cloud_src_demean);
    demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);

    getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);

2.2.3源码解析–计算对应点对

上述讲解了如何计算对应点对的转换矩阵，现在讲解下如何计算对应点对，经典icp指的的point to point的方法，即以点与点之间的某种关系作为一个度量值，pcl中的icp用的就是这种方法，其基本思路为：计算目标点的最近欧式距离，若小于某个阈值，则认为是对应点，若大于某个阈值，则认为是无效点，下面展示point to point的源码片段：

  //目录\pcl\registration\impl\correspondence_estimation
 // Iterate over the input set of source indices
    for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
    {
      tree_->nearestKSearch ((*input_)[*idx], 1, index, distance);
      if (distance[0] > max_dist_sqr)
        continue;
      corr.index_query = *idx;
      corr.index_match = index[0];
      corr.distance = distance[0];
      correspondences[nr_valid_correspondences++] = corr;