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

2020 年 4 月 20 日 计算机视觉life

点击上方“计算机视觉life”，选择“星标”

快速获得最新干货

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

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

论文《VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator》地址：https://arxiv.org/pdf/1708.03852.pdf

系列第一节主要目录如下图所示，本文主要介绍总体框架部分。

接下来，我们将对总体框架部分详细介绍。

0. VINS论文总体框架

从上图可以看出，系统主要包括五个部分：数据预处理、初始化、后端非线性优化、闭环检测、位姿图优化。

0.1 数据预处理

视觉传感器：
1）提取Harris角点，KLT金字塔光流跟踪相邻帧；
2）2 维特征点先矫正为不失真的，然后在通过外点剔除后投影到一个单位球面上；
3）去除异常点：先进行F矩阵测试，通过RANSAC去除异常点；
4）关键帧选取：1、当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧（因为视差可以根据平移和旋转共同得到，而纯旋转则导致不能三角化成功，所以这一步需要IMU预积分进行补偿）2、当前帧跟踪到的特征点数量小于阈值视为关键帧；

IMU传感器：
1）两帧k和k+1之间进行位置、速度、姿态（PVQ）预测；2）避免每次姿态优化调整后重复IMU传播，采用预积分算法，计算预积分误差的雅克比矩阵和协方差项。

0.2 初始化

采用松耦合的传感器融合方法得到初始值。首先用SFM进行纯视觉估计滑动窗内所有帧的位姿以及路标点逆深度，然后与IMU预积分对齐，继而恢复对齐尺度s，重力g，imu速度v，和陀螺仪偏置bg。

VINS初始化过程中忽视掉了加速度计的bias，因为加速度计与重力耦合，并且重力向量很大，初始化过程动态过程很短，幅度又不大，加速度计偏置很难观测到。

A. 滑动窗口(Sliding Window)纯视觉SfM

1、选择一个滑动窗，在最后一帧与滑动窗之前帧寻找帧：跟踪到的点数目大于30个的并且视差超过20的，找到后用5点法本质矩阵初始化恢复出R和t。否则，滑动窗内保留最新图像帧，继续等待下一帧。

2、随意设置一个尺度因子，三角化这两帧观测到的所有路标点。再用PnP算法估计滑动窗内所有其余帧的位姿。滑动窗内全局BA重投影误差优化所有帧位姿。

3、假设IMU-Camera外参已知，乘上视觉得到的位姿，转换到IMU坐标系下。

B. 视觉惯性校准（IMU预积分与视觉结构对齐）

1、陀螺仪零偏bg标定

旋转两种方式：陀螺仪测量值和视觉观测值，二者的误差其实就是陀螺仪偏置bg。

目标函数：visual给出的相邻帧间的旋转应等于IMU预积分的旋转值Q之间的差。

考虑到了陀螺仪bias噪声，我们可以得到陀螺仪偏置bias的初始校准,需将陀螺仪偏置bg代入到IMU预积分重新计算预积分。

2、速度v、重力g和尺度初始化s

优化变量：速度、重力向量和尺度

目标函数：相邻两帧IMU预积分增量与预测值之间平移、速度（P、V）的差。通过HX=B 利用cholesky分解获得。具体推导过程如下：

3、重力矢量修正

重力向量的大小是已知的，加入了模长限制，这导致三维重力向量只剩2个自由度。

主要做的是优化方向，一个二维向量。在其切线空间上用两个变量重新参数化重力，采用球面坐标进行参数化。

0.3 紧耦合后端非线性优化（IMU约束+视觉约束+闭环约束）

初始化后，采用基于滑动窗口的紧耦合单目VIO进行状态估计。

A 公式

需要优化的量：IMU状态（PVQ、加速度bias、陀螺仪bias）、IMU到Camera的外参、m+1个路标点逆深度。

第一个式子是滑动窗口内所有状态量，n是帧数，m是滑动窗口内特征点总数。特征点逆深度为了满足高斯系统。

