视觉惯性里程计VIO综述

参考：https://blog.csdn.net/xiaoxiaowenqiang/article/details/81192045
目前主流的VIO开源方案主要有以下几类，按照相机与IMU的耦合方式可分为松耦合和紧耦合，按照后端优化方案又可分为滤波优化和非线性优化。崔华坤老师有做过总结，详情可见老师github：
https://github.com/StevenCui/VIO-Doc
更新：
VI-ORB ——> ORB-SLAM3
ROVIO ——> ROVIOLI、Maplab

---

VIO简介

VIO算法和常规SLAM算法最大的不同在于两点：

• VIO在硬件上需要传感器的融合，包括相机和六轴陀螺仪，
  相机产生图片，
  六轴陀螺仪产生加速度和角速度。
  相机相对准但相对慢，六轴陀螺仪的原始加速度如果拿来直接积分会在很短的时间飘走（zero-drift），
  但六轴陀螺仪的频率很高，在手机上都有200Hz。
• VIO实现的是一种比较复杂而有效的非线性优化或着卡尔曼滤波，比如MSCKF（Multi-State-Constraint-Kalman-Filter），
  侧重的是快速的姿态跟踪，而不花精力来维护全局地图，
  也不做keyframe based SLAM里面的针对地图的全局优化（bundle adjustment）。
  最著名的商业化实现就是Google的Project Tango和已经被苹果收购的Flyby Media

传感器融合是一个趋势，也或者说是一个妥协的结果。主要的原因还是由于单一的传感器不能适用所有的场景，
所以我们寄希望于通过多个传感器的融合达到理想的定位效果。

• 简单的，目前行业中有很多视觉+IMU的融合方案，
  视觉传感器在大多数纹理丰富的场景中效果很好，但是如果遇到玻璃，白墙等特征较少的场景，基本上无法工作；
  IMU长时间使用有非常大的累积误差，但是在短时间内，其相对位移数据又有很高的精度，
  所以当视觉传感器失效时，融合IMU数据，能够提高定位的精度。
• 再比如，无人车当中通常使用 差分GPS + IMU + 激光雷达（视觉）的定位方案。
  差分GPS在天气较好、遮挡较少的情况下能够获得很好的定位精度，
  但是在城市高楼区域、恶劣天气情况下效果下降非常多，
  这时候融合IMU+激光雷达（视觉）的方案刚好能够填补不足。

惯性传感器（IMU）

能够测量传感器本体的角速度和加速度，被认为与相机传感器具有明显的互补性，而且十分有潜力在融合之后得到更完善的SLAM系统。

• IMU虽然可以测得角速度和加速度，但这些量都存在明显的漂移（Drift），
  使得积分两次得到的位姿数据非常不可靠。
  好比说，我们将IMU放在桌上不动，用它的读数积分得到的位姿也会漂出十万八千里。
  但是，对于短时间内的快速运动，IMU能够提供一些较好的估计。
  这正是相机的弱点。
  当运动过快时，（卷帘快门的）相机会出现运动模糊，
  或者两帧之间重叠区域太少以至于无法进行特征匹配，
  所以纯视觉SLAM非常害怕快速的运动。
  而有了IMU，即使在相机数据无效的那段时间内，
  我们也能保持一个较好的位姿估计，这是纯视觉SLAM无法做到的。

• 相比于IMU，相机数据基本不会有漂移。
  如果相机放在原地固定不动，那么（在静态场景下）视觉SLAM的位姿估计也是固定不动的。
  所以，相机数据可以有效地估计并修正IMU读数中的漂移，使得在慢速运动后的位姿估计依然有效。

• 当图像发生变化时，本质上我们没法知道是相机自身发生了运动，
  还是外界条件发生了变化，所以纯视觉SLAM难以处理动态的障碍物。
  而IMU能够感受到自己的运动信息，从某种程度上减轻动态物体的影响。

难点与分类

复杂性主要来源于 IMU测量 加速度 和 角速度 这两个量的事实，所以不得不引入运动学计算。

目前VIO的框架已经定型为两大类：
按照是否把图像特征信息加入状态向量来进行分类

• 松耦合（Loosely Coupled）
  松耦合是指IMU和相机分别进行自身的运动估计，然后对其位姿估计结果进行融合。
• 紧耦合（Tightly Coupled）
  紧耦合是指把IMU的状态与相机的状态合并在一起，共同构建运动方程和观测方程，然后进行状态估计。

紧耦合理论也必将分为 基于滤波(filter-based) 和 基于优化(optimization-based) 两个方向。

• 在滤波方面，传统的EKF以及改进的MSCKF（Multi-State Constraint KF）都取得了一定的成果，
  研究者对EKF也进行了深入的讨论（例如能观性）；

• 优化方面亦有相应的方案。

值得一提的是，尽管在纯视觉SLAM中优化方法已经占了主流，但在VIO中，由于IMU的数据频率非常高，对状态进行优化需要的计算量就更大，因此目前仍处于滤波与优化并存的阶段。

