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。

    

转载于:https://www.cnblogs.com/mowangaxing/p/11294124.html