我已经在 robots.stackexchange 上发布了它，但我没有相关答案。
我目前正在开发一个机器人上的 SLAM 软件，我尝试了扫描匹配算法来解决里程计问题。
我读了这篇文章： 传感器位移估计的基于度量的迭代最近点扫描匹配
我发现它解释得很好，我严格按照文章中给出的公式来实现算法。
你可以在那里看到我在 python 中的实现： ScanMatching.py
我遇到的问题是，在我的测试过程中，找到了正确的旋转，但翻译完全错误。翻译的价值非常高。
你们知道我的代码中可能存在什么问题吗？
否则，我应该在数学堆栈交换上发布我的问题吗？
ICP部分应该是正确的，因为我测试了很多次，但最小二乘法似乎没有给出好的结果。
正如您所注意到的，我使用了许多 bigfloat.BigFloat 值，因为有时最大浮点数不足以包含一些值。