VIO为将来SLAM的小型化与低成本化提供了一个非常有效的方向。而且结合稀疏直接法，我们有望在低端硬件上取得良好的SLAM或VO效果，是非常有前景的。

松耦合

松耦合的方法采用独立的惯性定位模块和定位导航模块 ,

两个模块更新频率不一致 , 模块之间存在一定的信息交换 .
在松耦合方式中以惯性数据为核心 , 视觉测量数据修正惯性测量数据的累积误差.

松耦合方法中视觉定位方法作为一个黑盒模块 ,由于不考虑 IMU 信息的辅助 ,
因此在视觉定位困难的地方不够鲁棒 , 另外该方法无法纠正视觉测量引入的漂移 .

紧耦合

紧耦合方式使用 IMU 完成视觉 VO 中的运动估计 ,
IMU 在图像帧间的积分的误差比较小 , IMU的数据可用于预测帧间运动 ,
加速完成点匹配 , 完成VO 位姿估计 .
相对于松耦合 ,
紧耦合的另外一个优点是 IMU 的尺度度量信息可以用于辅助视觉中的尺度的估计

一、基于滤波器的紧耦合 Filter-based Tightly Coupled method

1. MSCKF

据说这也是谷歌tango里面的算法。
在传统的EKF-SLAM框架中，特征点的信息会加入到特征向量和协方差矩阵里,
这种方法的缺点是特征点的信息会给一个初始深度和初始协方差，
如果不正确的话，极容易导致后面不收敛，出现inconsistent的情况。

MSCKF的目标是解决EKF-SLAM的维数爆炸问题。传统EKF-SLAM将特征点加入到状态向量中与IMU状态一起估计，当环境很大时，特征点会非常多，状态向量维数会变得非常大。MSCKF不是将特征点加入到状态向量，而是将不同时刻的相机位姿(位置 [公式]和姿态四元数 [公式])加入到状态向量，特征点会被多个相机看到，从而在多个相机状态（Multi-State）之间形成几何约束（Constraint），进而利用几何约束构建观测模型对EKF进行update。由于相机位姿的个数会远小于特征点的个数，MSCKF状态向量的维度相较EKF-SLAM大大降低，历史的相机状态会不断移除，只维持固定个数的的相机位姿（Sliding Window），从而对MSCKF后端的计算量进行限定。

EKF-SLAM: 多个特征点同时约束一个相机位姿，进行KF更新
MSCKF : 一个特征点同时约束多个相机位姿(多相机观测同时优化，窗口多帧优化)，进行KF更新

• 传统的 EKF-based SLAM 做 IMU 融合时，
  一般是每个时刻的 系统状态向量(state vector) 包含当前的 位姿pose、速度velocity、以及 3D map points 坐标等（IMU 融合时一般还会加入 IMU 的 bias（飘逸: 零飘和溫飘）），
  然后用 IMU 做 预测predict step，
  再用 image frame 中观测 3D map points 的观测误差做 更新update step。

• MSCKF 的 motivation改进 是，EKF的每次 更新(类似优化)update step 是基于 3D map points 在单帧 frame 里观测的，
  如果能基于其在多帧中的观测效果应该会好（有点类似于 local bundle adjustment 的思想）。
  所以 MSCKF 的改进如下：
  预测阶段predict step 跟 EKF 一样，
  而 更新阶段update step 推迟到某一个 3D map point 在多个 frame 中观测之后进行计算，
  在 update 之前每接收到一个 frame，只是将 state vector 扩充并加入当前 frame 的 pose estimate。
  这个思想基本类似于 local bundle adjustment（或者 sliding window smoothing），
  在update step时，相当于基于多次观测同时优化 pose 和 3D map point。

2. ROVIO

基于稀疏图像块的EKF滤波实现的VIO
优点：计算量小(EKF，稀疏的图像块)，但是对应不同的设备需要调参数，参数对精度很重要。
缺点：没有闭环.，没有mapping thread。，经常存在误差会残留到下一时刻。

二、基于滤波器的松耦合 Filter-based Loosely Coupled method

松耦合的方法则简单的多，避免把图像的feature加入状态向量，
而是把图像当成一个black box,计算vo处理之后才和imu数据进行融合。
Ethz的Stephen Weiss在这方面做了很多的研究，
他的ssf和msf都是这方面比较优秀的开源算法，有兴趣的读者可以参考他的博士论文。

3. ssf

https://github.com/Ewenwan/ethzasl_sensor_fusion
红色字体部分是从传感器获取的数据，用于输入到预测(prediction)和更新阶段(update).
蓝色字体是更新阶段会变化的部分。
黑色部分为约束部分，是不变的。

变量：
p for pwi: 在世界坐标下的 IMU位置 IMU position in the world frame
v for vwi: 在世界坐标下的 IMU速度 IMU velocity in the world frame
q for qwi: 在世界坐标下的 IMU姿态 IMU attitude in the world frame

b_w for bw: 陀螺仪漂移 the gyro biases
b_a for ba: 陀螺仪漂移 the accelerometer biases

L for λ: 视觉尺度因子 the visual scale factor with pmetric*λ = psensor

