前言 随着自动驾驶技术的快速发展，多传感器融合的方案显得愈发成熟，本文就简单介绍下近几年很火的VINS-Mono。传统的单目视觉SLAM尺度具有不确定性，融合了IMU之后，可以有效的解决单目尺度不确定性的问题，并且视觉还可以消除IMU的积分漂移，极大的提升系统的鲁棒性。除此之外，在后端初始化中，IMU和视觉的融合事实上仅是一个松耦合，定位精度有效媲美OKVIS,而且具有比OKVIS更加完善和鲁棒的初始化以及闭环检测过程。那么接下来本文就简单介绍下VINS的原理。
1.VINS-mono VINS-Mono是由一个单目相机和一个低成本IMU组成的鲁棒通用的单目视觉惯性系统。通过融合预积分的IMU测量值和特征观测值来获得高精度的视觉惯性里程计，在结合闭环检测和图优化，构成了一个完整的单目VIO-SLAM系统。
1.1算法框架
大体流程如下：
- 从单目相机得到视觉图片，对采集到的图片做一次自适应直方图均衡化处理，然后基于FAST算法提取Harris角点特征，这里直接调用了OpenCV中的cv::goodFeaturesToTrack函数，并采用KLT金字塔光流算法进行特征点跟踪。对跟踪到的特征点放入一个队列中，五点法求出本质矩阵后，用随机一致性采样剔除Outliers。随后进行IMU的预积分，主要是为防止重新传播IMU观测值，加大运算难度。选用IMU预积分模型，从世界坐标系转为本体坐标系。离散状态下采用中值积分的预积分。
- 初始化完成后，采用基于滑窗的非线性优化方法来估计body的状态。其中body的状态包括滑窗中关键帧的IMU状态(位置，速度，旋转，加速度bias和角速度bias)；
- 当系统检测到回环时，在1中提到的代价函数中添加闭环检测得到的视觉测量残差部分，对状态进行重定位。其中闭环检测采用的DBoW2的方法。
- 在闭环检测后，对位姿中会产生漂移的4个参数进行位姿图优化，四个参数为X,Y, Z，yaml。 由于篇幅限制，下面就只介绍边缘化方法和IMU预积分。
1.2 IMU预积分
IMU是惯性测量单元，在VINS中，IMU具有两个设备，分别为陀螺仪和加速度计，其中陀螺仪测量IMU坐标系的旋转角速度，加速度计测量IMU坐标系下线性加速度。附上参考链接：
zhuanlan.zhihu.com/p/133666509…
为什么需要预积分？因为一个刚体在同一个惯性坐标系进行平移运动，其平移量对时间的一阶导和二阶导即速度和加速度。
预积分：
下一次的状态可以通过当前的状态迭代求解，如下：
预积分项：
因为IMU采集的数据是离散的，所以这里采用的积分方式为中值积分
由于公式过多，下面就不再推导公式，需要的可以自行查阅。我们需要知道预积分的目的是为了解决我们后端优化非线性优化时，需要迭代更新第k帧v和R，这将导致我们需要根据每次迭代后的值重新进行积分，计算量非常大。所以将优化变量从IMU的预积分项中分离开来。
1.3边缘化
根据前面讨论的基于高斯牛顿的非线性优化理论可知
需要注意的是，上式中的状态变量并非一定为相机位姿和路标点部分，而是希望Marg掉的和保留的部分。VINS中则真正需要边缘化的是滑动窗口中的最老帧或者次新帧，目的是希望不再计算这一帧的位姿或者与其相关的路标点，但是希望保留该帧对窗口内其他帧的约束关系。我们不能直接丢弃变量，因为它包含了对其他变量的约束，直接丢弃会缺少约束，丢失信息。需要采用Schur complement进行消元。
那我们就可以直接算出
，并且不会丢失任何约束，因此不会丢失信息。
VINS根据次新帧是否为关键帧，分为两种边缘化策略：通过比对次新帧和次次新帧的视差量，来决定marg掉次新帧或者最老帧。把上一次先验项中的残差项传递给当前先验项，并从中去除需要丢弃的状态量。
当次新帧不是关键帧时，MARGIN_SECOND_NEW，我们将直接扔掉次新帧及它的视觉观测边，而不对次新帧进行marg，因为我们认为当前帧和次新帧很相似，也就是说当前帧跟路标点之间的约束和次新帧与路标点的约束很接近，直接丢弃并不会造成整个约束关系丢失过多信息。但是值得注意的是，我们要保留次新帧的IMU数据，从而保证 IMU预积分的连贯性。通过以上过程先验项就构造完成了，在对滑动窗口内的状态量进行优化时，把它与 IMU残差项和视觉残差项放在一起优化，从而得到不丢失历史信息的最新状态估计的结果。
1.4数据集上对比
对比几个开源框架OKVIS,VINS不带回环，VINS带回环三种方法在EuRoC数据集下15个数据子集的效果。
2.VINS_Fusion 2.1基本介绍 VINS_Fusion是继VINS_Mono和VINS_Moblie后，港科大开源的双目视觉惯导SLAM方案，VINS_Fusion是一种基于优化的多传感器状态估计器，可实现自主应用的精确定位。是VINS-Mono的扩展，支持多种视觉惯性传感器类型。开源项目组还展示了将VINS于GPS融合的模组示例。
特征如下：
- 多传感器支持(立体摄像机/单声道摄像机+IMU/立体摄像机 + IMU)
- 在线空间校准(相机和IMU之间的转换)
- 在线时间校准(相机和IMU之间的时间偏移)
- 视觉循环闭合 基本原理和Mono一样，这里不再赘叙。
2.2Mono和Fusion对比
双目SLAM对比之前单目SLAM,主要一个好处在于初始化过程中，可以静止进行初始化。另一方面由于尺度信息不一定完全依靠IMU，因此不会造成尺度不客观的情况。但是由于视觉误差误匹配等各种原因，造成实际上双目的精度会比单目来的差一点。不过鲁棒性上考虑，双目明显优于单目。
本次实验使用的传感器是D435i，实验效果如下：
结束语 通过学习VINS的原理，我们可以了解VINS中最具创新性的地方，即预积分过程。这为我们以后改进自己的SLAM方案提供了不错的思路。
- End -
技术发展的日新月异，阿木实验室将紧跟技术的脚步，不断把机器人行业最新的技术和硬件推荐给大家。看到经过我们培训的学员在技术上突飞猛进，是我们培训最大的价值。如果你在机器人行业，就请关注我们的公众号，我们将持续发布机器人行业最有价值的信息和技术。 阿木实验室致力于前沿IT科技的教育和智能装备，让机器人研发更高效！
