港科VINS-Mono系统学习

视频地址：https://www.youtube.com/watch?v=I4txdvGhT6I

github：https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git

系统框图如下

主要分为：

     1、测量数据预处理：IMU预积分、Harris特征提取和跟踪

     2、外参标定和初始：手眼标定法获得外参旋转量、纯视觉SFM、Visual-Inertial调整

     3、里程计：IMU误差+重投影误差+先验误差的多目标非线性优化调整、滑动窗口调整，关键帧获取、重定位

      4、闭环：fast特征提取+BRIEF描述子、利用BoW的闭环检测，基础矩阵+RANSAC的位姿估计、4自由度Psoe-Graph优化

数据预处理

1、图像获取(feature_tracker节点)

   a) 通过直方图均匀化的方法使得原本过亮或过暗的图像能够提取更多的特征点

   b) 用Harris角点为特征，采用KLT光流法对图像进行跟踪，从而实现特征的匹配

这里光流法有一个特点：只跟踪前一帧已知的特征点，所以不会存在第1帧和第3帧有某个特征点，而第2帧没有这个特征点的情况。所以程序中的feature管理就很好的利用了这一点，去找每个特征点在每一个帧上出现的情况

  c)用PUB_THIS_FRAME作为标志，以10hz的频率向estimator节点发送点云内容，这里每一帧发送的图像包含足够多的特征点(150个角点)

   d)向estimator发布的点云内容包括：

          1)特征点在相机坐标系的归一化坐标;

          2)相机ID号;

          3)特征点的ID号;

1、IMU预积分（代码中用的是mid-point方法，也就是用平均值代替整个时间范围内的值）

      作用：1)通过分段的积分，减少漂移

                 2)如果长期积分，那么在通过图像对pose矫正之后需要重新修改imu位姿，这就很麻烦

                 3)将重力加速度g提取到了积分外面，减少了计算量

      a) 位姿、速度的传递

      b)协方差和雅克比矩阵传递

         假设：测量量p、v、θ服从高斯正态分布；漂移量ba、bw服从高斯游走分布（高斯游走分布：前后时刻的值的差服从正态分布）。

                             

初始化

    初始化是整个VIO系统中非常重要的一部分，VIO系统通过初始化得到IMU的一些包括漂移，重力加速度等在内的参数，同时也为视觉定位提供了尺度信息。VINS的整个初始化过程主要分为以下6个步骤：

a) 手眼标定方法得到外参旋转量

b) 单纯的SFM得到视觉上的每帧的位姿和特征点的位置（仅仅进行了尺度归一化）

c) 以在同一时间段内，IMU旋转量与camera旋转量应该相同为条件计算陀螺仪的偏差Bg

d) 粗略求解每帧速度、g和s

           原理：1、IMU测得的两帧之间的位移和camera测得的两帧之间的位移应该相等

                     2、IMU测得的两帧之间的速度和camera测得的两帧之间的速度应该相等

e) 重力加速度优化

在tangent空间，迭代矫正g的方向，从而使得模在9.8附近，重新计算d步骤的公式。

f) 要把重力加速度的方向变成[0,0,1]的方向，也就是竖直向下。实际上，就是把之前系统认为的世界坐标系改成重力加速度竖直向下的方向。然后更新所有的参数，统一坐标系。

里程计

里程计部分的算法都是在一个滑动窗口中进行。具体步骤如下：

a) 首先，结合上一时刻的参数更新当前帧的位姿和特征点坐标；

 b)优化

                  优化变量：

   优化函数：分别是IMU测量误差、camera重投影误差、先验误差三个部分

      

初值：通过IMU预测得到。

最后，通过Ceres Solver求解器进行求解。Ceres Solver求解器在整个过程中不仅进行了目标优化，还做了Margin的工作（舒尔补margin）。

 c)滑动窗口

准则：

若新帧是关键帧：margin窗口中最旧的帧，把IMU数据和camera数据都margin掉；

若新帧不是关键帧：margin掉新帧，保留IMU数据，去掉camera数据

其中，关键帧的判断在函数addFeatureCheckParallax（）里面，通过视差阈值判断以及前后帧特征点个数进行判断的。

真正的margin是通过ceres求解器完成的。

闭环检测

闭环检测的步骤如下：

a)在滑动窗口后，把新关键帧加入集合中，再提取500个FAST特征，计算BRIEF描述子

b)通过BoW去搜索匹配到的闭环候选帧

c)在小窗口范围进行特征匹配（这里只对之前的Harris特征进行基础矩阵求解，因为只有这些特征点具有深度），用fundamentalmatrix+RANSAC去outliers

d)利用共视的特征点，将得到的闭环帧放入当前的滑动窗口进行优化。这里闭环帧的位姿是被固定的。

e)4自由度的位姿图优化

   这里roll，pitch是可观的，不进行优化。

f) 关键帧地图的构建

                         节点：每次margin后的关键帧，每3帧插入1帧

                          边1：连续的两帧

                          边2：闭环检测得到的帧间关系

                          这几个边都是一个4自由度的变量，平移量+yew角