q_wv for q,vw: 更新阶段参考帧(相机参考帧) 和 世界参考帧 之间的姿态变化
q_ci for qic: IMU and the update-sensor(相机) 姿态变化
p_ci for pic: IMU and the update-sensor(相机) 位置变换

4. msf

https://github.com/Ewenwan/ethzasl_msf

三、基于优化的松耦合 Optimization-based Tightly Coupled method

松耦合的工作不多，这里就不介绍了

四、基于优化的紧耦合 Optimization-based Loosely Coupled method

5. OKVIS

前端：多目+IMU 后端：ceres solver优化库

上图左边是纯视觉的odemorty,右边是视觉IMU融合的odemorty结构，
这个核心在于Frame通过IMU进行了联合，
但是IMU自身测量有一个随机游走的偏置，
所以每一次测量又通过这个偏置联合在了一起，
形成了右边那个结构，对于这个新的结构，
我们需要建立一个统一的损失函数进行联合优化.

相对应于MSCKF的filter-based SLAM派系，OKVIS是keyframe-based SLAM派系做visual-inertial sensor fusion的代表。从MSCKF的思想基本可以猜出，OKVIS是将image观测和imu观测显式formulate成优化问题，一起去优化求解pose和3D map point。的确如此，OKVIS的优化目标函数包括一个reprojection error term(重投影误差)和一个imu integration error term(imu积分误差)，
其中已知的观测数据是每两帧之间的feature matching(特征匹配)以及这两帧之间的所有imu采样数据的积分，注意imu采样频率一般高于视频frame rate，待求的是camera pose和3D map point，优化针对的是一个bounded window内的frames（包括最近的几个frames和几个keyframes）。

需要注意的是，在这个optimization problem中，对uncertainty(不确定性，类似方差)的建模还是蛮复杂的。首先是对imu的gyro和accelerometer的bias(漂移)都需要建模，并在积分的过程中将uncertainty(不确定性，类似方差)也积分，所以推导两帧之间的imu integration error(imu积分误差)时，需要用类似于Kalman filter中predict step(预测阶段)里的，uncertainty propagation(不确定性传播)方式去计算covariance(协方差矩阵)。

另外，imu的kinematics微分方程也是挺多数学公式，这又涉及到捷联惯性导航(strapdown inertial navigation) https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf

OKVIS使用keyframe的motivation(创新点)是，由于optimization算法速度的限制，优化不能针对太多frames一起，所以尽量把一些信息量少的frames给marginalization(滤出)掉，只留下一些keyframes之间的constraints。关于marginalization的机制也挺有趣。

6. VI-ORB（ORB-SLAM3）

ORB稀疏前端、图优化(g2o)后端、带闭环检测和重定位

ORB_SLAM2的作者在2017年提出了具有地图重用功能的单目视觉惯性定位算法VI-ORB，后延续到了ORB-SLAM3。该算法的具体思路和港科大的VINS有着异曲同工之妙，整体过程可分为下面几个部分：

• 整体流程与基础知识总结

• 基于流型的IMU预积分

• IMU初始化（视觉惯性联合初始化）

• 紧耦合优化模型
  1 视觉特征跟踪和基于流型IMU预积分
  2. IMU初始化（视觉惯性联合初始化）
  终于来到视觉惯性初始化阶段了，这段是视觉和惯性数据融合的第一步，是一段松耦合过程。
  理论上是将准确的视觉数据（但缺少尺度）和快速的IMU数据（需要重力加速度又存在零偏误差）相结合。

• 陀螺仪偏置标定（零偏）
  这一部分比较简单，直接联立N-1个相机做旋转矩阵的最小二乘即可，然后通过高斯牛顿方法即可得到零偏bg。需要注意一点，当求出零偏后将其代入预积分公式会重新计算一遍预积分值，使预积分数值更加准确.

• 尺度恢复和重力加速度预估
  首先建立预估状态向量X=[s,gw]，其中s是尺度，gw是世界坐标系下的重力加速度也是第一个相机坐标系下的重力加速度。
  ORB_SLAM2中世界坐标选取的是第一个相机对应的坐标（VINS则不同），这样做会存在一个问题，因为第一个相机可能自身存在一定的旋转倾斜导致整个世界坐标看起来是歪着的，画轨迹的时候有一种倾斜的即视感，所以我觉得还是尽量固定好z方向，使轨迹没有横滚和俯仰。
  这里使用了三个关键帧联立视觉和IMU预积分数据构建一个AX=B的最小二乘超定方程，至少需要四个关键帧，采用奇异值分解求最小二乘问题，速度较慢但精度高。

• 加速度计偏置标定和尺度重力加速度优化
  上面计算过程没有考虑到加速度计偏置的影响，使得重力加速度和加速度计偏置难以区分，
  很有可能会导致系统病态性问题，文中提出了重力加速度的大小G，假设其是一个不变值，优化重力加速度的方向。

3. 紧耦合优化模型
   

7.VINS-Mono

前端基于KLT跟踪算法， 后端基于滑动窗口的优化(采用ceres库)， 基于DBoW的回环检测