基于视觉惯性融合的SVO

基于视觉惯性融合的SVO
7月初的时候尝试把IMU作为视觉导航的辅助项，加入到SVO的框架中去，目前已完成基于半直接单目视觉里程计SVO 的 视觉惯性融合 的程序框架搭建。以下围绕 ‘IMU数据以什么样的方式参与视觉系统导航？’、‘IMU可作用于视觉导航的哪些部分？’、‘融合IMU数据可以得到怎样的结果？’几个方面进行讲述。
本文仅代表个人观点，并作为工作学习记录，若有错误或者不妥之处欢迎指出！！！

在HeYijia对于SVO 的改进版本上https://github.com/HeYijia/svo_edgelet进行修改，并且参考VI-ORB代码https://github.com/jingpang/LearnVIORB以及VINS-Mono https://github.com/HKUST-Aerial-Robotics/VINS-Mono中的IMU使用。

#程序流程图

原来SVO框架
视觉惯性融合的半直接视觉里程计程序流程图大致如上，在原有的SVO框架上添加了视觉惯性联合初始化模块和后端优化模块。
#1IMU数据以什么样的方式参与视觉系统导航？
##IMU预积分
作用： IMU预积分算法可将积分增量与世界系下的惯性状态解耦，避免在线性化点更新时对测量值的重复积分计算，减少计算复杂度并获取连续时间内的相对运动增量
IMU测量频率在200Hz,视觉图像频率约20Hz，在两帧图像之间，对IMU采用预积分的方式进行运动增量计算。IMU就以这种预积分的方式参与视觉系统各个环节的计算。
在纯视觉系统中，我们只能得到视觉的状态估计量，无法构建运动模型，因此通过视觉测量方程构建重投影误差进行运动优化估计。在加上IMU测量数据之后，相当于引入了运动测量。在VIO中，就可以通过IMU的运动测量与视觉测量进行联合估计，有效提高系统鲁棒性。

#2 IMU可作用于视觉导航的哪些部分？
##2.1视觉惯性联合初始化
视觉惯性联合初始化是VIO必不可少的一部分，通过将视觉数据与IMU数据对齐，获取导航必须的初始参数估计。
最最重要的一点就是：单目视觉的尺度恢复！！
其实在SVO上添加IMU 的初衷也是为了恢复单目尺度。在原SVO中，作者将首帧固定，作为第一个关键帧，并在后续普通帧上进行光流跟踪，计算匹配特征点的个数以及平均视差，当平移量足够大 平均视差>50个像素时，才认为找到第二个关键帧，随后再通过视差的方差是否超过阈值来判断计算E矩阵还是H矩阵。通过前两个关键帧完成视觉初始化之后，得到两关键帧之间的平移量与旋转量t，R ，也得到了由两个关键帧中多有特帧点构建的初始地图。
在这一环节中，环境的尺度（包括特征点的坐标以及相机位姿）均以两关键帧之间的平移量t作为单位1。
而IMU得到的测量数据（加速度与角速度）均为世界系下的真实尺度的运动测量，通过IMU 预积分获取一段时间内的运动增量，并将其与视觉位姿估计的运动增量对比，可以恢复出视觉的真实尺度。这是单目尺度恢复的基本思想。

联合初始化的理论部分参考论文：Visual-Inertial Monocular SLAM with Map Reuse 通过 4 个步骤将视觉与惯性信息对齐，解决了视觉惯性系统的初始化问题。具体公式就不进行展开了，找论文都能看到。这篇论文是ORB-SLAM的作者写的，相应程序中也是使用同样的初始化方法。
我在程序中同样使用这种联合初始化方法，对关键帧处的陀螺仪偏差、单目尺度、重力矢量、加速度偏差以及速度进行估计，但让我很迷的一点就是“初始化的终止条件”？？Visual-Inertial Monocular SLAM with Map Reuse一文中作者把初始化的时间设为一个定值，貌似是做过实验：15s的初始化时间可以使得所有待估计参数可观。 但是在仿真过程中发现，15s并不是一个十分靠谱的数。之前Closed-Form Solution of Visual-Inertial Structure from Motion文章中提到过，当机体处于静止或匀速运动状态下时，VI-SfM 问题将存在无穷解，此时是无法进行视觉惯性联合初始化的。这是否表明，若是采用给定15s的初始化时间，那此区间内不能存在静止或者匀速运动？？？？ 我尝试过采用不同的起始帧进行图像跟踪，初始化估计出来的尺度时好时坏，并且与起始帧有着巨大的关联。使用某些起始帧，基本上在6、7s很快就能达到初始化参数收敛，用不到15s。而使用相差不远的帧作为起始帧，却估计出误差巨大的尺度。
关于这一点，目前还无法解决，希望有哪位研究过这类问题的同学可以解答一下！！

