ICP-Implementation:我对用于扫描网格对齐的迭代最近点算法的实现（具有二次采样和点到平面的改进）-源码

ICP实施 此任务的主要任务是从初始重叠区域开始，尽可能使两个3D几何图形对齐。 一种获得良好结果的广泛使用的算法称为“迭代最近点”（ICP）。 该算法输出由旋转矩阵和平移矢量形成的刚性变换作为输出。 这是为了解决最小化问题而完成的，其中最小化的误差定义如下： 其中p_i是我们要尝试保留的点，而q_i是参考点。 此外，R是我们要查找的旋转矩阵，而t是平移矢量。 当我们想计算最接近的点以匹配两次扫描时，而不是强行强制进行具有O（n ^ 2）复杂度的计算时，可以通过使用将点存储在其中的KD-tree数据结构来大大提高速度基于它们在空间中位置的树。 请注意，K表示点所在的维数，在这种情况下，我们有3D树。 因此，最近邻居搜索的时间复杂度下降为O（log（n））。 ICP的改进 二次抽样 一个非常直接的改进是尝试不使用两次扫描中的所有点。 有两种方法可以正确地对两个扫描进行二次采样，特别是一