VINS-Mono

非极大值抑制

NMS算法，思想是搜索局部最大值，抑制非极大值。
输入：目标边界框及其对应的置信度的分列表，设定阈值，阈值用来删除重叠较大的边界框。
IoU：两个边界框的交集部分除以它们的并集。
非极大值抑制的流程：

1. 根据置信度得分进行排序
2. 选择置信度最高的边界框添加到最终输出列表中，将其从边界框列表中删除
3. 计算所有边界框的面积
4. 计算置信度最高的边界框与其它候选框的IoU
5. 删除IoU大于阈值的边界框
6. 重复上述过程，直至边界框列表为空。

光流法

光流：空间运动物体在观察成像平面上的像素运动的瞬时速度。
光流法：利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系，从而计算出相邻帧之间物体的运动信息的一种方法。
光流矢量：二维图像平面特定坐标点上的灰度瞬时变化率。

光流法基本原理

基本假设条件

1. 亮度恒定不变
2. 时间连续或运动是“小运动”

基本约束方程

考虑一个像素I(x,y,t)在第一帧的光强度（t代表其所在的时间维度）。它移动了(dx,dy)的距离到下一帧，用了dt时间。因为是同一个像素点，假设认为该像素在运动前后的光强度不变，即
$$I(x,y,t)=I(x+dx,y+dy,t+dt)$$
泰勒展开得
$$I(x,y,t)=I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt+\epsilon$$
其中$\epsilon$代表二阶无穷小项，可忽略不计。前两式合并后同除dt, 可得：
$$\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}+\frac{\partial I}{\partial t}\frac{dt}{dt}=0$$
设u,v分别为光流沿X轴和Y轴得速度矢量，得：
$$u=\frac{dx}{dt},v=\frac{dy}{dt}$$
令$I_x=\frac{\partial I}{\partial x}$,$I_y=\frac{\partial I}{\partial y}$, $I_t=\frac{\partial I}{\partial t}$分别表示图像中像素点得灰度沿X,Y,T方向的偏导数。
综上，可得出
$$I_{x}u+I_{y}v+I_t=0$$
(u, v)为光流矢量。

引入另外的约束条件才能求得u和v的确切值。分为四种方法：基于梯度（微分）的方法、基于匹配的方法、基于能量（频率）的方法、基于相位的方法和神经动力学方法。

单目加IMU的挑战：

1. 需要给足够的激励来获取观测尺度。VINS estimator不能在静止环境下初始化，需要在未知运动状态下开始。系统时高度非线性的，初始化具有很大挑战。
2. 两个传感器，对camera-IMU的外参标定非常苛刻。
3. 为获得long-term drift在一个处理窗口内可接受，需要一个完整的系统，该系统包含visual-inertial odometry, loop detection, relocalization and global optimization部分。

IMU预积分

为什么要预积分

• slam系统中为了减小优化求解器的负担，采用了关键帧策略，IMU的速率显然要快于关键帧的插入，它们之间的关系可用下图表示。
  
  紧耦合的方式就是把imu和图像的信息共同来估计状态量，预积分就是协调两者之间的关系，通过重新参数化，把关键帧之间的IMU测量值积分成相对运动的约束，避免了因为初始条件变化造成的重复积分，在下一个关键帧到来之前先对IMU数据积分。

本论文的解决思路：
4. on-the-fly estimator initialization。该初始化模块也用于失败后的系统恢复。
5. tightly-coupled sliding window non-linear optimization.
6. 单目VIO模块不仅提供精确的局部位姿、速度和方向估计，也实现了camera-IMU的外参标定以及在线的IMU的biases correction.
7. 回环检测DBoW2.
8. 重定位：特征级tightly-coupled实现重定位。用最小计算量实现了鲁棒和精确的重定位。
9. 全局一致性：geometrically verfied loops加入到pose graph，由单目VIO观测到的roll和pitch角度，4DOF的pose graph保证了全局一致性。

本文贡献点

• 鲁棒的初始化步骤，可以对系统从位置的初始化状态加速。
• 紧耦合，基于优化的单目视觉惯性里程计，相机和IMU的外参标定以及IMU的偏置估计。
• 在线的回环检测和紧耦合重定位。
• 实时无人机导航，大尺度定位和手机AR应用。
• 开源PC版本，ROS集成，iOS版本在iPhone6s上跑。

相关工作

1. 单目版本，PTAM
2. visual-inertial状态估计

• 耦合方法
  • 最简单的方法：loosely-coupled sensor fusion
    • IMU被看作独立的模块，辅助仅用视觉的位姿估计，SFM。
    • 融合通过EKF实现，IMU用于状态传播，视觉位姿用于更新。
  • tightly-coupled, 相机和IMU用于状态传播，视觉位姿用于更新。
    • EKF
    • graph optimization
