这篇博客详细介绍了二维ICP（迭代最近点）配准算法的原理和步骤，包括点云归一化、对应点对寻找、变换矩阵计算、点云变换以及损失函数计算。同时提供了Python代码实现，包括关键函数如`GetTheAngleOfTwoPoints`、`GetDistOfTwoPoints`等，并解释了`Math.atan2()`、`Math.pow()`和`Math.sqrt()`等数学函数在算法中的应用。
二维 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中，
最低0.47元/天 解锁文章
扫一扫
2713
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
