lio-sam运行思路整理——图解预积分
再从实际数据的角度看看预积分阶段到底发生了什么
1.首先进入imuHandle，是向ImuOpt和imuQue两个队列里塞数据，此时doneFirst还是false(表示没有进行过优化)，然后直接return掉
2.在收到后端发过来的第一帧可靠的lidar位姿后，进入odomtryHandler，先把ImuOpt队列中早于lidar位姿的imu数据pop掉，保证时间上对齐
3.再进行优化器的初始化，刚得到的lidar位姿T1给他一个置信度成为先验因子，让其约束因子图中的X(0)位姿。实际上就是让X(0)=T1+ΔT，表明X(0)位姿在T1附近游动。同理，速度先验因子设程0，那么V(0)=0+ΔV；初始零偏是parma给出的，B(0)=b+Δb。再用零偏初始化Imu和Opt两个预积分器，初始化阶段就完成了。此时为第一帧Key=1，然后return掉
4.接下来再进入 imuHandle，这是第二次进入了，但doneFirs仍是false，所以此时依旧不发布消息，还是向ImuOpt和imuQue两个队列里塞数据
5.收到第二帧lidar位姿T2，再进入odomtryHandler。此时，isam优化器初始化已经完成，可以开始预积分和优化了。把小于T2时刻的imu数据取出来(实际上这里就是T1和T2之间的了)，在预积分器Opt_中做预积分。把得到的预积分量塞到因子图形成预积分约束，也就是X(0)和X(1)之间的约束。对新的因子图执行优化，得到此时最佳的状态量和零偏估计b2，标志位doneFirst置true
6.在这里终于要对第二个imu数列imuQue进行操作了，把早于T2时刻的imu数据丢掉。用刚得到最优的零偏估计b2重置预积分器Imu_，把剩下的imuQue队列中的数据塞进去。
7. 再进入 imuHandle，这时标志位doneFirst是true了，表明已经有优化好的预积分器Imu_了。然后每塞一个imu数据，就用预积分器预测一下位姿，在imu_incremental话题中发布出去。也就是每有一个imu数据，就有一个位姿发布。
>>>>>>>>>>>>>>>>>>>>>>>>>>>>分割线>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
再从实际数据的角度，看看工作流程, 跑的数据集是campus_small_dataset.bag。这里StartTime重置零偏后，是开始向预积分器Imu_塞数据的时刻， imuT是进入到imuHandler()中开始发布消息的时刻, Endtime对应的是发布的最后一帧数据(也就是说从imuT到Endtime，预积分器Imu_使用的是同一个bias)
这里009，563等都是毫秒ms，从实际数据也可以看出除了lidar位姿T1和T2之间前一段imu_incremental没有发布数据外，之后就可以一直发布数据了，理论上可以做到和imu相同的频率，也就是1s中能发布500帧位姿。但要注意的是imu_incremental是依赖于零偏的更新的，也就是说它两帧之间可靠, 三帧之间不可靠。
Key:2, StartTime:1573567237.009224------->imuT:1573567237.447254------->EndTime:1573567237.563250 dur:0.554026
Key:3, StartTime:1573567237.207258------->imuT:1573567237.567260------->EndTime:1573567237.861261 dur:0.654003
Key:4, StartTime:1573567237.411256------->imuT:1573567237.862245------->EndTime:1573567237.969236 dur:0.557980
Key:5, StartTime:1573567237.609205------->imuT:1573567237.970215------->EndTime:1573567238.343181 dur:0.733976
Key:6, StartTime:1573567237.819264------->imuT:1573567238.347262------->EndTime:1573567238.390182 dur:0.570918
Key:7, StartTime:1573567238.012241------->imuT:1573567238.395182------->EndTime:1573567238.666283 dur:0.654042
Key:8, StartTime:1573567238.215263------->imuT:1573567238.670254------->EndTime:1573567238.816201 dur:0.653989
>>>>>>>>>>>>>>>>>>>>>>>>>>>>更新前>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
从imuT到EndTime才是要发布的imu增量里程计数据(分别用^$%表示),大概示意图如下
T2优化后 #########^^
T3优化后 #######&&&&&
T4优化后 ########%%
这里在后文的imuOdometryHandler()中有疑惑, 此处是一种猜测
此处为什么说他是增量里程计呢, 连成一个整体(比如^^&&&&&%%)不就是imu里程计了吗?原因就在于他们预积分的基础不同,可能结果会有不同.比如imu消息^^^^是以优化过的T2为基础的, 而&&&&是以优化过的T3为基础的. 在优化T3的过程中,由于是全局优化, 可能会对T2有所调整. 也就是说单次的两帧之间的imu消息(比如^^^^, &&&&, %%%%)可靠度比较高, 而把两次结合在一起看做一个总imu里程计的话(比如^^^^&&&&&)可靠度就不高了. 也就是说它两帧之间可靠, 三帧之间不可靠
这是个人的理解，还是有些不太明白。后面再画个详细的流程图思考一下