VIO视觉惯导相关技术（概念版）
重要说明：本文从网上资料整理而来，仅记录博主学习相关知识点的过程，侵删。
一、参考资料
VIO系统介绍
主流VIO框架分析及VINS部分解析
多传感器融合详解
CSDN专栏：SLAM
二、VIO相关介绍
1. 视觉与IMU优势互补
本章节，分别介绍视觉、IMU的特性，通过分析单纯视觉、单纯IMU进行姿态估计的劣势和缺陷，全面阐述了视觉与IMU优势互补。
1.1 视觉与IMU特性对比
| 视觉 | IMU |
|---|---|
| 利用图像的VIO | 惯性测量单元 |
| 利用图像特征点匹配、像素（直接法）进行位姿估计 | 六自由度IMU，陀螺仪测量角速度，加速度计测量加速度 |
| 以图像形式记录数据，频率在15HZ-60HZ，载体位姿不能变化太快，受外界环境影响 | 高频>=100HZ，应对载体位姿变化比较快的情况 |
| 不产生漂移，直接测量旋转与平移 | 快速响应，不受成像质量影响，角速度普遍估计比较准确，可估计绝对尺度 |
| 受图像遮挡、运动物体干扰； 单目视觉存在尺度不确定性问题； 单目纯旋转运动无法进行位姿估计，动态场景出现错误匹配； 快速运动时，图像模糊，容易丢失； 在纹理少的区域无法工作 |
纯IMU预测会有漂移，角度漂移随时间线性发散，位置漂移随时间的二次方发散； IMU存在零偏； 低精度IMU积分的位姿发散，高精度IMU价格昂贵 |
| 适合长时间、运动慢的场景 | 适合短时间、运动快的场景 |
1.2 视觉与IMU优劣势对比
| 方案 | 视觉 | IMU |
|---|---|---|
| 优势 | 不产生漂移 直接测量旋转与平移 |
快速响应 不受成像质量影响 角速度普遍比较准确 可估计绝对尺度 |
| 劣势 | 受图像遮挡、运动物体干扰 单目视觉无法测量尺度 单目纯旋转运动无法估计 快速运动时易丢失 |
存在零偏 低精度IMU积分的位姿发散 高精度IMU价格昂贵 |
1.3 视觉劣势
当运动过快时，（卷帘快门的）相机会出现运动模糊，或者两帧之间重叠区域太少，以至于无法进行特征匹配，所以纯视觉SLAM不适合快速运动的场景。当图像发生变化时，本质上我们无法知道是相机自身发生运动，还是外界条件发生了变化，所以纯视觉SLAM难以处理动态的障碍物。这些都是相机的弱点。
1.4 IMU劣势
虽然，IMU可以测得角速度和加速度，但这些量都存在明显的漂移（Drift），使得两次积分得到的位姿数据非常不可靠。举个夸张的比喻，我们将IMU放在桌子上静止，用它的度数积分得到的位姿会漂移十万八千里。
1.5 视觉与IMU优势互补
IMU能够感受到自己的运动信息，从某种程度上减轻了动态物体的影响。而且，对于短时间内的快速运动，IMU能够提供一些较好的估计。相比于IMU，相机数据基本不会有漂移。如果相机放在原地固定不动，那么在静态场景下，视觉SLAM的位姿估计也是固定不动的。所以，相机数据可以有效估计并修正IMU的漂移，使得在慢速运动后的位姿估计依然有效。综合来说，利用视觉信息弥补IMU的零偏，减少IMU由于零偏导致的发散和累积误差；利用IMU为视觉提供快速运动时的定位。简单来说，IMU为快速运动提供了较好的解决方式，而相机又能在慢速运动下解决IMU的漂移问题。在这个意义下，IMU与相机实现优势互补。
1.6 总结
视觉和IMU融合有很好的互补性。首先，通过将IMU估计的位姿序列和相机估计的位姿序列对齐，可以估计出相机轨迹的真实尺度，而且IMU可以很好地预测图像帧的位姿以及上一时刻特征点在下一帧图像的位置，提高特征跟踪算法匹配速度和应对快速旋转的算法鲁棒性。最后，IMU中的加速度计提供的重力向量，可以将估计的位置转为实际导航需要的世界坐标系中。同时，智能手机等移动终端对MEMS器件和摄像头的大量需求，降低了两种传感器的价格成本。在硬件实现上，MEMS器件可以直接嵌入到摄像头电路板上。综上所述，融合视觉信息+IMU的VINS算法，可以很大程度提高单目SLAM算法性能，是一种低成本高性能的导航方案。
2. 引入VIO的原因
整体上，视觉和IMU定位方案存在一定互补性质：
- IMU适合计算短时间、快速的运动；
- 视觉适合计算长时间、慢速的运动。
视觉与IMU融合之后，会弥补各自的劣势，可利用视觉定位信息来减少IMU由于零偏导致的发散和累积误差；IMU可以为视觉提供快速运动时的定位，减少因为外界影响定位失败，同时有效解决单目相机尺度不确定性问题。
3. VIO的概念
视觉惯性里程计（visual-inertial odometry，VIO），简称视觉惯导，也称为视觉惯性系统（VINS，visual-inertial system），是视觉和IMU数据融合，实现SLAM的算法。
4. VSLAM与VIO
VSLAM是指基于视觉的SLAM方案，没有IMU信息；VIO是指视觉惯导，是基于视觉+IMU数据融合的SLAM方案。
5. 视觉+IMU融合优势
那么为什么要进行视觉与IMU的融合呢，自己总结的主要有以下几点：
- 视觉与IMU的融合可以借助IMU较高的采样频率，进而提高系统的输出频率。
- 视觉与IMU的融合可以提高视觉的鲁棒性，如视觉SLAM因为某些运动或场景出现的错误结果。
- 视觉与IMU的融合可以有效的消除IMU的积分漂移。
视觉与IMU的融合能够校正IMU的Bias。 - 单目与IMU的融合可以有效解决单目尺度不可观测的问题。
6. 主流的视觉+IMU融合方案
6.1 单目+IMU方案
ROVIO (单目+IMU）
RKSLAM (单目+IMU）
ORB_SLAM-IMU（单目+IMU）
VINS (单目+IMU、双目+IMU)
6.2 多目+IMU方案
OKVIS (单目+IMU、双目+IMU)
6.3 基于滤波的融合方案
松耦合：苏黎世联邦理工学院(eth)的 SSF(融合IMU和单个视觉传感器)和 MSF(融合IMU和多个位置传感器)；
紧耦合：比较经典的算法是MSCKF，ROVIO；
6.4 基于优化的融合方案
松耦合： 基于松耦合优化的工作不多。 Gabe Sibley在IROS2016的一篇文章《Inertial Aided Dense & Semi-Dense Methods for Robust Direct Visual Odometry》提到了这个方法。简单来说就是把VO计算的位姿变换添加到IMU的优化框架里面。
紧耦合：OKVIS（多目+IMU）、 VINS-Mono、VINS-Fusion、VI-ORB；
7. 相机-IMU标定
相机-IMU标定的目标是，获取两个传感器坐标系之间的空间关系和数据延迟，是VIO系统工作的前提工作。相机-IMU标定可以看成是状态估计的逆过程，通过标定板获取每个时刻的精确运动状态，计算出模型参数（坐标系间旋转、位移，时间延迟，IMU零偏），而运动估计则是在已知两个传感器坐标系间的模型参数，估计每个时刻的运动状态。
目前有现成的标定库：Camera IMU calibration。此外，相机-IMU标定需要事先知道相机和IMU的内参，在SLAM中经常需要对新的相机进行相机内参标定，推荐 ROS 自带的相机标定工具：Summary 可以实现在线标定。
8. VIO分类
根据融合框架的区别又分为紧耦合和松耦合，松耦合中视觉运动估计和惯导运动估计系统是两个独立的模块，将每个模块的输出结果进行融合，而紧耦合则是使用两个传感器的原始数据共同估计一组变量，传感器噪声也是相互影响的，紧耦合算法上比较复杂，但充分利用了传感器数据，可以实现更好的效果，是目前研究的重点。
8.1 松耦合
松耦合将多传感器（以视觉传感器和 IMU为例）作为两个独立的模块，两个模块均可以计算得到位姿信息，然后再进行融合。简单理解，图像通过特征点估算出位姿，然后和imu结合统一优化位姿。一般通过EKF卡尔曼滤波进行融合。
松耦合情况下滤波与优化的对比图。从图中可以看出，优化的优势并不是很明显。
8.2 紧耦合
8.2.1 紧耦合的概念
紧耦合是指将视觉和 IMU得到的中间数据，通过一个优化滤波器进行处理，紧耦合需要将图像特征加入到特征向量中，共同构建运动方程和观测方程，然后进行状态估计，最终得到位姿信息的过程。紧耦合使用两个传感器的原始数据共同估计一组变量，传感器噪声会相互影响。基于此，系统状态向量最终的维度也会非常高，同时计算量也很大。但充分利用传感器数据，可以实现更好的效果，是目前研究的重点。
紧耦合整体流程图，可以分解为五个部分：数据预处理、初始化、局部非线性优化、回环检测和全局优化。各个模块的主要作用是：
图像和IMU数据预处理模块：对于图像，提取特征点，利用KLT金字塔进行光流跟踪，为后面视觉初始化求解相机位姿做准备。对于IMU，将IMU数据进行预积分，得到当前时刻的位姿、速度、旋转角，同时计算在后端优化中将要用到的相邻帧间的预积分增量，及预积分的协方差矩阵和雅可比矩阵。
初始化模块：初始化中，首先进行视觉的初始化，解算出相机的相对位姿；然后再与IMU预积分进行对齐求解初始化参数。
局部非线性优化模块：对应流程图中滑动窗口的视觉惯导非线性优化，即将视觉约束、IMU约束放在一个大目标函数中进行优化。这里的局部优化就是只优化当前帧及之前的n帧的窗口中的变量，局部非线性优化输出较为精确的位姿。
回环检测模块：回环检测是将前面检测的图像关键帧保存起来，当再回到原来经过的同一个地方，通过特征点的匹配关系，判断是否已经来过这里。前面提到的关键帧就是筛选出来的，能够记下但又避免冗余的相机帧。关键帧的选择标准是：当前帧与上一帧之间的位移超过一定阈值，或者匹配的特征点数小于一定阈值，记为关键帧。
全局优化模块：全局优化是在发生回环检测时，利用相机约束和IMU约束，再加上回环检测的约束，进行非线性优化。全局优化在局部优化的基础上进行，输出更为精确的位姿。
8.2.2 代码示例
//i时刻相机坐标系下的map point的逆深度
double inv_dep_i = parameters[3][0];
//i时刻相机坐标系下的map point坐标
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
//i时刻IMU坐标系下的map point坐标
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
//世界坐标系下的map point坐标
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
//在j时刻imu坐标系下的map point坐标
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
//在j时刻相机坐标系下的map point坐标
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
Eigen::Map residual(residuals);
#ifdef UNIT_SPHERE_ERROR
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif
紧耦合情况下滤波与优化的对比图 ，从图中可以看出，优化的优势较为明显。
for (uint32_t j = 0; j < gnss_meas_buf[0].size(); ++j)
{
const uint32_t sys = satsys(gnss_meas_buf[0][j]->sat, NULL);
const uint32_t sys_idx = gnss_comm::sys2idx.at(sys);
const double obs_local_ts = time2sec(gnss_meas_buf[0][j]->time) - diff_t_gnss_local;
const double lower_ts = Headers[0].stamp.toSec();
const double upper_ts = Headers[1].stamp.toSec();
const double ts_ratio = (upper_ts-obs_local_ts) / (upper_ts-lower_ts);
GnssPsrDoppFactor *gnss_factor = new GnssPsrDoppFactor(gnss_meas_buf[0][j],
gnss_ephem_buf[0][j], latest_gnss_iono_params, ts_ratio);
ResidualBlockInfo *psr_dopp_residual_block_info = new ResidualBlockInfo(gnss_factor, NULL,
vector{para_Pose[0], para_SpeedBias[0], para_Pose[1],
para_SpeedBias[1],para_rcv_dt+sys_idx, para_rcv_ddt,
para_yaw_enu_local, para_anc_ecef},
vector{0, 1, 4, 5});
marginalization_info->addResidualBlockInfo(psr_dopp_residual_block_info);
}
const double gnss_dt = Headers[1].stamp.toSec() - Headers[0].stamp.toSec();
for (size_t k = 0; k < 4; ++k)
{
DtDdtFactor *dt_ddt_factor = new DtDdtFactor(gnss_dt);
ResidualBlockInfo *dt_ddt_residual_block_info = new ResidualBlockInfo(dt_ddt_factor, NULL,
vector{para_rcv_dt+k, para_rcv_dt+4+k, para_rcv_ddt, para_rcv_ddt+1},
vector{0, 2});
marginalization_info->addResidualBlockInfo(dt_ddt_residual_block_info);
}
// margin rcv_ddt smooth factor
DdtSmoothFactor *ddt_smooth_factor = new DdtSmoothFactor(GNSS_DDT_WEIGHT);
ResidualBlockInfo *ddt_smooth_residual_block_info = new ResidualBlockInfo(ddt_smooth_factor, NULL,
vector{para_rcv_ddt, para_rcv_ddt+1}, vector{0});
marginalization_info->addResidualBlockInfo(ddt_smooth_residual_block_info);
三、相关经验
VINS-MONO
CSDN专栏：VINS-MONO
github官方仓库：VINS-Mono
VINS-MONO github源码
VINS-mono详细解读
VSLAM: VINS-MONO论文理解——整体思路分析
港科大VINS-MONO入门（一）：框架入门及源码解析
【SLAM】VINS-MONO解析——对vins-mono的提升建议
建图导航 - 古月居
VINS-MONO实验总结
VINS-MONO简介
VINS-Mono是HKUST的Shen Shaojie团队开源的一套Visual-Inertial融合定位算法，是用紧耦合方法实现的，通过单目+IMU恢复出尺度，效果很好。
VINS-FUSION
VINS-FUSION 前端后端代码全详解