视觉惯性融合学习笔记一 预积分理论

这个笔记会整理网上和论文里的一些参考资料，也会有一些自己的心得，欢迎交流╰(￣▽￣)╮。

一、概述

视觉惯性导航系统（VINS）相比较传统的视觉SLAM有更大的优势。对于相机快速运动和环境剧烈光照变化导致相机导航失效的情况，IMU能在短时间内提供较精确的定位；对于IMU随着时间的增长会积累大量的导航误差的情况，相机识别过去的位置能帮助减少甚至消除IMU累积的误差。显然，VINS比视觉SLAM有更高的鲁棒性，最近几年也受到较大的关注。

二、坐标系

VINS系统里会用到的坐标系比较多，弄清楚每个坐标系对理解目前开源的VINS非常重要。
首先是利用外参进行坐标变换可能会用到的三大坐标系：

1. 体坐标系，坐标系原点固定在惯性导航单元（IMU）的位置，方向与IMU测量的方向一致。从IMU获取的加速度和角速度都是基于这个坐标系，记为B；
2. 相机坐标系，坐标系原点固定在相机光心位置（一般把凸透镜的中心看作光心），x，y轴与图像的X，Y轴平行，z轴为相机光轴，从光心出发指向前方，记为C；
3. 世界坐标系，通常而言，坐标系原点固定在第一帧图像对应的相机坐标系原点，x，y，z轴的方向与对应的第一帧相机坐标系方向平行，记为W；
   除了上述的几个坐标系外，还有一个惯性坐标系。惯性坐标系z轴方向与重力方向一致。

然后是利用相机模型（这里指针孔相机模型），也可以说是相机的测量模型，进行坐标变换可能会用到的三个空间点坐标：

1. 空间点的相机坐标，空间点在相机坐标系下的3D坐标，记为P=[X,Y,Z]；
2. 空间点的归一化坐标，从空间点出发的光线经过光心，投影在相机前z=1的平面上，就得到了空间点的归一化坐标，记为Pc=[X/Z,Y/Z,1]。如果考虑畸变校正的话，一般是代入归一化坐标计算；
3. 空间点的像素坐标，图像左上角为原点，沿着图像长的方向为u轴，沿着图像宽的方向为v轴，记为 p=[u,v]，u，v一般为整数；
   *物理成像坐标是指从空间点出发经过光心的光线，投影在相机前z=f的平面上（实际上是z=-f平面），以z=f平面与光轴的交点为原点，一般不使用；

这里引用《视觉SLAM十四讲》的图5-1，图中是针孔相机模型的示意图。

三、IMU预积分

这部分更详细的内容可以参考IMU 预积分推导和【泡泡机器人原创专栏】IMU预积分总结与公式推导，也可以直接看论文On-Manifold Preintegration for Real-Time Visual–Inertial Odometry。

IMU通常指由3个加速度计和3个陀螺仪组成的组合单元，能测量对应轴上的加速度和角速度，当然测量值受到测量噪声和bias的影响，长时间运行的系统会无限累积误差。

发展至今，VINS大致分为两类：依靠滤波融合视觉和惯性导航的数据；依靠优化全局位姿来降低误差。后者能获取更高的精度，但是计算量大。

当采用全局优化的方法时，每执行一次全局优化，所有关键帧的位姿都发生改变，这时就需要重新积分IMU测量获取新的状态，再利用估计残差进行非线性优化。这一过程消耗大量的计算资源，而IMU预积分的作用就是避免IMU积分的重复运算。
因为偏置也被优化了，所以预积分还是要更新，不过相比重新计算所有状态量，计算量要小。

3.1 IMU测量模型

首先从IMU能获取其角速度和加速度，测量值是真实值、bias和噪声的和。
陀螺仪测量模型（不考虑地球自转）：
$${\omega }_{WB}^B(t)={\omega }_{WB}^B(t)+b^g(t)+{\eta }^g(t)$$

加速度计测量模型（不考虑地球自转）：
$$a^B(t)=R_{WB}^T(a^W(t)-g^W)+b^a(t)+{\eta }^a(t)$$

其中${\omega }_{WB}^B(t)$和$a^B(t)$分别表示角速度和加速度的测量值，${\omega }_{WB}^B(t)$和$a^B(t)$分别表示角速度和加速度的真实值，$b^g(t)$和$b^a(t)$分别表示陀螺仪和加速度计的偏置，$\eta$为噪声。

3.2 IMU预积分状态量

