Vins-mono概述

        上图为vins-mono算法框图，来自vins相关论文。

         VINS的功能模块可包括五个部分：从上到下从左到右分别为，数据预处理、初始化、滑窗非线性优化BA、闭环检测及全局图优化。

        代码中主要开启了四个线程，分别是：前端图像跟踪、滑窗非线性优化BA（其中初始化和 IMU 预积分在这个线程中）、闭环检测、全局图优化。

1. 数据预处理
   1. 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地图。