VINS代码主体结构和流程剖析——前端

前言

最近花了点时间学习了下经典的VSLAM框架VINS，前后端都采用优化的方式，且在运行过程中实时估计IMU加速度计和陀螺仪bias，在初始化阶段还可以实时优化camera到IMU的外参，解决了很多之前SLAM框架在实际中使用的问题，例如使用时间久之后的外参漂移、IMU的bias不准导致的系统不够鲁棒等等，实测也具有非常良好的效果。但是VINS的代码确实不太友好，当然秦大佬只是用于发paper并不需要放过多心力在工程化上。已经有很多文章分析这份代码的细节了，这里主要针对VINS-Fusion代码主体的结构和流程做一些解析记录，便于日后的工程化和一些进一步优化工作。

代码剖析

Estimator是前端的一个核心类，整个系统的入口和核心处理逻辑都位于这个类中，其中通过inputIMU和inputImage将IMU数据和图像数据喂入，待进一步处理，整体逻辑如下：

• 图像被喂入后马上进行了特征跟踪与提取，由featureTracker这个类负责，这个待会儿再谈；提取完特征后图像就被抛掉了，之后只需要用该帧图像的feature参与到之后的计算即可
• imu数据进入后也被存入buff中，待之后的使用
• processMeasurements()这个函数可以与数据接受部分解耦，因此VINS也提供了多线程开关，把这个函数单独放在一个线程中运行，避免其阻塞数据的读取。

先看看feature track部分：

• 使用上一帧的feature结合光流法得到本帧的feature，这些feature被认为是追踪上了，在之后的triangulate中只有连续追踪到三帧以上的feature才会进行triangulate，这里如果有预测过程会使用预测的feature位置
• 判断通过光流得到的feature数量是否足够（feature数量可通过参数指定），不够则在已有feature的mask外补充feature
• 对所有feature进行反畸变和计算速度等，然后返回结果。

再看看processMeasurements()这个函数：

• 做图像与IMU数据的同步
• imu数据采用一个专用类IntergrationBase进行积分处理，这个类主要完成IMU数据积分得到RPY，并使用ESKF的思路在后续bias更新后同步更新一段时间内的RPY，细节可以参考Error-State Kalman Filter理解与公式推导
• 拿到同时刻的image feature和imu积分数据，然后进入processImage()这个函数，到这里就开始真正的计算了。
  

进入processImage()后首先会将所有feature送入feature manager管理起来，这个类的主要功能是根据feature的id把每个feature放入一个特定的buffer管理起来，用于保存这个feature与keyframe的关联序列，同时也可以完成traiangulate；然后如果参数有设置的话会估计一次camera到IMU的外参的旋转部分，这个过程只进行一次，也就是外参在线校准。之后可以看到有两条支线，对应initialization过程和vio过程，也对应vins-mono论文中的V和VI部分。

• initialization过程：首先initialStructure()，也就是采用SFM估算sliding window中的两个关键帧的pose并初步triangulate一些feature，再采用PnP得到所有帧的pose，对应论文中V中A部分，这里提一下，这个过程中如果运动不够（通过IMU速度判断），或者用于SFM的feature视差过小都会导致初始化失败；然后进入visualnitialAlign()部分，对应论文V中B和C部分，依次完成IMU陀螺仪bias校准，速度、重力和尺度初始化，以及重力值的优化，然后做一些位置更新、IMU使用新的bias和重力重新积分、以及位置更新到世界坐标系的操作；完成之后，更新sliding window中的所有状态，完成初始化。
• VIO过程：因为完成了初始化，因此直接对feature manager中管理的符合条件（连续跟踪三次以上）的feature进行triangulate，得到其深度，然后构建优化问题进行优化，得到最新的优化后的pose，对应论文中VI部分；之后将追踪上的feature对其在跟踪到的帧上依次进行重投影误差计算，得到误差大于一定阈值后将被抛掉，避免一些错误的feature引入噪声，这部分论文中没有；最后进行滑动窗口进行marginalization，对应论文中VI的D部分。

到此，前端处理的部分就结束了。

一些小细节

• 关于slidingwindow：
  
  每帧都保留着从图像中追踪的feature和一个IntergrationBase，这个IntergrationBase存有从上一帧到这一帧的所有imu原始数据、bias、重力值以及积分结果，用以在优化和优化完成后使用新的bias重新积分使用。其内部使用了一些ESKF的思路，可以参考KF、EKF、ESKF的区别与联系中最后的部分。
• 有一个比较恶心的数据结构：map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>，其中数据的意义分别是：
  int: feature id
  int: camera id
  7*1 matrix: 3d点x,y,z，2d点x,y(投影到相机sensor平面的坐标)，2d点速度v_x,v_y