点云配准python代码_[Python] 二维 ICP 配准算法 ( 含有笔记、代码、注释 )

二维 ICP 配准算法

① ICP算法简单来说就是对源点云，B点云，不停进行矩阵变换，把它变换到和目标点云，A点云，很接近就停止。

② 算法步骤：

1) 点云归一化。

2) 找到两片点云中的对应点对。

3) 根据对应点对的信息，计算得到这次的变换矩阵。

4) 变换点云。

5) 计算当前的损失。

6) 是否决定继续迭代，如果损失小于设定阈值或者达到最大迭代数停止，否则返回第1步。

1) 点云归一化

① 点云归一化只是我的一个说法，实际并不完全等同传统的归一化操作。

② 点云A和B归一化后，得到点云a和b。

注：归一化后的操作，有一部分是使用归一化后的点云a和b，有一部分是使用原来的点云A和B，这里是不一样的。

③ 这里归一化做的操作是，取点云的中心(有权重时可以取重心，或者其他合理的点也是可以的)。

④ 这里的

和

实际就是点云A和点云B的平均值。

⑤

和

是点云A和点云B中的第i个点，接下来将每个点都减去这个平均值，得到归一化后的点云a和b。

2) 找到对应点对

① 当A和B还没有经过ICP时，没有人能够说出哪两个点是对应的。

② ICP算法的对应点对，通过计算欧式距离，认为欧式距离最近的点为对应点对，也就是ICP的C，closet。

③ 归一化后的点云a和b中，

对应点为

，因此它的数学公式为：

④ 找到对应点对过程中，有一个很重要的步骤去噪，因为数