t时刻的三个状态量：B系到W系旋转$R_{WB}(t)$、W系下IMU速度$v^W(t)$和IMU位置$p^W(t)$。假设在之后的$\Delta t$时间内，IMU测量值保持不变，由欧拉积分能获取三个状态量在$t+\Delta t$时刻的值。考虑离散情况，令i表示t时刻，j表示$t+N\Delta t$时刻，则用IMU测量值表示的j时刻的状态量为：
$$R_j=R_i\prod _{k=i}^{j-1}Exp[({\omega }_k-b_k^g-{\eta }_k^{gd})\Delta t]$$$$v_j=v_i+\sum _{k=i}^{j-1}[R_k(a_k-b_k^a-{\eta }_k^{ad})]\Delta t+g\Delta t_{ij}$$$$p_j=p_i+\sum _{k=i}^{j-1}{v}_k\Delta t+\frac{1}{2}\sum _{k=i}^{j-1}g\Delta t^2+\frac{1}{2}\sum _{k=i}^{j-1}{R}_k(a_k-b_k^a-{\eta }_k^{ad})\Delta t^2$$
其中$Exp()$表示$Exp:so(3)\to SO(3)$，$so(3)$是李代数，就是一个3维向量；$SO(3)$是李群，就是3*3的旋转矩阵；$Exp()$表示从李代数换算到李群。

通过上述几个公式可以定义出IMU预积分状态量计算值（下面第一个等于后）和测量值（下面第二个等于后）两组值。
$$\Delta R_{ij}=R_i^T{R}_j=\prod _{k=i}^{j-1}Exp[({\omega }_k-b_k^g-{\eta }_k^{gd})\Delta t]$$$$\Delta v_{ij}=R_i^T(v_j-v_i-g\Delta t_{ij})=\sum _{k=i}^{j-1}[\Delta R_{ik}(a_k-b_k^a-{\eta }_k^{ad})]\Delta t$$$$\Delta p_{ij}=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}\sum _{k=i}^{j-1}g\Delta t^2)=\sum _{k=i}^{j-1}[\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}(a_k-b_k^a-{\eta }_k^{ad})\Delta t^2]$$
为什么这里我要写成计算值和测量值？因为计算值是通过非IMU测量数据得到的，比如纯视觉SfM计算的IMU位姿；测量值顾名思义，就是用IMU测量数据得到的；这两个值的差作为目标函数

3.3 IMU预积分测量噪声

对预积分状态量测量值分离噪声，方便后续作为非线性优化迭代时的状态量增量。
假设ij时间段内的bias不变，推导过程这里省略，直接给出分离出的预积分测量噪声的递推形式结果：
$$\delta {\varphi }_{ij}=\Delta R_{j-1j}\delta {\varphi }_{ij-1}+J_r^{j-1}{\eta }_{j-1}^{gd}\Delta t$$$$\delta v_{ij}=\delta v_{ij-1}+\Delta R_{ij-1}{\eta }_{j-1}^{ad}\Delta t-\Delta R_{ij-1}(a_{j-1}-b_{j-1}^a{)}^{\wedge }\delta {\varphi }_{ij-1}\Delta t$$$$\delta p_{ij}=\delta p_{ij-1}+\delta v_{ij-1}\Delta t+\frac{1}{2}\Delta R_{ij-1}{\eta }_{j-1}^{ad}\Delta t^2-\frac{1}{2}\Delta R_{ij-1}(a_{j-1}-b_{j-1}^a{)}^{\wedge }\delta {\varphi }_{ij-1}\Delta t^2$$
IMU预积分理想值、测量值、测量噪声之间的关系为：
$$\Delta R_{ij}=\Delta R_{ij}Exp(-\delta {\varphi }_{ij})$$$$\Delta v_{ij}=\Delta v_{ij}-\delta v_{ij}$$$$\Delta p_{ij}=\Delta p_{ij}-\delta p_{ij}$$
其中$\delta {\varphi }_{ij}$、$\delta v_{ij}$、$\delta p_{ij}$分别表示旋转矩阵，速度，位置在ij时间内预积分状态量的测量噪声；$J_r^{j-1}$是右乘雅克比矩阵，矩阵解释参考《视觉SLAM十四讲》，具体推导参考【泡泡机器人原创专栏】IMU预积分总结与公式推导（二）；^表示计算反对称矩阵；$\Delta R_{ij},\Delta v_{ij},\Delta p_{ij}$表示IMU预积分理想值；$\Delta R_{ij}，\Delta v_{ij}，\Delta p_{ij}$表示预积分测量值。
把这些预积分测量噪声合成一个向量，并记为${\eta }_{ij}=[\delta {\varphi }_{ij}\delta v_{ij}\delta p_{ij}]$，将IMU测量噪声合成一个向量，并记为${\eta }_j^d=[{\eta }_j^{gd}{\eta }_j^{ad}]$，则上述结果可以用矩阵表示，系数简单用字母代替
$${\eta }_{ij}=A_{j-1}{\eta }_{ij-1}+B_{j-1}{\eta }_{j-1}^d$$
于是预积分测量噪声的协方差矩阵${\Sigma }_{ij}$也可以通过递推得到
$${\Sigma }_{ij}=A_{j-1}{\Sigma }_{ij-1}{A}_{j-1}^T+B_{j-1}{\Sigma }_{\eta }{B}_{j-1}^T$$

