VINS-FUSION算法总结

简介

vins-fusion在将单目vio系统vins-mono扩展到了双目，同时添加了GPS的融合信息，支持纯双目、双目+IMU、双目+imu+GPS、单目+IMU等多种传感器组合方式。本篇对vins-fusion的主要改进点进行总结。

1. 双向光流

1.引入预测机制优化光流金字塔的层数
引入一些预测的机制，如果预测成功(hasPrediction)说明像素在相邻两帧之间的运动较小，所以不需要太多层金字塔，将光流跟踪的图像金字塔由4层变为2层。如果基于2层金字塔跟踪到的点比较少(<10)，则重新使用4层金字塔进行跟踪。

2.引入双向光流的检查机制
当特征点从上一帧跟踪到下一帧之后，再进行一次反向跟踪即使用下一帧的特征点在上一帧图像上进行跟踪，对于跟踪结果要满足对于同一特征点对应的状态位相同(正着跟踪成功同时反向也跟踪成功)，以及正反跟踪到的特征点的像素差值小于阈值(0.5)。

对于不满足判定条件的特征点，将其对应的状态位重置为0。

3.双目相机配置
对于双目相机，还会进行一步左目到右目的光流跟踪，实现左右目特征点的数据关联。同样的，左右目的之间也会进行一次双向光流跟踪。

左右目之间的光流跟踪使用4层金字塔进行光流跟踪。

2. 初始化

在初始化模块的改进主要体现在增加了双目初始化的内容，更具体的是特征点三角化部分。

对于双目相机，可以根据相机内参和左右目相机的外参恢复出当前帧特征点的深度信息。

首先将当前帧位姿从imu坐标系转换到相机坐标系，得到左目和右目相机坐标系下的位姿

然后基于左右目位姿和特征点在左右目相机的坐标三角化恢复出特征点在左目相机坐标系下的3D坐标，要注意的是因为双目模式下左右目相机的位姿是通过左右目的外参得到的，这里外参的平移是具备真实尺度信息的，因此恢复出的3D点具备真实的尺度信息。

如果对于某个特征点没有同时在左右目相机中出现，或者干脆就是单目模式，则使用当前帧位姿和下一帧位姿对特征点进行三角化。

3. 后端优化

对于双目系统，在没有IMU的情况下为6自由度客观(原来为4自由度客观)

对于双目+IMU模式，关于外参和延时参数的更新添加了关于运动激励的约束条件，当滑窗被填满，滑窗第0帧速度的绝对值大于0.2米每秒才会开始估计外参。

双目重投影残差计算针对右目有所更新

如果要约束的第i帧和第j帧不是同一帧： 第i帧左目和第j帧左目之间关于重投影的视觉残差和vins-mono相同，第i帧左目和第j帧右目的重投影视觉残差中间多了一步外参的转换，需要先将右目的特征点转换到左目相机坐标系再计算重投影误差。

如果要约束的第i帧和第j帧是同一帧： 无法基于位姿建立约束关系，但是可以基于外参和特征点深度信息建立约束进行求解。

4. 外点剔除

1.在后端增加课外点的筛选机制
遍历滑窗中的图像帧，对于某个特征点计算其在观测到该点的第一帧和其他帧之间的重投影误差(重投影的结果减去光流跟踪到的结果)，双目模式下还要计算与右目的重投影误差，最后求一个平均重投影误差，如果重投影误差大于3个像素，则认为该点是一个外点。

2.增加对特征跟踪的预测机制
这里的预测机制和上述关于光流金字塔的改进相对应,主要是predictPtsInNextFrame。

这里定义了一个匀速模型(参考ORB-SLAM中的匀速模型)，通过匀速模型和当前帧位姿可以初步预测出下一帧的位姿，然后根据预测的下一帧位姿将当前帧图像坐标系下的3D点转换到下一帧图像坐标系下(中间经历了相机转IMU坐标系再转下一帧相机坐标系的转换)。

然后将3D点载投影到像素坐标系保存下来，用于后面的光流跟踪，同时将成功预测的标志hasPrediction设置为true，hasPrediction为真时在进行光流跟踪时使用2层金字塔进行跟踪。

5. GPS融合

GPS主要给slam系统提供了绝对位姿估计的信息。

在global_fusion节点中主要是订阅vio的位姿和gps信号，然后将vio的位姿转换到gps坐标系并进行单位的统一。在SLAM系统运行后，将初始帧对应的GPS经纬度作为SLAM系统的起点和参考系，后续的位姿都基于该参考系进行更新。

在构建约束关系时，主要利用VIO之间的帧间位姿约束、GPS的绝对位置(x,y,z)以及VIO位姿和GPS位置之间的旋转变换关系设计代价函数。