环境
- 我正在运行 ROS1 和 ROS2 python。
- 在使用 Anaconda 时可能无法正常工作。
目的
虽然是很简单的ICP算法，但还是想自己写下来，可视化点对点的对应和匹配。因此，我尝试使用 python 为一次扫描重现 ICP 扫描匹配（2D）。该算法是参考“SLAM 简介”创建的。
存储库
git clone https://github.com/KOKIAOKI/simple_icp.git
概述
-
对于点云，以 csv 格式准备任意 x 和 y 坐标。 （点云数据最终会被计算为一个numpy数组，请自行更改。）
-
可以手动给出初始位置。你可以重新开始。点云的代表点，即优化方法的参数，是点云坐标的平均值。
-
您可以使用最速下降法、牛顿法或共轭梯度法（CG 法）将点云相互匹配。我们使用 kd-tree 进行最近邻搜索。
-
输出显示动画（对应的可视化）和轨迹，直到匹配。
-
在轨迹图上，使用收敛时的扫描点云位置，将点云之间的平均距离分布以热图的形式输出。
用法
请参阅自述文件。
关于内容
- ICP错误函数
扫描点云 $\boldsymbol{p_i}(i=1,2...N)$ 和参考点云（地图侧）$\boldsymbol{q_j}(j=1,2...M)$ 的误差函数定义$E$。扫描点云和参考点云之间的对应关系使用kd-tree设置为$\boldsymbol{p_i}$的最近点$\boldsymbol{q_j}$。 $R$ 是旋转矩阵，$ \boldsymbol{t}$ 是平移向量。从语义上讲，它是扫描点云和相关参考点云之间距离的均方。尽量减少这一点。对于最小化，使用下面描述的三种优化方法之一，每次获得局部最优解时进行最近邻搜索。收敛判断是硬编码在类的 init 中的，所以请随便玩玩。
E( \boldsymbol{x}) =\frac{1}{N} \sum_{i=1}^{N}\|(R \boldsymbol{p_i}+ \boldsymbol{t}) - \boldsymbol{q_j}\|^{2}
有时间我会在下面写。
优化方法
《你能理解的优化数学》和《深入学习的数学优化》这几本书很容易理解。
-
最速下降法
-
牛顿法
-
共轭梯度法（CG法）
-
渐变 F
-
黑森州
综上所述
没有人看过代码的内容，所以如果我犯了错误或计算效率低下，我很抱歉。如果您能告诉我如何改进它，我将不胜感激。脚本是日文写的，希望能对你实现优化部分和理解ICP有所帮助。
将来，我想为一次 NDT 扫描做一个简单的可视化。
参考
- SLAM 机器人自定位和地图构建技术介绍，Masahiro Tomono，Ohmsha，2018
- 从基本原理到计算方法理解优化数学，Kenichi Kanaya，共立出版有限公司，2005 年
- 彻底学习从模型到算法的数学优化，Toshiharu Umetani，讲谈社，2020
原创声明：本文系作者授权爱码网发表，未经许可，不得转载;
原文地址：https://www.likecs.com/show-308623309.html