##2.2运动先验
视觉静止的先验模型：SVO 、LSD-SLAM
匀速运动的先验模型：ORB-SLAM、PTAM、DSO
在视觉惯性联合初始化之后，IMU就可以光明正大地参与运动跟踪了。
在VIO中，通常都会拿IMU作为运动先验，因为在联合初始化完成之后，陀螺仪和加速度计的偏置都可以被估计出来啦，而且重力矢量也可观测。此时的IMU除了一些小的测量误差以及器件的随机游走之外，基本上是没什么其他误差的。由于两帧图像之间的时间很短，其间的传感器偏差可视为定值，通过IMU测量数据预积分，可以在新一帧图像到来之前对运动进行先验估计。
SVO原程序中，采用的是位姿变化为0的先验模型，也就是静止模型：将上一帧位姿作为当前帧的位姿先验，进行图像匹配。这种方法虽然在能动性上差了一点但是也是有好处的，比如在运动缓慢的情况下、或者静止的情况下，视觉静止模型就表现得非常稳定。
而反观IMU作为先验，虽然在机体快速运动过程中IMU的测量精度很高，但是也要考虑缓慢运动状态下的IMU漂移可能引入累积误差的情况。
出于这一点考虑，我认为将IMU先验模型与其他静态更稳定的模型进行组合，再作为运动先验是一个比较合适的想法。

此处说明一下先验模型的误差计算方法：

数据集中一般都提供了groundtruth，使用其中的姿态四元数以及位移矢量作为真值。但是要验证运动先验额准确性的话需要基于上一帧的位姿，为避免位姿估计误差的影响，可以采用计算单帧运动增量的方法验证先验模型。
通过先验模型估计运动增量，再与真实增量求绝对误差，通过整条运动轨迹求单帧先验误差可以得到关于先验模型误差的对比曲线。

##2.3后端非线性优化
到上述为止都是关于前端的内容，视觉也好，IMU测量也好，主要目的都是为了估计当前帧的位姿。
在处理完最新一帧图像之后，就要进入一个局部的优化环节。
SVO原程序中在bundle_adjustment.cpp中是有写优化的内容的，但是没有使用，他自带的local_ba也是采用一种比较简单的局部优化方法。
当看到VI-ORB中的Optimizer.cc中的各种图优化函数时，我凌乱了···加了IMU测量的图优化模型开始有了千奇百怪的顶点和边，赶紧补了一波g2o的知识，在其他****上可以找到一些特定类型的函数模板，但是对应到自身程序上时还是需要进一步的加工和考量。
定义顶点和边的时候比较容易，确定好参与优化的变量以及顶点类型基本上就没什么问题；边的话要确定好每个边关联的顶点以及误差项的计算方法，接下去就直接调用g2o库就可以了，但其中的部分参数还是有点伤脑筋。例如设定误差边时的信息矩阵该如何选取？信息矩阵代表的是这个测量值的可信程度，信息矩阵的大小影响着对应边的误差在整个目标函数中的占比。参考版本函数大多都设为weight*InvCov（权重乘协方差的逆）这样的形式，但是当一个图中含有多种类型的边时，不同边的权重需要如何权衡，如何在多类型边的图中选择合适的权重可能要经过较长时间的测试进行调整。

在视觉后端优化中，为了提高计算效率，通常采用的是滑动窗口优化的方法。SVO 仅在关键帧上提取特征点，因此考虑使用基于关键帧的滑动窗口进行优化。
所以我在localBA的基础上又添加了一个windowBA的函数，在windowBA中结合了IMU数据进行联合优化，具体步骤类似VI-ORB中的Optimizer::LocalBundleAdjustmentNavState,用到了IMU的PVR以及bias。

就运行结果来说，没有想象中那么好。
每确定一个关键帧后都要进行一次滑窗优化显然有些浪费资源，并且图优中包含各种误差边的计算、变量优化反复迭代，消耗了较长的时间。
从效果层面，滑窗确实可以提高局部运动的鲁棒性，但是无法避免误差累积。 显然依靠简单的滑窗是无法达到高精度的目标的，尤其是在运动剧烈、旋转较大的数据集上，优化的方法经常出现无法运行的情况···

#总结
目前SVO与IMU融合的算法虽然在部分数据集中可以跑出比较好的效果，但还是存在很多漏洞和不当的地方。只能说搭建好了框架，可以使用，但无法普遍适用，后续将持续进行改进···