经典文献阅读之--FAST-LIVO（Fast-LIO系列的激光视觉惯性里程计）

0. 简介

在此之前博主对Fast-LIO2和R3LIVE进行了系统性的学习。最近文章《Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry》提出了FAST-LIVO，这是一种快速LiDAR惯性-视觉里程计系统，它建立在两个紧耦合的直接里程计子系统之上：VIO子系统和LIO子系统。LIO子系统将新扫描的原始点(而不是边缘或平面上的特征点)添加到增量构建的点云地图中。这些地图点还额外关联了图像块，VIO子系统使用点云地图附加的图像块，在不提取任何视觉特征(例如ORB或FAST角点特征)的情况下最小化直接光度误差，以此来对齐新图像。 为了进一步提高vio系统的鲁棒性和准确性，作者提出了一种新的方法来剔除边缘或者在视觉中遮挡的地图点。 这里的代码也已经完成了开源，在Github上可以找到。

1. 文章贡献

在机器人技术中使用的传感器中，相机、激光雷达和惯性测量单元(imu)可能是SLAM任务中使用最广泛的传感器。几个最近的激光雷达惯性视觉里程测量系统(LIVO)。它们联合融合状态向量，但分别处理每个数据，而不考虑它们的测量级耦合。这样的系统通常会占用大量的计算资源。为了解决这一问题，我们提出了一种快速、紧密耦合的稀疏直接激光雷达惯性视觉测程系统(fast - livo)，结合稀疏直接图像对齐和直接原始点配准的优点，在降低计算成本的情况下实现准确可靠的姿态估计。本文的贡献如下:：

1. 一个紧凑的lidar惯性视觉里程计框架，它建立在两个直接且紧密耦合的里程计系统上:LIO子系统和VIO子系统。这两个子系统通过将各自的激光雷达或可视化数据与imu融合，共同估计系统状态。
2. 一个直接高效的VIO子系统，最大限度地重用LIO子系统中构建的点云图。具体来说，地图中的点会与之前观测到的图像块关联，通过最小化直接光度误差，将点投影到新图像上来对齐其姿态（因此是完整的系统状态）。VIO子系统中的LiDAR点云重用避免了视觉特征的提取，三角化以及优化，并在测量层面上将两个传感器耦合起来。
3. 本系统实现了开源，可以在Intel或ARM处理器上实时运行，并支持多线旋转激光雷达和具有完全不同扫描模式的新兴固态激光雷达。
4. 在开放数据序列(NTU-VIRAL)和我们定制的设备数据上验证开发的系统。结果表明，该系统能够以较低的计算成本处理具有挑战性的传感器退化环境。

2. 详细内容

首先我们来看一下整个框架，本文采用表1中的定义方法。

我们的系统概述如图1所示，其中包括LIO子系统(蓝色部分)和VIO子系统(红色部分)两个子系统。LIO子系统首先通过后向传播[24]对激光雷达扫描中的运动失真进行补偿，然后计算帧-映射点-平面残差。类似地，VIO子系统从视觉全局地图中提取当前FoV中的视觉子地图，并丢弃子地图中的离群点(被遮挡或深度不连续的点)。然后，进行稀疏直接视觉对齐以计算帧到地图的图像光度误差。在误差状态迭代卡尔曼滤波器中，激光雷达点面残差和图像光度误差与IMU传播紧密融合。融合后的姿态用于在全局地图上添加新的点。

2.1 boxplus“$⊞$”和boxminus“$\boxminus$”操作符

在本节中，我们使用“$⊞$”和“$\boxminus$”运算来表示流形$\mathcal{M}$上的状态误差，对于本文考虑的$\mathcal{M}=SO(3)\times {\mathbb{R}}^n$，我们有:

其中$r\in {\mathbb{R}}^3,a,b\in {\mathbb{R}}^n$, $Exp(\cdot )$和$Log(\cdot )$表示由Rodrigues公式3导出的旋转矩阵与旋转向量之间的双向映射。

2.2 状态转移模型

在我们的系统中，我们假设三个传感器(LiDAR, IMU和摄像头)之间的时间偏移是已知的，可以提前校准或同步。我们将IMU框架(设为$I$)作为主体框架，将第一主体框架作为全局框架(设为$G$)。此外，我们假设三个传感器刚性连接在一起，并对表I中定义的外部元件进行了预校准。那么，第i次IMU测量时的离散状态转移模型:

其中$∆t$为IMU采样周期，状态$x$、输入$u$、过程噪声$w$、函数$f$定义如下:

其中${}^G{R}_I$和${}^G{p}_I$表示IMU在全局坐标系中的姿态和位置，${}^{G}g$为全局坐标系中的重力矢量，${\omega }_m$和$a_m$为IMU的原始测量值，$n_g$和$n_a$为${\omega }_m$和$a_m$中的测量噪声，$b_a$和$b_g$为IMU偏差，分别建模为高斯噪声$n_{bg}$和$n_{ba}$驱动下的随机游走

2.3 Frame-to-map 测量模型

文中的前向传播和误差传播Fast-Lio2类似，这里就不讲了，我们直接看一下。

1. LiDAR测量模型：如果在tk时刻接收到激光雷达扫描，我们首先进行[24]中提出的反向传播来补偿运动失真。${}^L{P}_j$是去除运动畸变后的点云。我们假设每个点都位于地图中的一个临近平面上，使用预测状态将点云变换到世界系后，利用ikdtree找到临近的五个点计算法向量$u_j$和中心点$q_j$。如果使用真值状态(比如姿态) $X_k$将LiDAR局部坐标系中表示的测量${}^L{P}_j$转换到全局坐标系中，则残差应为零：

…详情请参照古月居