vins概述

基本框架如上，VINS的功能模块可包括五个部分：数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程，分别是：前端图像跟踪、后端非线性优化（其中初始化和 IMU 预积分在这个线程中）、闭环检测、闭环优化。

正常运行过程中，初始化只进行一次，前端不断获取特征点发送给后端，后端进行IMU数据采集、预积分、优化和滑动窗口操作等等。

1.预积分：IMU得到的是角速度和加速度，通常对其积分，来获得位姿信息（位置、速度、角度）。又因为IMU采集频率比图像获取的频率高，所以要进行预积分使两者对齐。

2.初始化：初始化要做的事其实说起来很简单，就是计算出绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v。用SFM进行纯视觉估计滑窗内所有帧的位姿及 3D 点逆深度，最后与 IMU 预积分进行对齐求解初始化参数（松耦合）。（视觉系统获取的深度是伪深度，与现实世界差一个尺度，所以需要IMU来标定这个尺度。）

用 SFM求解滑动窗口内所有帧的位姿（以第一帧作为参考坐标系）和所有路标点的 3D 位置。然后将 SFM 的结果与 IMU 预积分的值进行对齐，实现对陀螺仪偏置的校正，再求解每一帧对应的速度，求解重力向量方向，恢复单目相机的尺度因子。

3.后端非线性优化：VIO 紧耦合方案的主要思路就是通过将基于视觉构造的残差项和基于 IMU 构造的残差项放在一起构造成一个联合优化的问题，整个优化问题的最优解即可认为是比较准确的状态估计。

基于滑动窗的紧耦合后端非线性优化：将视觉约束、IMU约束、闭环约束放到一个大的目标函数中进行非线性优化，求解出滑动窗口中所有帧的PVQ、bias等。

在视觉约束和IMU约束中，基本思想是找到优化状态向量，然后通过视觉残差和IMU测量残差分别对状态向量求导，获得视觉和IMU预积分的Jacobian和协方差矩阵。

4.闭环优化：VINS 是采用 BRIEF 描述子的 DBoW2 词袋进行闭环检测。因为前端识别的 Harris 角点数量通常只有 70 个(VINS-Mobile),对于闭环检测远远不够,因此会对新来的 KeyFrame 即后端非线性优化刚处理完的关键帧,再重新检测出 500 个 FAST 角点进行闭环检测用,同时对所有新老角点进行 BRIEF 描述。

然后,计算当前帧与词袋的相似度分数,并与关键帧数据库中所有帧进行对比，并进行闭环一致性检测,获得闭环的候选帧。当检测到闭环后,我们利用 BRIEF 描述子对闭环对的老帧500个FAST 角点,和当前帧中的 70个Harris角点进行邻域匹配,然后对匹配点对利用求基础矩阵对异常点进行 RANSAC 排除。当匹配点超过阈值我们则认为该候选帧是一个正确的闭 环帧 。

当后端优化完成后,会将滑窗内的次新帧进行闭环检测,即首先提取新角点并进行描述,然后与数据库进行检索,寻找闭环帧,并将该帧添加到数据库中。当检测到闭环帧后,会将闭环约束添加到后端的整体目标函数中进行非线性优化,得到第 i 帧(注意这里的帧为闭环帧中的老帧)经过滑窗优化后的位姿,这时可以直接根据计算结果修正滑窗内所有相机的位姿,但是因为此处计算的T i←i_opt 并不准 确 , 因此我们会在检测 到闭环成功后,进行PoseGraph 的四自由度优化,来计算T i←i_opt ,并将此值(r_drift、t_drift)传回给后端优化线程,来更新滑窗内的相机位。

仅供自己学习记录，若有侵权请联系我删除。