ROS-3DSLAM（十一）lvi-sam源代码阅读9

2021SC@SDUSC

（十一）lvi-sam源代码阅读9

前置知识

vins-mono

lvi-sam的视觉部分是基于vins-mono算法的。所以在阅读开始之前学习以下这个算法是很有必要的。

• vins-mono github地址：https://github.com/HKUST-Aerial-Robotics/VINS-Mono

先对整个项目做一个大致的了解，然后重点关注其中的重定位部分

Vins-mono是香港科技大学开源的一个VIO算法，用紧耦合的方法，通过单目+IMU恢复出尺度。

单纯视觉：

• 优点：不产生漂移、直接测量旋转与平移。
• 缺点：尺度不确定性、单目纯旋转无法估计、快速运动易丢失、受图像遮挡运动物体干扰。

单纯IMU：

• 优点：快速相应、可以估计绝对尺度**、角速度估计准确**。
• 缺点：零偏导致漂移、低精度IMU积分位姿发散

结合视觉+IMU： 可用视觉弥补IMU的零偏，减少IMU由于零偏导致的发散和累计误差，IMU可为视觉提供快速g响应的定位。

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

针对lvi-sam项目中我所负责的visual-loop部分，我决定先大致了解VINS整体的流程，然后重点学习以下闭环检测以及闭环优化的部分。

框架概述

1 数据预处理

视觉：

• 提取Harris角点，通过外点剔除后投影到一个单位球面上
• 通过RANSAC（不懂？）去除其中的异常点；
• 确定当前帧是否为关键帧
  • 当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧
  • 当前帧跟踪到的特征点数量小于阈值视为关键帧

IMU：

• 两帧k和k+1之间进行位置、速度、姿态（PVQ）预测；
• 避免每次姿态优化调整后重复IMU传播，采用预积分算法，计算预积分误差的雅克比矩阵和协方差项。

2 初始化

采用松耦合的传感器融合方法得到初始值。首先用SFM进行纯视觉估计滑动窗内所有帧的位姿以及路标点逆深度，然后与IMU预积分对齐，继而恢复对齐尺度s，重力g，imu速度v，和陀螺仪偏置bg。VINS的初始化过程中忽视掉了加速度计的bias，因为加速度计与重力耦合，并且重力向量很大，初始化过程动态过程很短，幅度又不大，加速度计偏置很难观测到。

• 滑动窗口(Sliding Window)纯视觉SfM
• 视觉惯性校准（IMU预积分与视觉结构对齐）
• 重力矢量修正

3 紧耦合后端非线性优化（IMU约束+视觉约束+闭环约束）

初始化后，采用基于滑动窗口的紧耦合单目VIO进行状态估计。

BA优化

4 重定位

尽管滑动窗和边缘化减小了计算复杂度，但是仍旧引进了系统的累计漂移误差。具体来说，就是全局三维位置（xyz）和围绕重力方向的旋转（yaw）。作者采用紧耦合重定位模块与单目VIO进行组合实现漂移误差的消除。
目的：局部滑动窗口移动并与过去的位姿对齐。

vins的重定位模块主要包含回环检测，回环候选帧之间的特征匹配，紧耦合重定位三个部分。

5 全局位姿图优化

这一步是为了确保基于重定位结果对过去的位姿进行全局优化。

由于视觉-惯性 使得横滚角和俯仰角完全可以观测，因此只有（XYZ和yaw航向）4个自由度存在累积漂移。接下来只进行4-DOF的位姿图优化。

重定位和全局位姿图优化部分详解

上图展示了重定位步骤。
1中VIO启动时刻只进行位姿估计（蓝色部分），过去状态一直被记录（绿色部分）；2中如果最新帧中回环被检测到，呈现红色虚线连接，表示启动重定位；3、4中多个特征的多个观测直接用于重定位，从而提高了定位的精度和状态估计的平滑性。5-7是位姿优化。

重定位部分：

A、回环检测（只对关键帧）

1. 采用DBoW2词袋位置识别方法进行回环检测。经过时间空间一致性检验后，DBoW2返回回环检测候选帧。

2. 除了用于单目VIO的角点特征外，还添加了500个角点并使用BRIEF描述子，描述子用作视觉词袋在数据库里进行搜索。这些额外的角点能用来实现更好的回环检测。

