前端
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的和最大。
同理这样利用miss rate。
使用ceres来优化，需要有一个比较好的先验pose，所以用来做pose的refine。
CorrelativeScanMatcher
把平移和旋转离散化后，对所有离散的取值计算scan和submap的匹配分数。然后分数最高的就是对应的变换。
也不可能对所有的可能取值都穷举，所以这里有个平移和角度的窗口，是当前pose为中心。
即使有了搜索窗口，计算量还是太大。于是作者提出把submap分成不同scale的层级。粗的层级可以预先排除一些不可能是解的大区域。
全局图优化
constraint是scan和submap之间的相对pose。可以是submap内部的scan到自己的相对pose，也可以是通过loop closure建立起来的scan到其他submap的相对pose。
优化变量是每个scan到世界的pose，以及每个submap到世界的pose。
误差项是submap到世界乘以世界到scan的变换和直接获得的submap到scan的变换的差异。
原文：https://blog.csdn.net/ziliwangmoe/article/details/84961863
