我已将它发布在robotics.stackexchange上，但我没有相关的答案。
我目前正在机器人上开发SLAM软件，我尝试使用扫描匹配算法来解决测距问题。
我读过这篇文章： Metric-Based Iterative Closest Point Scan Matching for Sensor Displacement Estimation
我发现它解释得非常好，我严格按照文章中给出的公式来实现算法。
你可以在python中看到我的实现： ScanMatching.py
我遇到的问题是，在我的测试中，发现了正确的旋转，但翻译完全错误。翻译的价值非常高。
你有没有人知道我的代码中可能出现什么问题？
否则，我应该在数学堆栈交换中发布我的问题吗？
ICP部分应该是正确的，因为我多次测试，但最小二乘最小化似乎没有给出好的结果。
正如您所注意到的，我使用了许多bigfloat.BigFloat值，因为有时最大浮点数不足以包含一些值。
答案 0 :(得分：0)
不知道你是否已经解决了这个问题。
我没有阅读完整的文章，但我注意到它已经过时了。
恕我直言（我不是这里的专家），我会尝试聚合特定的算法，如特征检测和描述，以获得点云，描述符匹配器关联点，束调整以获得旋转相关矩阵。
我自己要尝试sba（http://users.ics.forth.gr/~lourakis/sba/），或者更具体地说是cvsba（http://www.uco.es/investiga/grupos/ava/node/39/）因为我在opencv上。
如果你有足够的cpu / gpu功率，请有机会使用AKAZE特征探测器和描述符。