ICP、NDT、CPD配准算法简单总结

迭代最近点ICP算法：用于将两个或多个点云的坐标系进行对齐

原理：通过迭代的方式，不断优化两个点云之间的匹配误差，直到误差最小化为止

ICP算法的基本流程如下：

1.对两个点云进行初步的粗配准，得到一个初始的变换矩阵。

2.对其中一个点云进行采样，得到一组采样点。

3.在另一个点云中寻找与采样点最近的点，得到一组匹配点。

4.根据匹配点计算出变换矩 阵，将其中一个点云进行变换。

5.重复步骤2-4，直到误差最小化。

主要方法：

1.Point to Point：根据源曲面上的一个点p，在目标曲面上找出对应于p点距离最近的q点。在这个方法中通常运用kd-tree的方法实现就近点搜索。

2.Point to Plane：根据源曲面上的一个点p，在目标曲面上找出对应于p点一个最近的q点。搜索算法是根据源曲面上p点的切平面的法线，确定发现于目标曲面的交点q’。

3.Point to Projection：一种快速的配准方法。

---

正态分布变化NDT算法：基于概率统计学的一种点云配准融合的方法，更适用于不规则形状、噪声和部分遮挡的场景

核心思想：先根据目标点云（Target）来构建多维变量的正态分布，如果变换参数能使得两幅激光点云数据匹配的很好，那么源点云在目标点云中的联合概率密度将会很大。因此，同样可以使用优化的方法求出使得联合概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。

原理：将点云转换为高斯分布的函数形式，通过计算不同高斯分布函数之间的匹配来进行点云配准。

基本流程：具体来说，算法首先将一个点云转换为三维高斯分布函数（即NDT分布），然后通过最小化两个点云之间的NDT分布函数之间的KL散度来进行点云配准。具体来说，NDT算法首先将原始点云数据离散化为一个三维的网格（voxel grid），并对每个网格中的点云进行采样和特征提取。这里采用了点云法向量作为特征，通过计算每个网格中点云的均值和协方差矩阵，可以得到一个高斯分布，即一个GMM。NDT算法会将目标点云和参考点云都转化成GMM的形式，然后计算两个GMM之间的KL散度，用于评估它们之间的相似度。在计算KL散度时，NDT算法采用了一种高效的方法，即通过将每个网格中心点的高斯分布变换到参考点云坐标系下，可以大大减少计算复杂度。最后，NDT算法通过最小化两个GMM之间的KL散度来计算相对位姿变换，得到一个最优的刚体变换矩阵，从而实现点云配准。由于NDT算法采用了高效的计算方法，因此在处理大规模点云数据时具有较高的效率和精度。

---

一致点漂移CPD算法：两个点集看作一个概率密度估计问题，其中一个点集代表混合高斯模型的形心，另一个代表数据。

核心思想：将一个点集看作是由一个概率密度函数生成的样本，然后用高斯混合模型来描述这个概率密度函数，从而实现点云对齐。在CPD算法中，源点云和目标点云之间的对应关系是通过计算两个高斯混合模型之间的最小二乘解来实现的。

基本流程：

1.将源点云和目标点云表示为概率密度函数的样本；

2.通过最大期望(EM)算法估计高斯混合模型的参数；

3.根据高斯混合模型的参数计算两个点集之间的最小二乘解，建立源点云和目标点云之间的对应关系；

4.根据计算出的对应关系，计算两个点云之间的变换矩阵，将源点云变换到目标点云坐标系下;

5.重复步骤2到4，直到满足停止迭代的条件。