文章目录
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()的主要工作流分为三步骤:
- getIMUInterval() 函数,把前一帧图像和当前帧图像之间的IMU信号提取出来,存入accVector, gyrVector中。
- processIMU()函数,利用前一帧图像和当前帧图像之间的IMU信号,更新当前帧的位姿Rs[i]、Ps[i]、Vs[i],作为后端非线性优化的初始值,同时计算得到的IMU预积分也作为后端非线性优化的一个输入。
- 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()的主要工作流分为三步骤:
- addFeatureCheckParallax() 函数通过视差判断该帧是否是关键帧,并决定后续边缘化次新帧MARGIN_SECOND_NEW ,还是边缘化最老帧MARGIN_OLD
- 用当前帧的特征点信息和IMU预积分信息构建imageframe和all_image_frame
- 分为初始化前和初始化后的进行后端的各项操作(非线性优化、边缘化等)
5.1 初始化完成前
VINS系统启动后,首先要完成初始化,初始化的流程分为:
- 视觉惯性联合初始化initialStructure()
- 视觉惯性联合初始化成功后,进行非线性优化optimization()、滑动窗口内数据更新slideWindow()。
5.2初始化完成后
VINS系统完成初始化后:
- 按照当前位姿,对之前求出的深度为负值的特征点重新求深度f_manager.triangulate()
- 进行非线性优化optimization()
- 删除重投影误差过大的特征点f_manager.removeOutlier()
- 出现较大的异常failureDetection(),清除当前的状态
- 滑动窗口内数据更新slideWindow()。
- 清除深度估计错误的特征点f_manager.removeFailures()