VINS 详解

VINS是视觉与IMU融合SLAM的代表，其实现了一个较为完整的SLAM工作，开源地址为：GitHub - HKUST-Aerial-Robotics/VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

优点：

缺点：

预知识储备

1、标定

1. IMU内参标定
   SLAM+语音机器人DIY系列：（三）感知与大脑——2.带自校准九轴数据融合IMU惯性传感器 - 小虎哥哥爱学习 - 博客园
2. 单目相机、双目相机标定
   （超详细）张正友标定法原理及公式推导 - it610.com
3. IMU与单目相机外参标定
   VINS在线标定原理：
   Tong Q , Shen S . Online Temporal Calibration for Monocular Visual-Inertial Systems[J]. IEEE, 2018.
   kalibr-IMU 相机标定原理：
   Furgale P , Rehder J , Siegwart R . Unified temporal and spatial calibration for multi-sensor systems[C]// IEEE/RSJ International Conference on Intelligent Robots & Systems. IEEE, 2013.

• IMU数据噪声模型：

        加速度测量值 = 加速度真值 + 加速度计零偏（由于工艺、温度等导致的偏差值） + R*重力加速度常数（当地的地心引力方向与IMU内部坐标轴间的偏差） + 高斯噪声

        角速度测量值 = 角速度真值 + 零偏 + 高斯噪声

 思路：

• 相机借助标定板通过3D-2D投影关系，得到自身离散位姿信息，接着通过样条曲线（B-spline）得到连续的相机位姿信息。
• 接着预设一个IMU与相机之间的外参矩阵，得到IMU的估计位姿信息。
• IMU运动轨迹分解出平移向量，并对其求两次导得到某个时刻IMU的加速度。再将其运动轨迹分解出旋转变换，并对时间求导，得到某个时刻的角速度。
• 将上述IMU估计值与实际IMU观测值进行比较，并优化求解得到外参、IMU与相机离散采样延迟时间、相机内参以及IMU内参（轴偏差、尺度偏差、零篇误差）优化值。

2、融合方法

1. 松耦合：图像经过单独处理得到视觉里程计，然后再将视觉里程计与IMU组合成一个状态向量，送入EKF滤波器进行更新。复杂度低
2. 紧耦合：图像中提取出的特征点直接作为观测数据与IMU观测数据一起送入EKF进行融合更新。精度高

优化方法：ORBSLAM3、V INS等，当IMU紧耦合到视觉SLAM中时，机器人位姿和路标同样作为带估量，由图结构中的节点表示，只是观测约束除了视觉还有IMU。

 引自：张虎 机器人SLAM导航核心技术与实践

3、KLT光流法

手撕自动驾驶算法——KLT 光流算法_令狐少侠、的博客-CSDN博客_klt光流

 VINS功能模块：数据预处理、初始化、后端非线性优化、闭环检测以及优化

• 图像和IMU预处理：
  • 图像：提取图像 Harris 角点，利用金字塔光流跟踪相邻帧，通过 RANSAC 去除异常点，最后将跟踪到的特征点 push 到图像队列中，并通知后端进行处理。
  • IMU：将 IMU 数据进行积分，得到当前时刻的位置、速度和旋转（PVQ），同时计算在后端优化中将用到的相邻帧的预积分增量，及预积分误差的 Jacobian 矩阵和协方差项​​​​
• 初始化：
  • 利用 SFM 进行纯视觉估计滑窗内所有帧的位姿及 3D 点逆深度，最后与 IMU 预
    积分进行对齐求解初始化参数
• 后端滑窗优化：
  • ​​​​​​​​​​​​​​将视觉约束、IMU 约束和闭环约束放在一个大的目标函数中进行非线性优化，求解滑窗内所有帧的 PVQ、bias 等。 
