cartographer算法理解

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