【论文】VIRAL SLAM: Tightly Coupled Camera-IMU-UWB-Lidar SLAM

这里是引用

Abstract

这是一个基于视觉惯性雷达UWB紧耦合的SLAM系统。

这篇论文有两个创新点：
1）雷达相机IMU数据都是在里程计的滑动窗口核心线程中处理的
2）所有的传感器都是紧耦合的。

I. INTRODUCTION

对于多传感器融合的雷达和相机融合的重要运用方式是，相机可以很精确地辨别回环，而雷达可以给相机的特征点提供准确的深度信息。

SLAM系统中融合进UWB，最重要的是估计SLAM的坐标L和UWB锚点坐标W之间的转化关系。

论文主要贡献：
1）提出了一个完整的相机雷达imuUWB的紧耦合融合方式。
2）提出了一个让相机特征点和雷达点云匹配融合的方法（MMM）。
3）提出了一个新的回环检测机制。这个方法是受到了特征点回环检测和两步点云对齐的启发。
4）提出了一个新的UWB融合机制，估计锚点和bias范围，并将他们传到BA中进行优化。同时估计值在滑动窗口优化中是固定不变的。
5）做了很多实现与SOTA方法做比较。

II. RELATED WORKS

说了很多的方法都没有像本文一样使用多传感器紧耦合的优化，在低纹理的情况下就很容易会发生严重偏移。

介绍了一点自己之前UWB与雷达紧耦合的论文。本文则在自己之前工作的基础上加入了视觉的紧耦合优化。

III. PRELIMINARIES

A. Coordinate frames

在本文中定义了两个坐标系。第一个是local frame L，这个坐标系是在飞机初始化的时候依据自身的位置创建的。另一个是固定的锚点坐标系w，是UWB的其中一个基站坐标。

B. State estimates

定义了一个滑动窗口，滑动窗口内需要优化的变量包括pvq 和 imu的角速度bias加速度bias。
同时对于特征点的深度也会进行优化。
UWB位置：

T表示的世界坐标w和无人机自身的L之间的变换矩阵。
$b^r$是一个UWB的bias，UWB相较于飞机摆放的位置会影响这个bias的大小。

C. Synchronization

以雷达的CFC事件开始为基准，CFC的时间确定了滑动窗口的时间。
UWB的是将在时间间隔中的信息组成一个group。
对于相机的延迟时间，将会用于补偿在图片中提取特征点的时间，也就是说相机和CFC是默认同步的。

D. System overview

大多数的时间开销都是用在了传感器数据的同步和处理生成代价函数的过程上了。

IV. REAL-TIME LOCALIZATION FUNCTION BLOCKS

A. Local sliding window optimization

滑动窗口优化的代价函数也是和其他一样，将所有的残差相加。

B. Sensor data processing

1. Lidar & IMU:
   与上篇论文一样，我还没有看：

2. UWB：
   
   每一个UWB采样的所包含的信息，i表示UWB的包下标，d表示与世界坐标W原点距离，x表示世界坐标W的原点在飞机坐标L下的位置，y表示UAV没看懂，r是时间时间戳，tm-1和tm是时间范围。
   
   没看懂的地方（这里我感觉是不是公式错了），pm是使用VIO计算出来的飞机起始坐标L下的m时刻位置，$y^i$是UWB接收器到VIO模组的距离。

总的来说公式7就是 世界坐标原点到UAV起始坐标L原点 + L到飞机VIO模块 + VIO模块到UWB接收器 + UAV滑动窗口m时刻到滑动窗口m中的i时刻的飞行距离 = UWB到UAV上的UWB接收器的距离。

3. Camera:
   冲投影误差。
   

V. GLOBAL OPTIMIZATION BLOCKS

A. Key frame management

与【17】论文的方法类似，他的简而言之我没看懂

MMM算法

还是没看懂但觉得挺有用的，需要时候再来推吧。。。

2. Key frame selection:

关键帧的选择需要FMM算法没看懂，应该也是【17】论文中的东西。
关键帧的选择是优化之前的，选择是基于IMU的推倒位姿。

关键帧是Ka，Kb，Kc三个帧集合的并集。
Ka：滑动窗口中的上M个关键帧 。
Kb： 是k时刻最临近的M个关键帧。
Kc： 是以K时刻为圆心花一个2m * 2m * 2m 的方格中的关键帧。

B. Bundle Adjustment

第一个残差是位姿图的残差优化，第二个是回环检测出现的残差优化，第三个和前面的UWB残差是一样的。

第三个UWB残差：

值得注意的是，为了确保位姿图有足够的信息，UWB的残差在一开始是不会直接进入BA的，直到满足特定的条件：

会计算上面𝚪的奇异值，直到三个奇异值满足𝜎_1 < c1同时 𝜎1/𝜎3 < c2，这里的c1和c2都是大于1的值。我们预设的。

C. Loop Closure

为了构建回环检测，本文进行了三步走：
1）当有新的帧到来时候，会利用BoW进行一次回环检测，找到对应的Tp和Tc（当前帧），便利Tp前后的的关键帧，构出loacal Mp，然后利用这些关键帧已经被边缘化的点云图做第二步操作。
2）利用ICP算法 来对齐当前帧的点云与上面的BpMp 图，对齐过程中得到猜测的（p和c点之间的变换矩阵）。如果拟合的得分低于阈值，将会执行第三步。
3）利用FMM算法将当前帧的点云图 和BpMp来计算出FMM参数，然后构建下面的公式：

优化他来获得c和p之间的相对位姿，如果$f{(}_{B_p}^{B_c}T)/N_L^c$小于阈值的话，就啊将此时的${}_{B_p}^{B_c}T$作为回环的先验T。