VIO学习总结
重磅干货，第一时间送达
本文转自|新机器视觉
VIO（visual-inertial odometry）即视觉惯性里程计，有时也叫视觉惯性系统（VINS，visual-inertial system），是融合相机和IMU数据实现SLAM的算法，根据融合框架的区别又分为紧耦合和松耦合，松耦合中视觉运动估计和惯导运动估计系统是两个独立的模块，将每个模块的输出结果进行融合，而紧耦合则是使用两个传感器的原始数据共同估计一组变量，传感器噪声也是相互影响的，紧耦合算法上比较复杂，但充分利用了传感器数据，可以实现更好的效果，是目前研究的重点。
单目视觉 SLAM 算法存在一些本身框架无法克服的缺陷，首先是尺度的问题 ，单目 SLAM 处理的图像帧丢失了环境的深度信息，即使通过对极约束和三角化恢复了空间路标点的三维信息，但是这个过程的深度恢复的刻度是任意的，并不是实际的物理尺度，导致的结果就是单目SLAM 估计出的运动轨迹即使形状吻合但是尺寸大小却不是实际轨迹尺寸；由于基于视觉特征点进行三角化的精度和帧间位移是有关系的，当相机进行近似旋转运动的时候，三角化算法会退化导致特征点跟踪丢失，同时视觉 SLAM 一般采取第一帧作为世界坐标系，这样估计出的位姿是相对于第一帧图像的位姿，而不是相对于地球水平面 (世界坐标系) 的位姿，后者却是导航中真正需要的位姿，换言之，视觉方法估计的位姿不能和重力方向对齐。
重力向量构建了视觉坐标系和世界坐标系的联系
通过引入 IMU 信息可以很好地解决上述问题，首先通过将 IMU 估计的位姿序列和相机估计的位姿序列对齐可以估计出相机轨迹的真实尺度，而且 IMU 可以很好地预测出图像帧的位姿以及上一时刻特征点在下帧图像的位置，提高特征跟踪算法匹配速度和应对快速旋转的算法鲁棒性，最后 IMU 中加速度计提供的重力向量可以将估计的位置转为实际导航需要的世界坐标系中。同时，智能手机等移动终端对 MEMS 器件和摄像头的大量需求大大降低了两种传感器的价格成本；硬件实现上， MEMS 器件也可以直接嵌入到摄像头电路板上。综合以上，融合 IMU 和视觉信息的 VINS 算法可以很大程度地提高单目 SLAM 算法性能，是一种低成本高性能的导航方案，在机器人、AR/VR 领域得到了很大的关注。
从另外一个角度理解视觉惯性融合的意义，我们一般采用运动方程和测量方程描述机器人的运动过程，具体到视觉惯性紧耦合运动估计问题，这里的第k 时刻的状态
为IMU的运动测量输入，
实现根据上一时刻估计的状态和k-1到k 过程中IMU的运动测量预测的当前时刻状态
，由于测量噪声的存在，预测状态
和当前真实状态
的差异由噪声项
表示。测量方程中，
为k时刻可以观测到的所有3D路标点集，h在这里对应相机投影模型，
为k 时刻对路标点集
的观测，观测值为
，观测噪声为
，即k时刻3D路标点集
在相机图像上的投影点集为
。
整个机器人的运动过程就是运动方程根据上一时刻状态和当前时刻运动输入
预测当前状态，利用传感器的测量方程作为矫正的过程，在纯视觉SLAM中，因为缺少运动输入，预测方程提供信息不足，我们是直接采用测量方程构建重投影误差进行运动优化估计，而引入IMU数据之后，用IMU作为运动输入
可以对状态进行很好的预测，VIO就是利用了运动预测和测量方程构建联合估计。具体来说，在假设噪声服从高斯分布，对非线性方程进行一阶近似的条件下，从最大后验估计的角度就可以推导出我们在VIO论文中看到的联合优化目标函数：
书籍
对三维旋转的描述计算需要李群李代数的知识，高博的书和《state estimation for robotics》都有详细讲解，简单来说，使用旋转矩阵表示的旋转方便向量的坐标计算，但直接对其优化是有约束优化问题（9个参数3个自由度），解决思路就是在三维旋转群的正切空间上对用李代数表示的旋转误差量进行无约束优化，对正切空间的理解可以参考:
《Lie groups, Lie algebras, projective geometry and optimization
for 3D Geometry, Engineering and Computer Vision》。
推公式的时候发现矩阵白学了？宝宝不哭，这有一本《the Matrix CookBook》送给你。
VIO 开源框架
vinsmono的代码和论文比较一致，是目前大家学习参考比较多的开源框架。
VIO中，目前多采用流形空间上预积分的方法对IMU数据进行预处理，核心思路是在两帧之间计算IMU的帧间运动增量，在迭代优化时直接使用运动增量，提高计算效率。这部分参考论文《On-Manifold Preintegration for Real-Time
Visual-Inertial Odometry》，整篇论文推导的思路是先推导出流形空间上的运动状态的表达公式，然后为了能够整合到最大后验估计的优化框架里，分离出运动表达式中的噪声项，使其近似满足高斯分布，之后推导了在bias变化时的运动估计值的更新方法。
前面有提到VIO用到了以IMU作为控制输入的运动方程，Joan Sola 写的《Quaternion kinematics for the error-state Kalman filter》对以IMU测量值为运动输入的运动方程的误差递进形式进行了详细推导，如果在vinsmono和okvis的运动递进方程的雅克比矩阵推导遇到困难，或者对四元数方法进行运动描述不了解，可以仔细阅读下这篇文章。文章对坐标系的locally和globally的解释也让人印象深刻。
VIO的初始化是系统工作非常关键的部分，这部分可以参考vinsmono以及ORB作者写的的VIO文章《Visual-Inertial Monocular SLAM with Map Reuse》。两篇文章思路比较相似，先是通过单目运动估计的方法获取多帧图像的位姿，然后以此为运动参考估计其他参数，整个过程和相机IMU标定比较相似。
初始化主要完成三部分工作：
为非线性估计系统提供一个运动初值 2. 估计重力向量在视觉坐标系下的投影向量，以此将视觉坐标系对齐到世界坐标系下。 估计尺度、IMUbias
vinsmono的代码中协方差矩阵递进公式采用的是中值积分的方法，推导思路和Joan Sola文章中的一致，具体推导可以参考:
https://www.zhihu.com/question/64381223/answer/255818747
https://blog.csdn.net/u012871872/article/details/78128087
vonsmono代码中的融合部分是非常值得学习参考的，但是代码中的视觉处理部分多是直接使用OPENCV的函数，而且代码风格是C++/C 混合的。所以推荐看下ORBSLAM的代码，首先编程实现非常规范，对编程学习有很大的参考价值，而且整个代码对opencv的依赖较低，视觉部分的特征提取，视觉运动估计，都是作者自己编程实现的，对理解视觉几何的实现有很好的帮助，泡泡有篇公开课对ORBSLAM的代码进行了梳理:
https://pan.baidu.com/s/1c1QOoHM 密码: xfjd