三维点云配准 -- ICP 算法原理及推导 - 知乎
基于PCL的ICP及其变种算法实现_pcl icp-CSDN博客
ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数为n。然后通过最小二乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小。ICP算法计算简便直观且可使拼接具有较好的精度，但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。各种粗拼接技术可为ICP算法提供较好的初始位置，所以迭代过程中确立正确对应点集以避免迭代陷入局部极值成为各种改进算法的关键，决定了算法的收敛速度与最终的拼接精度
https://blog.csdn.net/qq_29923461/article/details/120785717
矩阵迹（trace）, 行列式（determinate）_矩阵的迹-CSDN博客
如果：AAt=E（E为单位矩阵，At表示“矩阵A的转置矩阵”。）或AtA=E，则n阶实矩阵A称为正交矩阵.
主要性质：
矩阵 的逆、 迹、 秩_逆矩阵的迹-CSDN博客
如何理解矩阵的「秩」？ - 知乎
矩阵的迹是特征值的加和，也即矩阵A的主对角线元素的总和。
有两种方法可以计算
1、利用线性代数的求解SVD
2、非线性优化方式Ax=B
点云配准: 刚性ICP中 Point-to-Point 和 Point-to-Plane 公式推导 ==> 帮助你代码实现 - 知乎
点云刚性配准：point2point 和 point2plane 代码 - 知乎
下面这位博主已经写的很好了，但是在我看的阅读手推的时候，发现了一些细节性的东西，下面是我对整个流程的理解和细节补充。
点云配准经典方法——ICP 原理推导及PCL实现-CSDN博客
前提：
在点云配准的时候如果发现原点云的个数大于目标点云的个数，那么我们的原点云要进行下采样（随机采样）等的相关操作，让原点云的个数小于等于目标点云的个数，不然下面这个公式是不成立的（个人理解）
三维点云配准 -- ICP 算法原理及推导 - 知乎
计算最优旋转 R
计算两个点云的残差和最小
我们可以先计算出各自的点云质心
原始点云的质心
目标点云的质心
我们不考虑平移：loss 函数可以写成;
去中心化，将点云的质心带入
下面这副图是表示去中心化 的东西，下面的没有带入，因为写起来麻烦
用的还是旧的点云质心。
我们先化简
我们带入到 中
所以后面就只有
我们取掉常数-2，可以转换为
那么我们可以通过迹来计算
trace(AB)=trace(BA) ,这里要求AB是一个矩阵，但是A、B不一定是一个方阵
那么
下面我们定义一个矩阵H
对H进行矩阵分解 svd 分解
从上面我们可以看出 U、V、R都是正交矩阵，E是单位矩阵 ，那么 VRU 也是正交矩阵
那么我们可以计算
将M矩阵带入到迹中
根据奇异值非负的性质和正交矩阵的性质（正交矩阵中的元素绝对值不大于 1），容易证得只有当 M 为单位阵时 trace(ΣM) 最大，即：
有两个点云，A（目标点云） B（原点云），然后我们能见B点云一步一步的靠近A点云的过程
0、点云预处理， 滤波等
1、对两个点云的点数比较，有必要就要做采样处理
2、计算两个点云的质心 ， 我们可以设点R=E并且计算 t*=
3、去中心化，并将上面的公式简化成P、Q的形式
4、H=PQ ，将H矩阵进行svd 矩阵分解
5、计算R T
6、计算两个点云的间距，计算loss 函数，并判断时候收敛，继续迭代，返回到2
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output)
主要是将点云的个数对应一致
主要的函数是 computeTransformation 函数
但是里面有几个特别重要的函数
determineCorrespondences函数
determineRequiredBlobData()函数
estimateRigidTransformation函数----非常重要的函数
ConstCloudIterator 函数
该函数被调用了两次，目的是将 source 与 target 内的点根据对应关系对应起来，即通过索引直接对应，source[i] 对应 target[i]。
CC点云匹配
CloudCompare 技巧四 点云匹配_cloudcompare点云配准-CSDN博客
code 测试
简单，不需要对点云进行分割和特征提取
计算速度慢，开销大
只考虑点对点之间的距离，缺少对点云特征的利用
对R t的设定比较重要会影响速度
很容易拿到局部最优的R t
点云的噪点影响很大，所以一般在进行ICP迭代的时候要先进行一次滤波
point-to-plane
点对面的计算考虑到点云的特征和结构，不会陷入局部最优
plane-to-plane
考虑点云结构和特征，计算面到面的距离
Generalized ICP （GICP）
考虑Point-to-Point plane-to-plane plane-to-plane,这样精度就会提高
Normal Iterative Closest Point(NICP)
考虑法向量和局部曲率，更进一步的进行匹配