• 优化方法
  • 基于EKF的VIO方法MSCKF。
    • 在状态向量中保存一些之前的相机位姿，采用多个相机视角的特征来形成multiconstraint update.
    • SR-ISWF, is an extension of MSCKF
      • 采用square-root形式获得单精度的描述，防止差的数据。采用逆深度滤波器，迭代重现性，等价于基于优化的算法。
  • Batch graph optimization或bundle adjustment保持和优化所有的测量数据，来获得优化的状态估计。为保持计算时间恒定， optimize over a bounded-size sliding window of recent states by marginalizing out past states and measurements。

3. visual measurement

• Direct approaches, minimize photometric error。
  • 需要好的初始化猜测
  • 由于直接在像素级操作，更好扩展到稠密地图。
• indirect approach, minimize geometric displacement
  • 耗费额外的计算资源来提取和匹配特征。
  • 在真实数据中更常见，成熟和鲁棒

4. IMU, 远高于相机的频率。
   • 在EKF方法中，直接用IMU进行状态传播。
   • 在graph优化中，高效的IMU预集成开发，防止重复的IMU re-integration。
   • IMU的偏置被忽略了。
   • 预集成方法，通过增加后置的IMU bia correction进一步改善。
5. 精确的初始化，对于有单目VINS的加速非常重要。
   • 线性估计初始化方法，受相对旋转的影响。相对旋转来自short-term IMU的preintegration。该方法没有对陀螺仪的偏置建模，也没有传感器噪声在原始投影上的建模。在真实应用中，视觉特征离传感器较远时，初始化不可靠。
   • 引入了单目VI的初始化闭合的解
   • 加入陀螺仪偏置标定的扩展闭合形式解，对惯性单元的积分不确定性没有建模，因为它们在扩展时间内依赖于IMU测量的双重积分。
   • 提出基于SVO的重初始化及掉帧恢复。是一个对紧耦合融合框架实用的方法。但需要一个俯视传感器来恢复尺度。
   • 基于ORB-SLAM的初始化方法。初始化初级的尺度，重力方向和速度以及IMU的偏置，在ORB-SLAM中给出的一系列关键帧集合，通过full BA计算出来。但尺度收敛的时间在10s以上。这对机器人导航任务来说是个问题，因为机器人导航需要在一开始就有尺度估计。
6. VIO方法在全局平移和旋转上存在累计漂移。

• 回环检测在长序列中扮演了重要角色。ORB-SLAM可以闭环和地图重用，利用了Bage-of-World的优势。
• 7DOF（位姿，方向和尺度）pose graph optimization紧跟在回环检测之后。
• 对单目VINS而已，多亏了IMU，漂移仅在4DOF中发生，即3D平移和重力方向（yaw angle）的旋转。
• 本文选择的方法：optimize the graph with loop constraints in the minimum 4DOF setting。

本文结构

• 测量与计算
  • 特征提取和跟踪，IMU测量单元在两个连续的帧预积分
• 初始化过程
  • 提供必要的数值，包括位姿、速度、重力方向、陀螺仪偏置和3D特征位置
  • 可以加速非线性优化序列
• 包含重定位的VIO与预积分的IMU紧耦合，特征观测和重检测特征，用于回环。
• pose graph optimization模块，需要几何校验的重定位结果，并进行全局优化来eliminate drift。
• VIO、重定位、pose graph optimization模块通过多线程并行执行。每个模块运行频率不同，实时保证了全局的运行时间。

测量预处理
视觉测量，由连续帧跟踪特征，并在最新的帧上检测新特征。
IMU测量，对两个连续帧预先积分。低成本的IMU会影响偏置和噪声。因此，将IMU的偏置考虑在IMU的预积分处理过程中。

视觉处理前端

• 对每张新的图，现存特征通过KLT稀疏光流法进行跟踪。同时新的角点特征进行检测，来保证每张图片100-300个特征点。检测器通过设置邻域特征的最小分割像素，来保证一个均匀的特征分布。2D特征首先去畸变，去掉异常点后，投影到单位球面。异常点去除通过基础矩阵模型中RANSAC方法实现。
• 筛选关键帧
  • 筛选标准
    • 与前一个关键帧的视差
      • 在当前帧和上一帧的跟踪特征平均视差，超过给定阈值，将该帧当作关键帧。
      • 平移和选择都会引起视差。但只有选择的特征没有办法三角化。为防止该情况，采用陀螺仪短时积分测量来补偿计算视差时的旋转。旋转补偿仅用于关键帧筛选，在VINS的旋转计算中并不涉及。即使陀螺仪包含大的噪声和偏置，它也仅仅影响关键帧选择的结果。
    • 跟踪质量
      • 跟踪特征数低于某个阈值，将该帧当作关键帧，防止特征跟踪完全丢失。

预积分简单理解

VINS-Mono理解1
VINS-Mono理解2