vins中imu融合_VINS-Mono paper Reading Note

VINS-Mono是一套较为完善的基于非线性优化的Visual-Inertial 系统，和此前基于滤波方案的MSCKF系列以及更早的一篇基于非线性优化的OKVIS，算得上是几篇非常值得学习的视觉定位框架，这三类都有着各自的特点。

先说说MSCKF吧，原版的作者没有开源相应的代码，但是Kumar机器人实验室的ke.Sun有基于MSCKF的思想开源了基于双目MSCKF代码https://github.com/KumarRobotics/msckf_vio, 和对应论文[1]；代码写的非常的整洁，注释也非常清楚； 另外宾大Daniilidis组开源了一套单目视觉惯导代码https://github.com/daniilidis-group/msckf_mono， 其核心代码应用在一篇基于even的paper[2]；基于滤波框架的VIO，融合通常由EKF完成，其中IMU的积分(现在更多使用预积分的方式表示其观测)用于状态传播，而视觉位姿用于待优化状态变量的更新。MSCKF在状态向量中维护以前的几个camera pose，并使用多个camera视角中相同特征的视觉观测来形成多约束更新。后续有时间想整理一篇读书笔记，展开阐述MSCKF的pipeline和后续的扩展优化。

网上关于这几篇经典paper的论文解析，博客，甚至代码解析都非常多。我这里主要对自己的一个读书笔记总结，很多方面不够系统全面。VINS-Mono是港科大沈劭劼老师组中具有代表性的一篇工作，沈老师师从宾大的Kumar教授。沈老师多次表示VINs-Mono的应用更应该称做单目视觉惯性状态估计器而不是视觉惯性里程计。下图主要是VINS-Mono的框架：

VINS-Mono 系统框架图

Vins-Mono paper[3]的结构主要包括5个大模块：

1) Measurement Preprocessing
2) System Initialization
3) Local Visual-Inertia BA and Relocalization
4) Global Pose Graph Optimization
5) Loop detection
6) Experimental Analysis

VIN-Mono的核心是一个紧耦合，滑动窗口，非线性优化单目视觉惯性里程计， 不仅提供精确的局部姿态、速度和方位估计(PVQ)，而且还能够在线进行Camera、IMU的外参校准以及IMU偏置校正。这篇工作是在沈老师组里一系列工作基础上的一个集成和优化，主要包括[4]-[7]，这四篇工作逻辑也很清晰，都是沈老师和学生关于视觉惯导鲁棒初始化、在线外参估计和校准、以及视觉惯导系统框架等不同侧重点的研究。相比上述的几篇工作，Vins-Mono改进了含偏置校正的IMU预积分、紧耦合重定位、全局位姿图优化这几个方面。先简单引入paper中的一些坐标表示：

表示世界坐标系(world frame)。重力方向与世界坐标系z轴对齐。

是体坐标系(body frame)，也叫错IMU坐标系。

是相机坐标系(camera frame)。

和

表示从body系到world系的旋转和平移。

表示获取第k个图像时的body系坐标。​

表示获取第k个图像时的camera系坐标。⊗表示两个四元数之间的乘法运算。

是world系上的重力向量。将(^)表示为某一具体量的噪声测量值或估计值。

1. Measurement Preprocessing

1.1 视觉前端的预处理

视觉部分使用KLT光流对前端进行持续的追踪，同时会detect新的corner，保证每帧都能有足够的feature和其他帧构建constrain，并且做了均匀化操作，相比ORB_SLAM前端的精细均匀化，VINS-Mono的前端主要通过一个mask限制corner之间几个pixel的间隔。其次，对观测outlier的剔除其实做的也很简单，主要使用RANSAC通过基本矩阵模型来判别，为了保证系统的高效性，在前端的特征track的过程中引入KeyFrame，而KeyFrame的选择满足以下条件之一即作为KeyFrame ：1）当前帧与最近的KeyFrame的视差超过某阈值，2）跟踪质量下降时（match少于一定阈值）。

1.2 IMU数据的处理

IMU数据主要通过预积分的方式来表示，预积分最早是在欧拉角上做参数化，Scaramuzza组的预积分paper[8]对其做了较为完整的扩展，在流行空间进行参数化并且增加后验IMU偏置校正, 这篇工作虽然15年才出来，但是可能都是大家非常熟悉的经典paper，其内容和补充材料的公式推导都非常的详细。接着还是简单梳理一下VIN-Mono中的预积分的应用。

IMU的原始加速度和角速度测量值

和

，由真实加/角速度以及bias和noise等构成。IMU的测量值都是在body系中测量得到：

（1.1）

给定连续两帧image的时刻

, 位置、速度和方向等状态变量，可以在world系下通过惯性测量值进行传递：

（1.2）

IMU状态传递需要坐标系body系

的旋转、位置和速度。而在优化过程中，每次迭代&#