3.4 IMU预积分状态量修正（考虑bias变化）

之前的推导基于一个假设：在ij时间段内bias不变。但实际情况是bias会不断累积，因此在VINS进行优化时也需要优化bias。bias的改变也会带来IMU状态量的改变，为了避免重新积分IMU状态量，就需要考虑IMU预积分当bias改变时的修正。为简化运算，采用关于bias的预积分的一阶近似更新。
$$\Delta R_{ij}({\hat{b}}_i^g)\approx \Delta R_{ij}({\bar{b}}_i^g)Exp(\frac{\partial \Delta {\bar{R}}_{ij}}{\partial {\bar{b}}^g}\delta b_i^g)$$$$\Delta v_{ij}({\hat{b}}_i^g,{\hat{b}}_i^a)\approx \Delta v_{ij}({\bar{b}}_i^g,{\bar{b}}_i^a)+\frac{\partial \Delta {\bar{v}}_{ij}}{\partial {\bar{b}}^g}\delta b_i^g+\frac{\partial \Delta {\bar{v}}_{ij}}{\partial {\bar{b}}^a}\delta b_i^a$$$$\Delta p_{ij}({\hat{b}}_i^g,{\hat{b}}_i^a)\approx \Delta p_{ij}({\bar{b}}_i^g,{\bar{b}}_i^a)+\frac{\partial \Delta {\bar{p}}_{ij}}{\partial {\bar{b}}^g}\delta b_i^g+\frac{\partial \Delta {\bar{p}}_{ij}}{\partial {\bar{b}}^a}\delta b_i^a$$
其中${\hat{b}}_i^g$和${\hat{b}}_i^a$表示bias更新后的估计值，${\bar{b}}_i^g$和${\bar{b}}_i^a$表示bias更新前的估计值，$\delta b_i^g$和$\delta b_i^a$表示bias更新量。
上式中的IMU预积分状态量关于bias的偏导参考【泡泡机器人原创专栏】IMU预积分总结与公式推导（三）。

3.5 优化

到这里已经定义了IMU预积分状态量，噪声，并且考虑了bias变化后带来的状态量修正。之后需要考虑的是如何利用ij时间段内的预积分优化时间段内的状态。

在ij时间段内待优化的状态量包括：$R_i,v_i,p_i,R_j,v_j,p_j,\delta b_i^g,\delta b_i^a$。考虑$\delta b_i^g,\delta b_i^a$的原因是需要用bias更新量修正预积分状态量。

待优化的目标函数是残差的加权平方和。残差是在3.2中定义的两个预积分状态量的差别，其中IMU预积分状态量计算值是通过视觉或者其他非IMU的方式计算的预积分值，预积分状态量测量值则是通过IMU测量值计算的预积分值；计算加权平方和的原因是考虑到状态量单位的影响，权重为预积分测量噪声协方差的逆。
残差定义：
$$r_{\Delta R_{ij}}=log[(\Delta R_{ij}({\hat{b}}_i^g){)}^T\Delta R_{ij}]$$$$r_{\Delta v_{ij}}=\Delta v_{ij}-\Delta v_{ij}({\hat{b}}_i^g,{\hat{b}}_i^a)$$$$r_{\Delta p_{ij}}=\Delta p_{ij}-\Delta p_{ij}({\hat{b}}_i^g,{\hat{b}}_i^a)$$

优化方法采用GN或者LM，具体参考《视觉SLAM十四讲》。每个状态量的更新方法如下：
$$R_i\leftarrow R_i\cdot Exp(\delta {\varphi }_i),v_i\leftarrow v_i+\delta v_i,p_i\leftarrow p_i+\delta p_i$$$$R_j\leftarrow R_j\cdot Exp(\delta {\varphi }_j),v_j\leftarrow v_j+\delta v_j,p_j\leftarrow p_j+\delta p_j$$$$\delta b_i^g\leftarrow \delta b_i^g+\delta b_i^g,\delta b_i^a\leftarrow \delta b_i^a+\delta b_i^a$$
要计算状态量更新中的更新量，就需要计算目标函数对状态量的雅克比矩阵。矩阵计算结果参考【泡泡机器人原创专栏】IMU预积分总结与公式推导（四）