上图为vins-mono算法框图,来自vins相关论文。
VINS的功能模块可包括五个部分:从上到下从左到右分别为,数据预处理、初始化、滑窗非线性优化BA、闭环检测及全局图优化。
代码中主要开启了四个线程,分别是:前端图像跟踪、滑窗非线性优化BA(其中初始化和 IMU 预积分在这个线程中)、闭环检测、全局图优化。
- 数据预处理
- 1光流追踪
Harris角点加KLT金字塔光流跟踪,输出跟踪上的特征点信息给滑窗。
Harris角点提取,采用opencv的goodFeatureToTrack()函数。
KLT金字塔光流跟踪,采用opencv的CalcOpticalFlowPyrLK()函数。
输出特征点信息为如下图。
1.2预积分
预积分只关心两帧之间的变化量,与上一状态无关。
位移P,姿态Q,速度V采用预积分公式计算。姿态Q采用四元素表示。预积分公式,参考vins论文,用的知识,分别为:a.加速度计、陀螺仪模型;b. 加速度、速度、位移积分公式属于初中数学知识;c. 姿态Q的积分,用了四元素对时间求导以及四元素乘法的相关知识。
协方差传递,采用误差卡尔曼滤波的predict的协方差传递公式。
2. 初始化
初始化如下五个参数:gyro零偏、外参(旋转量)、重力加速度初始方向、尺度和速度。
初始化过程:视觉和IMU积分的松耦合,各自计算轨迹,凑成一对。用视觉初始化惯导相关参数。先初始化外参(旋转量)、gyro零偏、重力加速度初始方向,因为此三个参数与尺度无关。最后初始化尺度和速度。
3. 滑窗
滑窗非线性优化BA将视觉约束、IMU约束和闭环约束放在一个大的目标函数中进行非线性优化,求解滑窗 内所有帧最优位姿。
滑窗优化示意图
上图来自vins相关论文。
x0到x4,代表机器的位姿,是被优化量。机器分别在这几个位姿拍摄了图片,观察到了一些路标f0到f3 。黄色的线条代表惯导测到的位姿之间的约束关系,黑色的线条代表视觉测到的位姿和路标之间的约束关系。xv0到xv4为闭环检测从全局图中找到回环帧。
非线性优化BA的目的,就是通过滑窗内多副图片的视觉、惯导约束关系来求解位姿x0到x4最优解,以保证机器位姿的准确性。
滑窗输出关键帧给后端回环。输出的关键帧,包括关键帧的姿态信息,地图点信息。
4. 回环检测
滑窗输出的的关键帧进行如下处理。
用fast额外提取了些特征点。在原图上计算BRIEF描述符。
DBoW查找4个得分最高的备选帧,从大到小排序。得分筛查。同时还要看id,id要尽量小。这里只选一帧,最近50帧不进行回环检测。
接下来备选帧与当前关键帧进行匹配。先是描述符匹配,大于25个匹配对时继续,PNP及随机一致性(opencv的函数)去除误匹配同时求出回环帧在当前vio下的位姿,如果匹配数大于25个就算成功,同时更新两帧之间的转换关系存在loop_info中并发布回环帧到滑窗。两帧不在同一个sequence中时,要进行sequence合并。
5. 图优化
上图来自vins相关论文。
每个关键帧为图中的一个节点。
顺序边,每个关键帧往前找5帧,建立顺序边。顺序边的建立方法为两个关键帧的位姿差,原点为当前关键帧。
残差构建,就是优化后的位姿差与建立的顺序边之间的差值。
采用ceres库,构建顶点和边,以及残差。然后求最优解。
6. 数据集运行
启动文件launch,启动各个节点。
配置文件yaml,配置所有参数。
打开三个终端,分别启动vins_estimator,rviz和播放bag文件。以MH_01为例:
roslaunch vins_estimator euroc.launch
roslaunch vins_estimator vins_rviz.launch
rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag
Ros 的rviz的数据流图。5个path应该是对应5个sequence地图。