点集配准—CPD(Coherent Point Drift) （附python代码）

点集配准-CPD（Coherent Point Drift）算法是一种点云配准算法，用于将两个点云对齐。该算法最初由Myronenko 和 Song 在2009年提出。

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

CPD算法的主要步骤如下：

1. 将源点云和目标点云表示为概率密度函数的样本；
2. 通过最大期望（EM）算法估计高斯混合模型的参数；
3. 根据高斯混合模型的参数计算两个点集之间的最小二乘解，建立源点云和目标点云之间的对应关系；
4. 根据计算出的对应关系，计算两个点云之间的变换矩阵，将源点云变换到目标点云坐标系下；
5. 重复步骤2到4，直到满足停止迭代的条件。

CPD算法的优点是可以处理刚性和非刚性变换，并且对于部分重叠或者噪声较大的点云配准效果也比较好。缺点是迭代次数较多，计算时间较长。

在实际应用中，CPD算法通常与其他算法结合使用，例如ICP算法，以便在配准过程中更快地收敛到最优解。

代码的主要作用是使用CPD算法对两个点云进行配准，并通过可视化工具展示配准结果。代码主要流程如下：

1. 加载原始点云数据，并复制一份作为目标点云；
2. 对目标点云进行旋转平移；
3. 对源点云和目标点云进行下采样；
4. 给源点云和目标点云上色，并可视化展示初始位置；
5. 使用CPD算法对源点云和目标点云进行配准&#