流形上的预积分

博主读研后时间比较忙，很长时间没有更新博客了，之前的一家公司合作的一个和imu相关的项目，博主使用了预积分，由于预积分是不计算重力的，博主也没有专门说明，导致沟通交流不当，项目出现问题，因此今天特来写一个预积分的博客，希望大家喜欢。

预积分的idea

为什么要使用预积分呢？预积分到底干了什么事？预积分相对于普通的积分有什么优势，我想这个事是最重要，也是应该最先说清楚的一个问题。
在两个视觉帧之间，往往有很多imu的采集数据，okvis的论文上怎么做的？我们来看一下公式(
公式来源于论文Keyframe-Based Visual-Inertial SLAM using Nonlinear Optimization ，特别说明一下，okvis那个代码是用的预积分，和论文不一致)：

$$\begin{array}{cc} \begin{split} \dot{\mathbf{p}}_{W}^{WS} &= \mathbf{v}_{W}^{WS} \\ \dot{\mathbf{q}}_{WS} &= \frac{1}{2}\boldsymbol{\Omega}\left(\tilde{\mathbf{\omega}}_{S}^{WS}, \mathbf{w}_g, \mathbf{b}_g \right)\mathbf{q}_{WS} \\ \dot{\mathbf{v}}_{W}^{WS} &= \mathbf{C}_{WS}\left( \tilde{\mathbf{a}}_S^{WS} + \mathbf{w}_a - \mathbf{b}_a \right) + \mathbf{g}_W \\ \dot{\mathbf{b}}_g &= \mathbf{w}_{b_g} \\ \dot{\mathbf{b}}_a &= -\frac{1}{\tau} \mathbf{b}_a + \mathbf{w}_{b_a} \end{split} \end{array}$$
这个模型就是标准的运动学方程，值得注意的是在对速度微分的时候，加速度除了测量值还有重力加速度。对这个模型积分，其实存在着一个问题。由于我们积分都是使用数值积分，可以看到，每次计算的时候，都会引入当前位置到世界坐标的旋转 $\mathbf{C}_{WS}$，这个在积分计算的时候，并不会造成什么影响，但是假如说我们要求导，会造成怎样的后果呢？
请看这个图(
Keyframe-Based Visual-Inertial SLAM using Nonlinear Optimization论文上的图)：

于是呢，你就看到从p时刻数字积分一次，就得到p+1时刻，然后呢，根据公式，这个过程是要乘以p时刻相机位置到世界坐标系旋转的，也就是说，pose耦合到了积分过程中。最终就导致，假如我要对k时刻相机的pose求导，那么就要通过链式法则，对每一个中间过程都求导，正如okvis论文上的公式16，这样带来了极大的计算代价，更悲剧的是，由于状态变了，所以积分的值也会跟着变，那不就每优化一次，我都得重新算一次积分了？这md绝对是个大坑，为了填坑，预积分就应运而生了。

预积分是个什么玩意儿？

参考论文IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation，我们首先应该看公式25，即积分的原始公式：

$$\begin{array}{cc} \begin{split} \mathbf{R}_j &= \mathbf{R}_i \prod_{k=1}^{j-1} Exp\left(\left(\tilde{\boldsymbol{\omega}}_k - \mathbf{b}_k^g - \boldsymbol{\eta}_k^{gd} \right)\Delta t \right) \\ \mathbf{v}_j &= \mathbf{v}_i + \mathbf{g} \Delta t_{ij} + \sum\limits_{k=i}^{j - 1}\mathbf{R}_k\left( \tilde{\mathbf{a}}_k - \mathbf{b}_k^a - \boldsymbol{\eta}_k^{ad} \right)\Delta t \\ \mathbf{p}_j &= \mathbf{p}_i + \sum\limits_{k=i}^{j - 1}\mathbf{v}_k \Delta t + \frac{1}{2}\mathbf{g} t_{ij}^2 + \frac{1}{2}\sum\limits_{k = i}^{j - 1} \mathbf{R}_k \left(\tilde{a}_k - \mathbf{b}_k^a - \boldsymbol{\eta}_{k}^{ad} \right) \Delta t^2 \end{split} \end{array}$$
这个公式就是直接从第i帧经由imu测量值，数值积分到第j帧的公式，我们将它变下形，让他只含有imu测量的合成一项，含有pose的作为另一项，以第i帧为参考标准，对公式进行简单的变形，可以得到：
$$\begin{array}{cc}\begin{split} \mathbf{R}_i^T\mathbf{R}_j &= \prod_{k = i}^{j - 1} Exp \left( \left( \tilde{\omega}_k - \mathbf{b}_k^g - \boldsymbol{\eta}_k^{gd} \right)\Delta t \right) \\ \mathbf{R}^T_i \left(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij} \right) &=\sum\limits_{k=i}^{j-1}\Delta \mathbf{R}_{ik}\left( \tilde{\mathbf{a}}_k -\mathbf{b}_k^a -\boldsymbol{\eta}_k^{ad}\right) \Delta t \\ \mathbf{R}^T_i \left(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i \Delta t_{ij } - \frac{1}{2}\mathbf{g}\Delta t_{ij}^2 \right) &= \sum\limits_{k = i}^{j - 1} \left[ \frac{3}{2}\Delta\mathbf{R}_{ik}\left(\tilde{a}_k - \mathbf{b}_k^a - \boldsymbol{\eta}_{k}^{ad} \right) \Delta t^2\right] \end{split}\end{array}$$
这个变形没啥复杂的，仅仅是做了点加加减减的事，然后乘以一个 $\mathbf{R}_i^T$，但是这个操作却让整个公式变得美丽了，你可以发现，等号右边的所有公式都立足于第i帧，而且仅仅只和测量值有关，和积分中间值没有任何关系，仔细想想，先前的矛盾是不是就解决了？我们求导不用再一步一步链式法则算，测量值合成的那些数据也不会因为pose变换而改变，这样矛盾是不是就解决了？
于是就把这三个量定义为了3个measurement，值得注意的是，这三个量都和重力加速度无关，在等号左边定义的量，是把重力加速度减去之后的值，所以返回去通过第i帧的pose和这些个measurement计算第j帧的pose时，需要把重力加速度加回去！！

还有些杂活儿

其实博文到这里应该就结束了，我想我已经说清楚了整个idea的由来和公式的演变，实际上预积分的核心部分也就这么多，还有些杂活儿就是定义一下误差，计算一下雅克比，唔，积分进行的时候，记得传递一下协方差，其实嘛，还有一件事，就是记住更新bias，这个事是咋搞定的呢？
首先，我们需要一个假设，就是假设合成测量值的时候，他是一个常数，然后最后再重新通过一个子问题更新它们，不过今天我不想说它，有兴趣请自读论文。