Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（五）

3. Joint optimization

3.2 IMU preintegration

3.3.2 IMU preintegration

在开始后面的推导之前，我们先整理一下上一篇博客中推导的公式：
$$\begin{gathered} R_i^T{R}_j=\prod _{k=i}^{j-1}Exp((\widetilde{w_k}-{\eta }_w^k-b_w^k)\Delta t)=\Delta R_{ij} \\ {R}_i^T(v_j-v_i-g\Delta t_{ij})=\sum _{k=i}^{j-1}(\Delta R_{ik}(\widetilde{a_k}-{\eta }_a^k-b_a^k)\Delta t)=\Delta v_{ij} \\ {R}_i^T(p_j-p_i-\frac{1}{2}g\Delta t_{ij}^2-v_i\Delta t_{ij})=\sum _{k=i}^{j-1}(\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}({\tilde{a}}_k-{\eta }_a^k-b_a^k)\Delta t^2)=\Delta p_{ij} \end{gathered}$$
非常好，现在计算出来的增量$\Delta =[\Delta R_{ij},\Delta v_{ij},\Delta p_{ij}{]}^T$就不依赖于$i$和$j$时刻的状态了，但是还遗留了一个问题，就是推导出来的增量还依赖于偏置$b_w,b_a$和噪声${\eta }_w,{\eta }_a$。在VINS的原始论文中，做了这样一个假设，即假设偏置在两个关键帧之间是不发生变化的，也即：
$$b_a^i=b_a^{i+1}=...=b_a^{j-1},b_w^i=b_w^{i+1}=...=b_w^{j-1}$$
现在假设偏置$b_a^i$和$b_w^i$已知，则可以将增量$\Delta$进一步化简。为了简化书写，有的地方不再显示的书写叉乘$\wedge$符号，还请注意。我们首先来看旋转矩阵：
$$\Delta R_{ij}=\prod _{k=i}^{j-1}Exp((\widetilde{w_k}-b_w^k-{\eta }_w^k)\Delta t)$$
怎样才能把噪声分离出来呢？我刚开始看的时候觉得，这还不简单吗，高中生都会啊$\exp (a+b)=\exp (a)\exp (b)$，这不就把噪声分离出来了吗！后来仔细想了想，好像有一丝丝不对，对于标量来说上式肯定成立，但是我们可是矩阵的指数函数啊！$\exp (a\wedge +b\wedge )=\exp (a\wedge )\exp (b\wedge )$成立吗？
我们从其表示的物理意义上考虑一下。$\exp (a\wedge )$表示的是旋转轴为向量a方向上的单位向量且旋转角为$||a|{|}_2$的旋转矩阵，$\exp (b\wedge )$对应的是旋转轴为向量$b$方向上的单位向量且旋转角度为$||b|{|}_2$的旋转矩阵，而$\exp (a\wedge +b\wedge )$表示的则是旋转轴为向量$a+b$对应的单位向量且旋转角度为$||a+b|{|}_2$的旋转矩阵，如果上式成立，那岂不是说：
先绕$a$旋转$||a|{|}_2$度，再绕$b$旋转$||b|{|}_2$度，等同于绕轴$a+b$旋转$||a+b|{|}_2$度？？？？再具体点，绕z轴$[0,0,1{]}^T$旋转45度，再绕x轴$[1,0,0{]}^T$旋转45度，等同于绕$[1,0,1{]}^T$旋转$45\times \sqrt{2}$度？？？？
这是什么神仙定律啊！！当然了，事实上这肯定是不对的，于是这可咋把噪声分离出来呢？聪明的数学家们人狠话不多，并直接向你脸上甩了个Baker-Campell-Hausdauff(BCH)公式，且当$a$或者$b$的其中一个模很小的时候，可以得到一阶近似：
$$\{\begin{array}{l}\exp (a\wedge +b\wedge )\approx \exp (a\wedge )\exp (J_r(a)b\wedge ),b为穷小量\\ \exp (a\wedge +b\wedge )\approx \exp (J_l(b)a\wedge )\exp (b\wedge )，a为无穷小量\end{array}$$
虎扑J_r注意了，式中的$J_r$和$J_l$表示的可是$SO(3)$的右雅克比和左雅克比。哭了，摊牌了，这个东西我真的推导不出来，关于这个BCH公式、BCH的无穷小一阶近似以及雅克比的具体形式的推导，感兴趣的可以去参考神书吧[1]。
总之有了这个公式后，我们可以把旋转矩阵中的噪声分离出来了，即：
$$\Delta R_{ij}\approx \prod _{k=i}^{j-1}\exp (({\tilde{w}}_k-b_w^i)\Delta t)\exp (-J_r{\eta }_w^k\Delta t)=\Delta \widetilde{R_{ij}}\exp (-\delta {\varphi }_{ij})$$
接下来，为了求速度增量$v_{ij}$，我们需要把当前计算好的$\Delta R_{ij}$带入到$\Delta v_{ij}$中，即：
$$\Delta v_{ij}\approx \sum _{k=i}^{j-1}(\Delta \widetilde{R_{ik}}\exp (-\delta {\varphi }_{ij})(\widetilde{a_k}-b_a^i-{\eta }_a^k)\Delta t)$$
为了把$\delta {\varphi }_{ij}$分离出来，我们需要对$\exp (-\delta {\varphi }_{ij})$进行泰勒展开和一阶近似，即$exp(-\delta {\varphi }_{ij})\approx I-\delta {\varphi }_{ij}$。然后将其带入上式：
$$\begin{gathered} \Delta v_{ij}\approx \sum _{k=i}^{j-1}(\Delta \widetilde{R_{ik}}(I-\delta {\varphi }_{ij})(\widetilde{a_k}-b_a^i-{\eta }_a^k)\Delta t) \\ =(\sum _{k=i}^{j-1}\Delta \widetilde{R_{ik}}(\widetilde{a_k}-b_a^i))-\delta v_{ij}=\Delta \widetilde{v_{ij}}-\delta v_{ij} \end{gathered}$$
同样可以得到关于位置的表示：
$$\Delta p_{ij}=\Delta \widetilde{p_{ij}}-\delta p_{ij}$$
最后整理一下：
$$\begin{gathered} \Delta \widetilde{R_{ij}}=R_i^T{R}_j\exp (\delta {\varphi }_{ij}) \\ \Delta \widetilde{v_{ij}}=R_i^T(v_j-v_i-g\Delta t_{ij})+\delta v_{ij} \\ \Delta \widetilde{p_{ij}}=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}g\Delta t_{ij}^2)+\delta p_{ij} \end{gathered}$$
看起来是不是很漂亮！！再对这些噪声做一个高斯噪声假设，再来个Negative-log，数学形式不要太完美。

3.3 Noise Propagation