• 闭环检测及优化：
  • ​​​​​​​​​​​​​​​​​​​​​​​​​​​​利用 DBoW 进行闭环检测，当检测成功后进行重定位，最后对整个相机轨迹进行闭环优化。

 VINS四个线程：前端图像跟踪、后端非线性优化（初始化以及IMU预积分都在该线程）、闭环检测、闭环优化

算法流程：

（1）预处理模块

 输入：相机图片流以及IMU位姿信息。

 输出：跟踪到的特征点（有效点）、关键帧

            1）IMU积分，得到PVQ位置、速度、旋转

            2）计算在后端优化中将用到的相邻帧的预积分增量

            3）计算预积分误差的Jacobian矩阵和协方差。

（2）初始化模块

视觉部分（与ORBSLAM2类似）：

• 连续图像帧需有足够大的视差，以及足够多的稳定共视点，使用ORB-SLAM2初始化方法求解相邻帧之间的旋转以及平移关系。
• 三角化求解地图云点。
• BA优化地图云点以及相机位姿。

视觉与IMU对齐：

（3） VIO融合模块

VINS的整个局部建图就是在VIO融合模块中完成的，以IMU观测以及相机观测作为约束，进行BA优化。需要注意的是VINS会采用滑窗的方式剔除旧节点，以保证滑窗内的节点的数量，但是在图优化过程中，约束边还是会使用旧的约束条件的。

（4）全局优化模块

与ORBSLAM2一样采用DBoW2词袋模型进行闭环检测和全局优化。添加约束至位姿图优化中。

嗨害海！小Tips：

• Linux设置每 0.01s 显示一次显存和算力的情况：      watch -n 0.01 nvidia-smi 
• 卡尔曼滤波宏观意义上：就是求出估计值以及测量值各占的权重。
• 边缘化：浅谈SLAM边缘化 - 知乎
• VINS是针对滑动窗口中的关键帧进行BA，为了保证滑动窗口中的关键帧数量限制在窗口的最大数量之内（VINS中取的是十个关键帧），当有新的关键帧进来的时候需要对之前的关键帧丢掉，但由于老的关键帧可能还会与窗口中的关键帧存在相机的观测约束，不能直接丢掉，所以丢掉的策略是对老的关键帧进行边缘化操作。

---

• VINS-MONO中的边缘化？

​​​​​​​（ ​​​​​​​​​​​​​​多传感器融合定位理论基础（十二）：滑动窗口原理及应用 - 知乎）这个写的蛮好，通俗易懂。

​​​​​​​        VINS根据次新帧是否为关键帧分为两种边缘化策略（marginalization_factor.cpp）：通过对比次新帧和次次新帧的视差量，来决定边缘化掉次新帧或者最老帧 。（这样做是为了不再计算这一帧的位姿或者与其相关的云点，但是希望保留该帧对窗口内其他帧的约束关系）

• 当次新帧为关键帧时，将边缘化掉最老帧，及其看到的路标点和相关联的 IMU 数据，将其转化为先验信息加到整体的目标函数中。
• 当 次新帧不是关键帧时 ，我们将 直接扔掉次新帧及它的视觉观测边 ，而不对次新帧进行边缘化 ，因为我们认为当前帧和次新帧很相似， 也就是说当前帧跟路标点之间的约束和次新帧与路标点的约束很接近 ，直接丢弃并不会造成整个约束关系丢失过多信息。但是值得注意的是，我们 要保留次新帧的 IMU 数据，从而保证 IMU 预积分的连贯性。

边缘化：

通俗来讲就是通过左乘一个矩阵，简化参数矩阵。但是并未丢失任何约束，因此也不会丢失信息。

 （a）

 （b）

 （c）将边缘化后的H矩阵

 (d)边缘化掉后的H矩阵

 ​​​​​​​

• ---

  VINS-MONO的边缘化过程用概率模型怎么描述？

• ---

  ORBSLAM3与VINS的区别

VIO 初始化系列3------ORB-SLAM3 与VINS-Mono初始化比较_Flying Youth的博客-CSDN博客_orbslam3与vinsfusion对比

