ICP在pose-graph中的作用

Posted 2020-10-21 wellp

从1维的角度进行解释：

                      1m               

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计算的初始位姿的修正作用。