Cartographer scan to map的详细过程
1:已知上一时刻的地图m,也是未完成的submap。因为scan to map的过程,就是scan to current submap的过程。
2:submap是栅格地图,这是为了方便匹配。
3:机器人在任意时刻,都能看到map上的所有occupied points。但是,scan不一定能扫描到所有的点。
4:matching的过程,实际上就是把scan的所有扫描数据,映射到当前的submap,去和已有的occupied points匹配,匹配度最高的位姿,就是当前时刻机器人的位姿。
5:由运动模型(odom,IMU),会得到机器人的估计位姿p,在p附近确定一个search window,即机器人的真实位姿可能在这个window的任意位置。遍历window的每一个位置,同时遍历该位置的每一个角度范围例如[-10,10]。
6:假设当前位于search window的某一个位置的某一个角度,即已知p,或者说已知scan到submap原点的变换矩阵T。对scan的每一个beam point(bp)进行映射:T×bp,即可得到该beam point在submap下的坐标,如果当前坐标处是occupied,那么该beam point的匹配值就是1,反之就是0。对所有的beam point求匹配值,所有beam point的匹配值之和就是当前位置的匹配度。但是求得的坐标可能带有小数,因此对应的匹配值用周围栅格的匹配值来近似表示,这一过程应用到了双三次插值算法,这里不做解释。
7:最佳位姿处的scan插入到submap,从而更新submap。