VIO系列一 | IMU预积分与边缘化解析

点击上方“AI算法修炼营”，选择加星标或“置顶”

标题以下，全是干货

VINS-mono 论文解读（IMU预积分+Marg边缘化）

前面，我们已经对接下来的VIO系列主要内容有所介绍（感兴趣的可以关注：视觉与惯性传感器如何融合？），并且已经介绍过了VIO系列一VINS-mono论文解读（IMU预积分+Marg边缘化）第一节的内容：VINS-mono的总体框架。

今天，本文主要介绍以下两点：

• IMU传感器预积分残差推导部分，主要将分为8个部分来介绍相关知识点，如下图1所示。
  

• 基于舒尔补的边缘化，主要分为舒尔补基础知识、边缘化后形成的先验、具体例子三个部分。

接下来，我们将对各个部分详细介绍。

1. IMU预积分残差推导

从上图可以看出对IMU传感器的分析，主要集中于IMU模型的确定，预积分部分的分析，为IMU传感器与视觉传感器融合提供坚实的理论基础。

1.1 IMU模型

测量值：加速度计a^、陀螺仪w^, 加上了bias游走和随机白噪声。

真实值：加速度计a、陀螺仪w。

实际情况下，可以获得测量值a^和w^，需要反推真实值。一般忽略随机游走高斯噪声n。w=w^-bg;     a=qwb(a^-ba)-gw;

1.2 连续时间IMU运动模型，积分IMU中的P位置V速度Q旋转矩阵（两帧之间）

将第k帧和第k+1帧所有的IMU进行积分，可得到第k+1帧的 PVQ，作为视觉估计的初始值。

a和w是IMU测量的加速度和角速度，相对于Body坐标系。

1.3 运动模型的离散积分（前后IMU）

从第 i个IMU时刻到第 i+1个IMU时刻的积分过程。两个相邻时刻k到k+1的位姿是由第k时刻测量值a^,w^计算得出的。

这与Estimator::processIMU（）函数中Ps[j]、Rs[j]、Vs[j]是一致的，代码中j就是此处的i+1

IMU积分出来第 j 时刻数值作为第 j 帧图像初始值。

欧拉法

中值法

1.4 IMU预积分

每次qwbt优化更新后，都要重新进行积分，运算量较大。将积分模型转为预积分模型：

PVQ积分公式中的积分项变为相对于第i时刻的姿态，而不是相对于世界坐标系的姿态：

1.5 预积分量

预积分量只与IMU测量值有关。

1.6 预积分误差

一段时间内IMU构建的预积分量作为测量值，与估计值进行相减。

1.7 预积分离散形式（IMU增量）

中值法：k到k+1时刻位姿由两时刻的测量值a w的平均值来计算。

K时刻：

K+1时刻：

1.8 bias 预积分量（bias发生变化）

因为 i 时刻的 bias 相关的预积分计算是通过迭代一步一步累计递推的，可以算但是太复杂。所以对于预积分量直接在 i 时刻的 bias 附近用一阶泰勒展开来近似，而不用真的去迭代计算。

2. 基于舒尔补的边缘化

基于高斯牛顿的非线性优化理论可知，H*delta_x=b可以写成：

其中，delta_xa和delta_xb分别是希望marg掉的部分和保留部分。

VINS中需要边缘化滑动窗口中的最老帧，目的是希望不再计算这一帧的位姿或者与其相关的路标点，但是希望保留该帧对窗口内其余帧的约束关系。我们基于与移除状态相关的所有边缘化测量值构造一个先验。新的先验项被添加到现有的先验项中。

2.1 舒尔补

2.2 边缘化后形成的先验

xa为需要marg的变量，假设为相机pose,我们更关心如何求解希望保留的xb，而不再求解xa（即marg的变量改为0，左乘时左上是0），这里是要变化为上三角。

即：new_H*delta_xb=new_b;

形成新的信息矩阵new_H具体流程：

。

注意：去掉了x1,但是之前和x1相连的所有量x2 x3  x4  x5 在marg掉x1后变得两两相连。

2.3 具体例子

• 原来的信息矩阵H的构成

上述最小二乘问题，用高斯牛顿求解为：

矩阵乘法公式写成连加：

雅克比J和信息矩阵H的稀疏性：由于每个残差只和某几个状态量有关，因此，雅克比矩阵求导时，无关项的雅克比为 0。

将五个残差的信息矩阵加起来，得到样例最终的信息矩阵 Λ, 可视化如下

• 舒尔补后形成新的信息矩阵new_H,并构造为先验

• 新测量信息和先验构成新的系统

参考：

1.https://blog.csdn.net/qq_41839222/article/details/85793998?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task

2.  https://blog.csdn.net/wangshuailpp/article/details/78461171 

3. https://blog.csdn.net/u014527548/article/details/86632197

4.https://www.cnblogs.com/ilekoaiq/p/8836970.html

5.https://blog.csdn.net/max_hope/article/details/90046770

目标检测系列秘籍一：模型加速之轻量化网络秘籍二：非极大值抑制及回归损失优化秘籍三：多尺度检测秘籍四：数据增强秘籍五：解决样本不均衡问题秘籍六：Anchor-Free
语义分割系列一篇看完就懂的语义分割综述最新实例分割综述：从Mask RCNN 到 BlendMask
面试求职系列决战春招！算法工程师面试问题及资料超详细合集

一起学C++系列内存分区模型、引用、函数重载
竞赛与工程项目分享系列如何让笨重的深度学习模型在移动设备上跑起来基于Pytorch的YOLO目标检测项目工程大合集点云配准领域全面资料、课程、数据集合集分享10万奖金天文数据挖掘竞赛！0.95高分Baseline分享目标检测应用竞赛：铝型材表面瑕疵检测
SLAM系列视觉SLAM前端：视觉里程计和回环检测视觉SLAM后端：后端优化和建图模块视觉SLAM中特征点法开源算法：PTAM、ORB-SLAM视觉SLAM中直接法开源算法：LSD-SLAM、DSO
视觉注意力机制系列Non-local模块与Self-attention之间的关系与区别？视觉注意力机制用于分类网络：SENet、CBAM、SKNetNon-local模块与SENet、CBAM的融合：GCNet、DANetNon-local模块如何改进？来看CCNet、ANN