从1维的角度进行解释:

1m

P   o-----------------------o   P

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

P   o-----------------------o   P

口口

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

05-11 17:21