预积分学习总结

今天是5月20日，为了避免像大多数人“只是过了个普通的周三”，昨天就准备了预积分的论文，准备今天认真学习。之前多次翻阅预积分的内容，也都是草草看过，一直没有搞清楚预积分具体在干什么。今天学习了 C. Forster 的"On-Manifold Preintegration for Real-Time Visual-Inertial Odometry"，并找到了邱博在泡泡SLAM上整理的《IMU预计分总结与公式推导》，大体上了解了IMU与积分的基础内容。看懂之后，觉得没有虚度周三，便写个总结。

0. 如果没有预积分？

在看推导的过程中，有困惑像邱博本人请教，邱博让我思考一个问题：如果不用预积分的话，IMU和视觉如何融合？这个问题在我理解了预积分的想法之后，认为：预积分构建了视觉两个关键帧之间的约束，结合视觉的预测，构成残差项，从而优化残差对R, v, p三个量进行优化。如果没有预积分，只能是IMU从头积分到尾，与全部的视觉关键帧，进行Full BA。

1. IMU测量与运动模型

测量模型即：获得角速度与加速度，角速度是以body坐标系计算的，而加速度需要从world系转到body系。二者都需要加上漂移bias和噪声$\eta$。

在两个Key Frame之间，角标用 i 和 j 表示，之间的旋转、速度、平移，分别利用李代数增量、$v_t=v_0+at$、$x_t=x_0+vt+1/2a{t}^2$，得到公式（32）。可以发现如果用（32）这个式子进行计算的话，等号右侧遇到了$R_i,v_i,p_i$ 三个变量，而这三个变量是依赖于状态 i 的，所以如果直接进行积分，每次计算下一个KF的状态时，需要从上一个KF的状态进行积分，造成了计算复杂度较高。

于是对（32）进行变形，定义了旋转、速度、平移的增量如下：

其中第一个等号上方有一个圆点，代表“定义为”，第二个等号后面为实际的计算式。可以看出，计算 $\Delta R_{ij}$ 时只需要IMU的数据进行积分，而不需要依赖之前状态。之后计算 v 和 p 时利用计算出来的$\Delta R_{ij}$也可以避免 v 和 p 的上一个状态。由此我们得到了预积分的式子。
这个式子还有一个问题，式中小括号里面的代表“真实变化量”，而我们拿到的是带有噪声的测量量（${\omega }_k,a_k$ 上方都带有波浪号~）。如何去掉噪声和漂移的影响？

2. 噪声项与噪声的传递

我们对（33）式进行变形，具体推导过程参考论文或邱博的文档。首先假设漂移量 $b^a,b^g$ 是不变的，分别得到旋转R、速度v、平移p，分离测量与噪声的表达式：

三个式子都表示成了：“真值=测量-噪声”的形式。只不过噪声项的形式非常复杂而已。那么我们如何估计噪声？或者说噪声是如何传递的？论文给出了推导，证明旋转R、速度v和平移p的噪声项一阶近似后是符合高斯分布的，由此可以推导出：从上一个时刻的噪声，到这一个时刻的噪声的表达式，即：

上式来自论文的附录A，同时邱博的文档中也给出了详细的推导，即 $A_{j-1},B_{j-1}$ 的表达式。到这为止，我们得到了假设漂移不变时，噪声的传递形式。

3. 漂移量的更新

论文指出，如果我们每次重新去估计当前时刻的漂移值，计算复杂度太高，于是发现下一时刻的漂移可以用当前时刻的漂移，与当前状态的线性化（雅克比）增量进行表示，即：

（44）中左侧表示下一个时刻的漂移，右侧表示用当前时刻的漂移（上方带一个横道，bar）与雅克比（R、v、p对 $b^g,b^a$ 的偏导）进行近似。具体的雅克比形式在论文附录邱博文档也给了出来，如下：

如此一来，我们可以推导 bias 的变化。

4. 残差项

当获得了预积分结果之后，我们得到的是两个关键帧之间的旋转、速度、平移约束，与视觉观测的结果做残差，得到残差项如下：
使残差最小，进行优化，即可完成 IMU 与视觉的融合。

5. 心得体会

耐下心认真看，总是能看懂的。希望自己下一个520有人陪着过。

6. 参考资料

C. Forster. On-Manifold Preintegration for Real-Time Visual-Inertial Odometry.
邱笑晨：《IMU预积分总结与公式推导》
下载链接：https://pan.baidu.com/s/1APWzyhWiRJKDwCgWsLKBiA 提取码: q82u