第二个式子xk是在第k帧图像捕获到的IMU状态，包括位置，速度，旋转（PVQ）和加速度偏置，陀螺仪偏置。

第三个式子是相机外参。

xk只与IMU项和Marg有关；特征点深度也只与camera和Marg有关；

视觉惯性BA，最小化边缘化的先验信息和IMU、视觉测量残差之和。

BA优化模型分为三部分：

1、Marg边缘化残差部分（滑动窗口中去掉位姿和特征点约束）

2、IMU残差部分（滑动窗口中相邻帧间的IMU产生）

3、视觉代价误差函数部分（滑动窗口中特征点在相机下视觉重投影残差）

B IMU残差

残差：状态量传播预测与IMU预积分的残差

优化变量：IMU时刻下的p位置，v速度，Q旋转，两个偏置ba,bw

C 视觉残差

与传统的针孔相机模型不同，这里用的是单位半球体的相机观测残差。是一个鱼眼相机。

在相机的归一化平面上比较残差，再将视觉残差投影到单位球面的正切平面上。由于视觉残差的自由度是2，所以我们将残差向量投影到切平面上。

在第i帧第一次观测到第l个路标点，在第j帧中对该路标点进行观测的残差为：

第一个式子就是残差的表达式，第二个式子是鱼眼相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据，b1和b2是此单位向量的切平面上的一组基。第三个式子是重投影估计模型。其实VINS代码中也可以使用普通的针孔相机模型。

D 边缘化 Marginalization

为了防止pose和特征的个数的复杂度随着时间不断增长，引入边缘化，在移除位姿时将关联的约束转化为先验放入优化问题中。

为了限制基于优化的VIO计算复杂度，引入边缘化。有选择地从滑动窗口中将IMU状态xK和特征λ1边缘化，同时将对应于边缘状态的测量值转换为先验。

分为两种情况：

1、一种是倒数第二帧如果是关键帧的话，将最旧的pose移出Sliding Window，将最旧帧关联的视觉和惯性数据边缘化掉。把第一个老关键帧及其测量值被边缘化；Margin_Old作为先验值。

2、如果倒数第二帧不是关键帧的话，那么就只剔除倒数第二帧的视觉观测，而不剔除它的IMU约束。原因是边缘化保证关键帧之间有足够视差而能够三角化足够多的地图点。并且保证了IMU预积分的连贯性。

为了保持系统的稀疏性，我们不会边缘化非关键帧的所有测量值。

E 相机速率下的状态估计--只有运动的VIO

采用了一种轻量级的纯运动视觉惯性BA，以提升状态估计速率到相机速率(30Hz)。

代价函数不变。

1、只对固定数量的最新IMU状态的姿态pose和速度v进行了优化，而不是对滑动窗口中的所有状态进行优化

2、将特征深度、外部参数、偏置和旧的IMU状态这些不希望优化的状态作为常量来处理。

与在最先进的嵌入式计算机上可能导致超过50ms的完全紧耦合单目VIO不同，这种纯运动的视觉惯性BA只需大约5ms来计算。

0.4 重定位

尽管滑动窗和边缘化减小了计算复杂度，但是仍旧引进了系统的累计漂移误差。具体来说，就是全局三维位置（xyz）和围绕重力方向的旋转（yaw）。作者采用紧耦合重定位模块与单目VIO进行组合实现漂移误差的消除。

目的：局部滑动窗口移动并与过去的位姿对齐。

vins的重定位模块主要包含回环检测,回环候选帧之间的特征匹配,紧耦合重定位三个部分。

上图展示了重定位步骤。

1中VIO启动时刻只进行位姿估计（蓝色部分），过去状态一直被记录（绿色部分）。

2中如果最新帧中回环被检测到，呈现红色虚线连接，表示启动重定位3。

4中多个特征的多个观测直接用于重定位，从而提高了定位的精度和状态估计的平滑性。

5-7是位姿优化。

A、回环检测（只对关键帧）

