- 前端
- IMU的数据:通过旋转角度和重力的滤波得到重力方向。
- 基于前一时刻的速度,yaw角。加上旋转角度的滤波得到的pitch和roll角。通过一次角速度和加速度的prediction得到当前时刻的pose的估计。
- 使用和loop closure相同的匹配方法得到一个pose。
- imu预测的pose和loop closure匹配方法得到的pose融合后得到一个coarse的pose。
- 在这个pose附近和submap进行优化匹配,得到refined的pose。
- 如果这个scan能带来足够的信息,就加入到submap中。
- 如果submap的scan超出上限,就新建submap,并且这个submap加入到pose graph中。
- 后端
- 但前位附近的scan都和新加入的这个scan做匹配,也就是得到历史scan在新的submap坐标系的表示。
- 匹配上的scan和submap作为全局图优化新的优化constrain。
- Submap
- 离散化后的空间,可以有2d格子也可以时3d格子。
- 每个格子有两个属性:hit和miss。
- 每有一个激光点落在里面hit+1
- hit的那个点到激光远点之间的格子的miss+1
- CeresScanMatcher
- scan的图案应该是hit和miss图的子集。
- 找到一个变换T能够让scan的点所在的格子的hit rate的和
cartographer算法理解
![](https://img-home.csdnimg.cn/images/20240711042549.png)