lio-sam运行思路整理——预积分模块imuHandler()

imu预积分模块

预积分模块订阅了两个话题imuTopic和lio_sam/mapping/odometry_incremental,发布了一个话题odometry/imu_incremental

• imuTopic实际上是imu/data_raw ,即imu的原始数据. imu_correct是作者修改驱动后发布的数据格式. 该回调函数的作用就是将获取的imu数据送到两个队列中,分别用于优化和预测. 并发布imu对轨迹的预测imu_incremental

• odometry_incremental*订阅了imu的增量数据imu_incremental利用该增量数据, 并结合之前接收的上一时刻的激光里程计的位姿信息, 解算当前时刻的imu里程计信息

这里先简单梳理一下代码运行的流程(只看前三条语句), 实际上这三条是同时运行的, 这里是从数据的角度来说的. 理解为主嘛, 哈哈

1. 先执行imu的回调函数imuHandler(),发现没有odometryHandler()的优化数据. 那就只把数据塞到两个队列里, 不做其他任何处理

2. 接下来就进入了odometryHandler(),得到了一个lidar位姿T1. 先初始化系统, 丢掉比lidar位姿较早的imu数据(imuQueOpt里的), key=1(即此时为第一帧)

3. 然后是执行发布imu_incrementa语句,但此时由于imuHandler()没做其他处理,所以也没有要发布的数据

4. 于是再进入imuHandler(), 注意哦!! 此时仍没有odometryHandler()的优化数据.所以依旧只把数据塞到两个队列里, 不做其他任何处理

5. 好,接下来是重点. 再进入odometryHandler(),得到lidar位姿T2 由于key=1帧我们已经完成初始化了. 所以接下来就要做优化了

   1. 首先要取出两个lidar位姿之间(即两帧之间)的imu数据, 由于我们在初始化时(即key=1帧)已经把T1之前的imu数据给pop出去了,所以只要把T2之前的imu数据取出来就是两帧之间的imu数据了(这里要仔细想一下). 并且我们要取一个就pop一个(这和我们在初始化阶段pop掉T1之前的数据是一回事),这是为了保证在下一帧能顺利取出T2和T3之间的imu数据

   2. 将两帧之间的imu数据送入预积分器(优化用的imuIntegratorOpt_ ,