从运动方程上来看lio-sam

论文地址
https://github.com/TixiaoShan/LIO-SAM/blob/master/config/doc/paper.pdf

slam中天天说位姿位姿，运动方程运动方程，观测方方程，啥玩意也不明白，今天就来看看抽象的方程在实际代码怎么工作的，思路。主要针对lio-sam运动估计，观测估计后续可能更新。

先来总体看看FJSLAM每个模块干了什么事：

imu预积分：实际上，他是自己去建立了一个因子图，建立因子图干啥呢？它是为了通过激光里程计的位姿，来调整imu的偏置，让当前激光帧后面的imu积分更准，以便获得更好的后续帧的激光里程计初始位姿估计。

那因子图节点有哪些变量呢：位置（3维），姿态（3维），速度（3维），偏置（6维）
那因子图有哪些因子（约束条件呢）：来自当前帧激光里程计位姿和上一帧激光里程计施加imu积分数据的位姿之间的约束；两帧之间imu积分数据的约束；两针imu偏置的约束。

那因子图建立完了，实际上我确实是优化了位姿，这个位姿确实也是激光帧的位姿，但是抱歉，我不是最后的激光位姿，并且我也不为你图优化激光帧做任何贡献，我只想优化我imu积分器的偏置，让我后面的积分更准，然后给你提供一个更准确的imu里程计。

现在从代码上来看有个问题：如果没有雷达里程计，那我imu预积分是没法工作的，抱歉

运动畸变去除：咱先不考虑运动畸变去除问题（在另一篇文章中已经说的很清楚），咱看看他为雷达的位姿做了哪些贡献。首先他检查imu里程计队列有没有数据，有的话，用imu里程计位姿给当前帧赋位姿初始值，然后检查imu原始数据，有的话，再用imu原始数据给当前帧设置位姿初始值。

这两个初值有啥用呢？那太有用了！

这为后续图优化提供了位姿初始值，但是不是这两个初始值都用。用一个，有imu里程计初始位姿估计，就不用imu原始初始位姿估计。为什么？因为里程计是经过imu预积分因子图优化了偏置而得到的位姿，和你imu原始能是一个级别的？通常情况下，只有第一帧激光帧没有imu里程计初始位姿估计，后面就有了，因为，没有激光里程计，imu预积分是不工作的。

现在逻辑就清晰的很了，怎么说?

当我数据流进来的时候怎么办，有cloud、imu，imu预积分部分首先傻眼，因为没有雷达里程计，他不会了。那咋办呢，他先等等，imu，cloud先去畸变，这时候就把imu数据当成当前帧的位姿了，然后在对点云提取特征，最后送过去图优化，那我虽然还没有imu里程计数据，但是我有imu原始位姿估计，所以我能通过因子图的优化，产生一个激光里程计，这样的话，imu预积分可算是找到存在感了，开始进行计算了，计算imu里程计了，那因为我激光里程计是经过偏置优化的积分数据，那后面图优化也知道它好，就用imu里程计作为初始位姿了。这样这四个模块就同步工作了。

讲到这，才解决了我一直以来的问题：imu预积分的图优化和图优化是两个图的两个优化，这只能通过代码了解，论文上是真看不出来。
还有系统开始是怎么工作的，这也只能通过代码来看