3. VINS只保留所有用于特征检索的BRIEF描述子，丢弃原始图像以减小内存。

4. 单目VIO可以观测到滚动和俯仰角，VINS并不需要依赖旋转不变性。

B、回环候选帧之间的特征匹配

1. 检测到回环时，通过BRIEF描述子匹配找到对应关系。但是直接的描述子匹配会导致很多外点。

2. 本文提出两步几何剔除法：

   1. 2D-2D：使用RANSAC进行F矩阵测试，
   2. 3D-2D：使用RANSAC进行PnP，基于已知的滑动窗特征点的3D位置，和回路闭合候选处图像的2D观测（像素坐标）。

当内点超过一定阈值时，我们将该候选帧视为正确的循环检测并执行重定位。

C、紧耦合重定位

1. 重定位过程使单目VIO维持的当前滑动窗口与过去的位姿图对齐。

2. 将所有回环帧的位姿作为常量，利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口。

• 和之前VIO优化模型不同的是，增加了回环项，从位姿图获得回环帧的姿态被视为常数。在重定位之后（重定位只是基于检测到的回环处暂时对滑动窗里的位姿进行重新优化）下一步要对过去位姿和闭合回路图像帧的全局优化。

全局位姿图优化部分：

A、位姿图中添加关键帧

当一个关键帧被滑动窗口中边缘化掉后，它会被添加到位姿图中。该关键帧会作为位姿图中一个定点，通过下面两类边与其他顶点相连接：

1. 顺序边（Sequential Edge）：关键帧将建立与之前关键帧的几个顺序边，一个顺序边表示局部滑动窗口中两个关键帧之间的相对转换，它的值直接从VIO中获取。令最新边缘化掉的关键帧为i，它的一个以前的关键帧为j，顺序边只包含相对位置和相对航向。

   

2. 回路闭合边（Loop Closure Edge）： 如果最新的边缘化掉的关键帧存在回路连接，它可以通过位姿图中的回路闭合边和回路闭合帧相连接。回环边的值由重定位结果得出。

B、4自由度位姿图优化

关键帧i和j之间的残差最小化表示为：

通过最小化以下代价函数，对顺序边和回环边的整个图进行优化：

S是所有顺序边的集合，L是回环边的集合。 尽管紧耦合的重定位已经有助于消除错误的回环，但我们添加了另一个Huber范数 ρ(·)，以进一步减少任何可能的错误回环的影响。相反，我们不对顺序边使用任何鲁棒范数，因为这些边是从VIO中提取出来的，VIO已经包含了足够多的外点排除机制。
位姿图优化和重定位(VII-C)异步运行在两个独立的线程中。以便在需要重定位时，能立即使用最优化的位姿图。同样，即使当前的位姿图优化尚未完成，仍然可以使用现有的位姿图配置进行重新定位。

C、位姿图管理

随着行程距离的增加，位姿图的大小可能会无限增长，从而限制了长时间系统的实时性。为此，我们实行了一个下采样过程：将位姿图数据库保持在有限的大小。**所有具有回环约束的关键帧都将被保留，而其他与相邻帧过近或方向非常相似的关键帧可能会被删除。**关键帧被移除的概率和其相邻帧的空间密度成正比。

其它

在学习过程中的很多概念，如Harris角点、RANSAC、BRIEF描述子等我还不是很清楚他们具体是什么，但是暂时这并不影响对项目代码的阅读；而且这些概念在SLAM14讲的视觉里程计和后端部分都有讲述，所以决定接下来继续开展代码的阅读，并在之间穿插SLAM14讲的学习

参考博客

概述：

https://blog.csdn.net/hltt3838/article/details/105338599

https://zhuanlan.zhihu.com/p/48728586

https://blog.csdn.net/xiaojinger_123/article/details/118895186

重定位和位姿优化部分：

https://blog.csdn.net/try_again_later/article/details/105036445

https://blog.csdn.net/hltt3838/article/details/109455065

以及其他组员的博客：

https://blog.csdn.net/m0_49088083/article/details/121163084

https://blog.csdn.net/loyer_kong/article/details/121320957?spm=1001.2014.3001.5501