• VINS-MONO有个边缘化的操作，为什么ORB-SLAM没有边缘化这样的操作，它是怎样不做边缘化来达到VINS-MONO的效果的？

​​​​​​​​​​​​​​        ORB-SLAM中地图点剔除机制和共视图选取local mapping大小的机制，都保证与被剔除的点和关键帧相关的协方差矩阵块比较稀疏。所以直接丢弃它们，可能对优化结果影响很小。

ORB-SLAM在进行局部BA时为什么没有进行边缘化思考_留白君的博客-CSDN博客_orbslam 边缘化

• ---

  Vins里面时间戳是如何对齐的？

​​​​​​​在标定时就会标定出IMU与相机之间采样延迟时间d。

• Vins里面重力是如何对齐的？

​​​​​​​​​​​​​​VINS初始化：视觉惯性对齐 - 知乎

• Vins是如何初始化的？

​​​​​​​​​​​​​​VINS——初始化过程_雨luo凡城的博客-CSDN博客_vins 初始化

第七章——VINS系统初始化_阿银的万事屋的博客-CSDN博客_vins初始化

• ---

  ​​​​​​​VINS前端流程?

      ​​​​​​​​​​​​​​对图像使用KLT角点检测算法，对角点使用LK光流（在金字塔上逐层进行）追踪(用光流计算最小化光度误差来进行追踪)。对已有特征点进行追踪时，不需要在新一帧图像上提取特征点,但当特征点数较少时，会额外提取特征点来维持一定的数量。每张图片维护大约100-300个特征点。检测出特征点时还会进行均匀化操作（对应于ORB:使用的是四叉树，对应于VINS：每检测出特征点，对特征点进行排序，认为被追踪的越久的特征点稳定性更好，质量更高。以特征点为中心画圈，在圈中不允许再有其他的特征点存在）。提取完特征点后要去畸变，去畸变使用了一个trick：逐渐逼近式去畸变。通过对极约束求一个本质矩阵，使用RANSAC选取一个置信度最高的本质矩阵，对outlier（不符合对极约束的点）进行筛查。

---

• ​​​​​​​ORB与VINS的区别？

        前端：

                ORB-SLAM：构建图像金字塔，对每一层提取Fast特征点，使用四叉树进行均匀化。对每一层图像进行高斯模糊，然后提取BRIEF描述子。再恢复到第0层并使用opencv自带函数进行图像去畸变；初始化过程通过计算H和F矩阵得分恢复初始化位姿和地图点；没有运动模型时先使用关键帧追踪，之后主要使用恒速运动模型追踪

                VINS：对图像使用KLT角点检测算法，对角点使用LK光流（在金字塔上逐层进行）追踪。均匀化操作是对检测出的特征点进行排序，以特征点为中心画圈，在圈中不允许再有其他的特征点存在。提取完特征点后要去畸变，去畸变使用了一个trick：逐渐逼近式去畸变。通过对极约束求一个本质矩阵，使用RANSAC选取一个置信度最高的本质矩阵，对outlier（不符合对极约束的点）进行筛查。另外vins涉及到了IMU，需要进行IMU的预积分和初始化

        后端：

                ORB：主要基于共视关键帧，优化变量：地图点坐标，关键帧和一级相邻关键帧的位姿，残差：视觉残差

                VINS：基于滑动窗口，优化变量为： 相机到IMU的外参，滑窗中地图点的逆深度，滑窗中所有IMU的状态（P,V,Q,ba,bw），残差：边缘化先验残差，IMU残差，视觉残差；对移除滑窗图像帧信息进行边缘化，将丢弃的信息保留下来继续约束剩余变量。

        回环：

                两个方法的回环都利用了词袋，vins对角点额外计算了BRIEF描述子。

                另外VINs对外参3自由度的平移和1自由度的yaw角进行优化。

---

 部分摘自：
1、崔华坤 VINS论文推导及代码解析
2、张虎 机器人SLAM导航核心技术与实践