原理可以看博客点云配准、拼接概念综述_点云拼接-CSDN博客，这里只是记笔记。
点云扫描设备在对环境进行扫描时，往往不能在同一坐标系下将环境的点云数据一次性测量。其原因是环境大小超过了扫描设备的测量范围，并且环境里的物体之间相互遮挡，点云扫描设备在一个角度不太可能扫描到物体的完整点云。得到多片点云数据后，我们需要一种技术将多片点云数据旋转平移到统一的坐标系下，使它们能够组成完整的环境点云数据，这种技术叫点云拼接。
点云拼接是任意位置的点云的重叠部分相互配准的过程，点云配准分为刚体和非刚体（只存在空间旋转平移变换的配准问题称为刚体配准，存在缩放、变形、仿射变换的配准问题称为非刚体配准），这里主要介绍刚体点云配准。刚体配准计算旋转矩阵R和平移矩阵T，使源点云旋转平移过去与目标点云重合，保证两片点云对应部分尽可能多的重合。解决的办法是可以将他转化成一个优化问题，即在适当的度量空间中，通过求解最佳旋转和平移矩阵使这样的数据集之间的重叠区域之间的各对应点的平均距离最小。
如上图，将两片点云的重叠部分进行配准，两片点云组合成一片更为完整的点云即实现了两片点云的拼接。
点云配准按照初始条件与精确度等，可以分为粗配准与精配准两种配准方法。
粗配准是在源点云与目标点云完全不知道任何初始相对位置的情况下，所进行的配准方法。该方法的主要目的是在初始条件未知的情况下，快速估算一个大致的点云配准矩阵。整个计算过程要求比较高的计算速度，对于计算结果的精确度则不做过高的要求。常见的粗配准算法的思路包括了：基于局部特征描述的方法、基于全局搜索策略以及通过统计学概率等方法。
其中，基于局部特征描述的方法是通过提取源点云与目标点云的邻域几何特征，通过几何特征快速确定二者之间的点对的对应关系，再计算此关系进而获得变换矩阵。而点云的几何特征包括了很多种，比较常见的即为快速点特征直方图（FPFH）。
基于全局搜索策略的代表算法是采样一致性算法（SAC_IA），该算法在源点云与目标点云之间随机选取几何特征一致的点组成点对。通过计算对应点对的变换关系，得到最优解。
正态分布算法（NDT）利用统计学概率的方法，根据点云正态分布情况，确定了对应点对从而计算源点云与目标点云之间的变换关系。
精配准是利用已知的初始变换矩阵，通过迭代最近点算法（ICP算法）等计算得到较为精确的解。ICP算法通过计算源点云与目标点云对应点距离，构造旋转平移矩阵RT，通过RT对源点云变换，计算变换之后的均方差。若均方差满足阈值条件，则算法结束。否则则继续重复迭代直至误差满足阈值条件或者迭代次数终止。ICP算法具有以下特点：
优点：配准结果精确度较高，是一种精确配准算法；
缺点：对于两片点云的初始位置要求较为严格，否则容易陷入局部收敛且会
影响配准速度，因此需要通过粗配准来为ICP提供较好的点云初始位置。
配准效果如上图，左边为两片兔子的外部轮廓的点云的粗配准，右边为精配准。
1.1 快速点特征直方图FPFH（粗配准）
点快速特征直方图(Fast Point Feature Histogram, FPFH)通俗地来说就是表示三维点的一种特征，类似二维图像中的SIFT、SURF、ORB特征等，都是携带了某种特定的信息。类似二维图像的配准，FPFH也可以用于三维点云之间的配准。参考博客快速点特征直方图FPFH-CSDN博客和博客浅谈FPFH算法实现原理及其在点云配准中的应用-CSDN博客。
上文我们已经说了点云配准的场景，在这里就先介绍PFH原理，因为FPFH原理是PFH原理的简化版。
如下图所示，首先，我们需要定义一个局部坐标系。设Ps点为当前需要计算PFH的点，Pt点为它的邻域内一点。基于法向量于两线之间的向量，定义局部坐标系：
该局部坐标三个轴的计算方法如下：
注意：上式中的"x"表示向量的“x乘”，这个计算得到的向量与参与计算的向量相互垂直， 其实，就获得了以相互垂直的三维坐标系。
ns为Ps点的法向。根据上面的公式，我们就得到了基于点Ps的一个局部坐标系。根据该坐标系，我们计算Pt点与Ps点之间的三元组特征表示：
注意，上式中“."表示向量的”.乘“，可以取查向量的”点乘原理“。
这样，我们就得到了以Ps为源点，对Pt的三元组特征表示。PFH计算在某一点邻域内，所有点对的三元组特征，并根据这些特征，建立直方图表示，进而得到PFH特征表述子。
如上图所示，点Pq为源点，在定义的邻域内有五个点。这六个点的点对数量为C62，即
换句话说，在邻域内共有15条线，对应15组三元组数据。对这15组三元组数据进行三维的直方图统计，以得到一个125维的特征向量，即为PFH。可以看到，PFH除了考虑源点与邻点的关系，还考虑了邻域内邻点之间的关系。可以说，对于邻域的几何特征表述是足够充分的。但是，这样的描述带来了极大的计算开销，其时间复杂度为C(nk^2),其中k为邻域内点的平均值。为了提高效率，我们希望把时间复杂度降为C(nk)，为此FPFH被提出。
FPFH（Fast Point Feature Histogram）
FPFH的核心计算过程和PFS基本一致，都要计算三元组。所不同的是，FPFH不计算邻域内邻点之间的三元组，这样能够显著的提升计算效率。但是，如果只计算源点和邻点之间的三元组，那么就会因减少了对局部邻域的完整描述，降低特征的表示精度。
为了弥补该问题，FPFH将邻点的邻点考虑进来，以扩大统计分析的范围。这样，在对源点附近区域进行特征统计分析时，能够在更大的范围获取三元组信息，以弥补FPFH对PFH特征描述的缺陷。需要注意的是，邻点对于源点的影响权重是不同的，权重应该与邻点与源点的距离成正比。因此，在考虑邻点的邻点时，需要加上基于距离的权重。
如上图所示，当我们在计算源点Pq点三元组的时候，第一步我们只计算源点与邻点的三元组，即五组数据（标记为粉色线），之后，分别计算邻点在其邻域的三元组，并且赋以基于距离的权重，最后将三元组组合，赋值给Pq，使用与PFH同样的方法，计算直方图，得到统计特征。
基于提取的特征点，FPFH结合一个成为SAMPLE ConSENSUS INITIAL ALIGNMENT的方法，即一个贪心搜索，以建立点云之间的初步对应关系，进而实现基于FPFH的点云配准应用。实验证明，FPFH在点云配准任务中具有优越的性能。
1.2点云的精配准——ICP方法
参照博客ICP算法思想与推导详解——什么东西“最近点”，又“迭代”了什么？-CSDN博客，写得很详细，只是少部分地方做了更详细的修改，作为以后的笔记用。
上文我们已经说了点云配准的场景，在这里就直接介绍ICP原理。
首先定义两个点云集合,。点云的配准就是将点云集P旋转平移到Q，使得点云集合P和Q重合，因此可以建立目标函数
其中R,t为所求的旋转矩阵和平移矩阵，求该函数的最小值即可。
（1）求平移矩阵t：
由得
令
代入上式得
再将该式代入得
令
代入上式得
显然，上式中，只有第二项与R有关，其他项与R无关（由于R为正交矩阵，所以，故第三项也与R无关）。
在这里求目标函数的最小值就转换为求 的最大值。
即求
注意，上式得到的结果是一个常数，当几个矩阵乘积为常数时，可以等于矩阵的迹。因此有等式
因为根据迹的公式
故有
注意：上式中，将两个矩阵的乘积看作了一个矩阵，从而上式成立。
令，则有
现在对H进行SVD分解可得：
故有
至此，求得最优的R与t。
2.1 点云粗配准代码
c++代码如下（基于PCL库）：
2.2 点云精配准代码
2.2.1 c++代码
icp的c++代码在这里不作过多的解释，可以看我的另一篇博客笔记4.点云数据的配准_点云叠加配准-CSDN博客,这里也有icp原理，可以结合该文章的原理一起看。
2.2.2 python代码
icp的python代码可以参考博客Python-open3d点云配准_python open3d icp-CSDN博客，在这里我用python重新实现了一次，icp的python代码如下：
添加噪点后的效果如下：