1、采用DBoW2词袋位置识别方法进行回环检测。经过时间空间一致性检验后，DBoW2返回回环检测候选帧。

2、除了用于单目VIO的角点特征外，还添加了500个角点并使用BRIEF描述子，描述子用作视觉词袋在数据库里进行搜索。这些额外的角点能用来实现更好的回环检测。

3、VINS只保留所有用于特征检索的BRIEF描述子，丢弃原始图像以减小内存。

4、单目VIO可以观测到滚动和俯仰角，VINS并不需要依赖旋转不变性。

B、回环候选帧之间的特征匹配

1、检测到回环时，通过BRIEF描述子匹配找到对应关系。但是直接的描述子匹配会导致很多外点。

2、本文提出两步几何剔除法：

1）2D-2D：使用RANSAC进行F矩阵测试，

2）3D-2D：使用RANSAC进行PnP，基于已知的滑动窗特征点的3D位置，和回路闭合候选处图像的2D观测（像素坐标）。

当内点超过一定阈值时，我们将该候选帧视为正确的循环检测并执行重定位。

C、紧耦合重定位

1、重定位过程使单目VIO维持的当前滑动窗口与过去的位姿图对齐。

2、将所有回环帧的位姿作为常量，利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口。

和之前VIO优化模型不同的是，增加了回环项，从位姿图获得回环帧的姿态被视为常数。

在重定位之后（重定位只是基于检测到的回环处暂时对滑动窗里的位姿进行重新优化）

下一步要对过去位姿和闭合回路图像帧的全局优化。

0.5 全局位姿图优化

这一步是为了确保基于重定位结果对过去的位姿进行全局优化。

由于视觉-惯性使得横滚角和俯仰角完全可以观测，因此只有（XYZ和yaw航向）4个自由度存在累积漂移。接下来只进行4-DOF的位姿图优化。

A、位姿图中添加关键帧

当一个关键帧被滑动窗口中边缘化掉后，它会被添加到位姿图中。该关键帧会作为位姿图中一个定点，通过下面两类边与其他顶点相连接：

1、顺序边（Sequential Edge）：关键帧将建立与之前关键帧的几个顺序边，一个顺序边表示局部滑动窗口中两个关键帧之间的相对转换，它的值直接从VIO中获取。令最新边缘化掉的关键帧为i，它的一个以前的关键帧为j，顺序边只包含相对位置和相对航向。

2、回路闭合边（Loop Closure Edge）：如果最新的边缘化掉的关键帧存在回路连接，它可以通过位姿图中的回路比河边和回路闭合帧相连接。回环边的值由重定位结果得出。

B、4自由度位姿图优化

关键帧i和j之间的残差最小化表示为；

通过最小化以下代价函数，对顺序边和回环边的整个图进行优化：

S是所有顺序边的集合，L是回环边的集合。尽管紧耦合的重定位已经有助于消除错误的回环，但我们添加了另一个Huber范数 ρ(·)，以进一步减少任何可能的错误回环的影响。相反，我们不对顺序边使用任何鲁棒范数，因为这些边是从VIO中提取出来的，VIO已经包含了足够多的外点排除机制。

位姿图优化和重定位(VII-C)异步运行在两个独立的线程中。以便在需要重定位时，能立即使用最优化的位姿图。同样，即使当前的位姿图优化尚未完成，仍然可以使用现有的位姿图配置进行重新定位。这一过程如上图(b)所示。

C、位姿图管理

随着行程距离的增加，位姿图的大小可能会无限增长，从而限制了长时间系统的实时性。为此，我们实行了一个下采样过程：将位姿图数据库保持在有限的大小。所有具有回环约束的关键帧都将被保留，而其他与相邻帧过近或方向非常相似的关键帧可能会被删除。关键帧被移除的概率和其相邻帧的空间密度成正比。

接下来，系列一的文章会详细介绍 IMU预积分和边缘化的知识，敬请期待。更多精彩内容，可以点击阅读原文！

参考：

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

交流群

欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~