ICP在pose-graph中的作用

最新推荐文章于 2025-07-16 17:23:49 发布

weixin_33787529

最新推荐文章于 2025-07-16 17:23:49 发布

阅读量277

点赞数

CC 4.0 BY-SA版权

原文链接：http://www.cnblogs.com/wellp/p/8319144.html

本文通过一维示例详细解释了LOAM（Laser Odometry and Mapping）与ICP（Iterative Closest Point）算法在Pose-Graph SLAM中的作用。阐述了如何通过ICP修正LOAM计算出的初始位姿，解决由误差导致的“重影问题”。此外还介绍了Pose-Graph中边的意义及其残差函数的概念。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

从1维的角度进行解释：

P₀ o-----------------------o P₁

假如LOAM计算的两个车辆初始位姿P₀、P₁如上图所示，作为pose-graph的两个顶点。那么二者之间边，也就是两个pose之间的 trans = P₀^-1 * P₁（= 1m），即P₀* trans = P₁ ,一维下也就是 P₀+ 1 = P₁,然而LOAM计算肯定有误差，P₀、P₁不可能跟groundtruth一样，导致变换到全局坐标系下的两个Pose的观测会有“重影问题”，例如，当观测是一个方块时：

P₀ o-----------------------o P₁

口口

原本应该重合的两个方块，却重影成了两个，这时候ICP就起作用了，通过对两个Pose对应的观测（也就是红黑两个方块，传感器是lidar时，两个方块都是点云的形式）进行ICP，若设P₀对应的红色方块为target，P₁对应的黑色方块为source，计算得出ICP配准矩阵E=0.1m,也就是黑色方块需要左移0.1m才会与红色方块重合，利用E来修正更新两个顶点之间的边，也就是trans = P₀^-1 * E * P₁ （= 1 - 0.1 = 0.9 m），从而实现了在pose-graph框架下，ICP对LOAM计算的初始位姿的修正作用。

进一步理解pose-graph中边的意义：边的值应该是其两个vertex之间的真实trans（trans其实是G2O函数库对边的封装格式，边的本质其实是残差函数），但凡是各种方式计算出的pose（IMU，GPS，SLAM）不等于groundtruth，那么 P₀^-1 * P₁就跟两个vertex之间的真实trans有残差，上述的ICP配准矩阵就是起消除这个残差的作用，若pose本来就是groundtruth，那么这时候ICP计算出的E就是单位矩阵。

转载于:https://www.cnblogs.com/wellp/p/8319144.html