VINS fusion软件架构分析（4）---后端处理processMeasurements()功能函数概览

1. 本章概述

上一章写完了如何将IMU和相机信息输入到VINS中，解析了inputIMU 和 inputImage函数。本章接着描述对IMU和相机信息进行的后续处理（初始化、非线性优化、边缘化、滑动窗口内数据更新等），这部分内容是在processMeasurements()中完成的。考虑到processMeasurements()涉及的内容过多，因此本章暂时只描述该功能函数概览。

2. processMeasurements在哪里调用的

在estimator.setParameter()中默认开启多线程，即MULTIPLE_THREAD=1。此时processMeasurements函数会一直在运转，处理IMU和相机信号

3. 信号数据结构

在解读代码过程中，对数据结构的正确理解是非常有必要的，下面分别是图像和IMU数据结构。

3.1 IMU数据结构

3.2 图像数据结构

• featureFrame包含某一帧的所有个特征点信息
• Estimator.feature包含时间戳t + featureFrame
• Estimator.featureBuf是图像信息的缓存区，存储了多个帧的时间戳t + featureFrame。
  这里解释一下，VINS先接收了图像信号，先放入featureBuf中暂时存储，会不断从featureBuf中提取图像信号进行处理。
  

3.3 组合图像+IMU信息的数据结构

在分别获取了图像信息和IMU信息后，VINS需要把这两个信息融合在一起，这里用到imageframe。
imageframe包含了当前帧所有特征点信息points（即featureFrame），时间戳，当前帧和上一帧之间的IMU预积分信息等。
打包时间戳t + imageframe，把所有帧的信息存入all_image_frame中。
注意：这里构建all_image_frame只在初始化中使用，后续的非线性优化中不使用all_image_frame。

3.4 特征点管理器的数据结构

f_manager是特征点管理器，管理当前窗口内所有帧观测到的所有特征点。
f_manager.feature是管理单个特征点的信息。
f_manager.feature.feature_per_frame是管理单个特征点在一帧图像上的信息。
非线性优化中构建的重投影误差就是利用特征点管理器f_manager中的信息进行构建的。因此对f_manager的正确理解非常重要。

4. processMeasurements()

processMeasurements()的主要工作流分为三步骤：

1. getIMUInterval() 函数，把前一帧图像和当前帧图像之间的IMU信号提取出来，存入accVector, gyrVector中。
2. processIMU()函数，利用前一帧图像和当前帧图像之间的IMU信号，更新当前帧的位姿Rs[i]、Ps[i]、Vs[i]，作为后端非线性优化的初始值，同时计算得到的IMU预积分也作为后端非线性优化的一个输入。
3. processImage()函数，利用当前帧图像信息，进行后续的关键帧判断、初始化、非线性优化、边缘化、滑动窗口移动等操作。
   

4.1 getIMUInterval()

getIMUInterval() 函数，把前一帧图像和当前帧图像之间的IMU信号提取出来，存入 accVector， gyrVector 中。

4.1 processIMU()

processIMU()函数，有两个功能：

• 利用前一帧图像和当前帧图像之间的IMU信号，更新当前帧的位姿Rs[i]、Ps[i]、Vs[i]，作为后端非线性优化的初始值
• 计算得到的IMU预积分pre_integrations[frame_count]，作为后端非线性优化的一个输入。
  

5. processImage()

processImage()的主要工作流分为三步骤：

1. addFeatureCheckParallax() 函数通过视差判断该帧是否是关键帧，并决定后续边缘化次新帧MARGIN_SECOND_NEW ，还是边缘化最老帧MARGIN_OLD
2. 用当前帧的特征点信息和IMU预积分信息构建imageframe和all_image_frame
3. 分为初始化前和初始化后的进行后端的各项操作（非线性优化、边缘化等）
   

5.1 初始化完成前

VINS系统启动后，首先要完成初始化，初始化的流程分为：

1. 视觉惯性联合初始化initialStructure()
2. 视觉惯性联合初始化成功后，进行非线性优化optimization()、滑动窗口内数据更新slideWindow()。

5.2初始化完成后

VINS系统完成初始化后：

1. 按照当前位姿，对之前求出的深度为负值的特征点重新求深度f_manager.triangulate()
2. 进行非线性优化optimization()
3. 删除重投影误差过大的特征点f_manager.removeOutlier()
4. 出现较大的异常failureDetection()，清除当前的状态
5. 滑动窗口内数据更新slideWindow()。
6. 清除深度估计错误的特征点f_manager.removeFailures()