Fig. 1. System overview of FAST-LIVO
| Notations | Meaning |
| $^{G}(\cdot)$ | The vector ( $\cdot$) in global frame. |
| $^{C}(\cdot)$ | The vector ( $\cdot$) in camera frame. |
| $^{I}T_{L}$ | The extrinsic of LiDAR frame w.r.t. IMU frame. |
| $^{I}T_{C}$ | The extrinsic of camera frame w.r.t. IMU frame. |
| $\mathbf{x}$, $\mathbf{\hat{x}}$, $\mathbf{\bar{x}}$ | The ground-truth, predicted and updated estimation of $\mathbf{x}$. |
| $\mathbf{\delta}\mathbf{x}$ | The error state between ground-truth $\mathbf{x}$ and its estimation. |
以及后续的滑动窗口优化。与这些方法相比，我们提出的FAST-LIVO直接使用原始激光雷达点和图像像素分别跟踪激光雷达扫描和图像，无需提取任何特征，因此具有更高的计算效率。
我们FAST-LIVO中的VIO子系统与DVL-SLAM [^28] [^29]最为相似，它会将激光雷达点投影到新图像中，并通过最小化直接光度误差来跟踪该图像。不过，DVL-SLAM仅进行帧间图像对齐，既不会利用惯性测量单元数据，也不进行激光雷达扫描配准。相比之下，我们的FAST-LIVO系统将帧到地图的图像对齐、激光雷达扫描配准以及惯性测量单元数据紧密耦合在统一的卡尔曼滤波器中。我们还提出了一种可靠且稳健的异常值剔除方案，用于在将激光雷达测量值投影到图像平面时应用，从而进一步提升系统精度。
### 三、系统概述
本文采用的符号说明见表I。图1展示了我们系统的整体架构，包含两大子系统：LIO子系统（蓝色部分）和VIO子系统（红色部分）。LIO子系统首先通过反向传播[^30]来补偿激光雷达扫描中的运动畸变，随后计算帧到地图的点面残差。类似地，VIO子系统会从视觉全局地图中提取当前视场内的视觉子图，并剔除子图中的异常值（即被遮挡或存在深度不连续性的点）。之后，系统会执行稀疏直接视觉对齐，以计算帧到地图的图像光度误差。激光雷达点面残差与图像光度误差会与惯性测量单元的传播结果一起，在误差状态迭代卡尔曼滤波器中被紧密融合。融合得到的位姿会被用来向全局地图中添加新点。
### 四、状态估计
FAST-LIVO的状态估计采用紧密耦合的误差状态迭代卡尔曼滤波器（ESIKF），可融合来自激光雷达、相机和惯性测量单元的测量数据。
---
此处我们主要介绍系统模型（状态转移模型和测量模型）。关于流形上迭代卡尔曼滤波器的详细结构与实现，读者可参考[^31]。
### A. boxplus“⊕”与boxminus“□”运算符
在本节中，我们使用“⊓”和“∩”运算来表示流形上的状态误差$\mathcal{M}$。具体而言，对于本文所考虑的$\mathcal{M} = SO(3) \times \mathbb{R}^n$，有：
$$ \begin{bmatrix}\mathbf{R}\\ \mathbf{a}\end{bmatrix}\boxplus\begin{bmatrix}\mathbf{r}\\ \mathbf{b}\end{bmatrix}\triangleq\begin{bmatrix}\mathbf{R}\cdot\operatorname{Exp}(\mathbf{r})\\ \mathbf{a}+\mathbf{b}\end{bmatrix},\begin{bmatrix}\mathbf{R}_{1}\\ \mathbf{a}\end{bmatrix}\boxminus\begin{bmatrix}\mathbf{R}_{2}\\ \mathbf{b}\end{bmatrix}\triangleq\begin{bmatrix}\operatorname{Log}(\mathbf{R}_{2}^{T}\mathbf{R}_{1})\\ \mathbf{a}-\mathbf{b}\end{bmatrix} $$
其中$\mathbf{r} \in \mathbb{R}^3$、$\mathbf{a}, \mathbf{b} \in \mathbb{R}^n$、$\text{Exp}(\cdot)$和$\text{Log}(\cdot)$代表根据罗德里格斯公式$^3$推导出的旋转矩阵与旋转向量之间的双向映射。
### B. 状态转移模型
在我们的系统中，假设三种传感器（激光雷达、惯性测量单元和相机）之间的时间偏移是已知的，这些偏移可以提前校准或同步。我们以惯性测量单元坐标系（记为I）作为机体坐标系，将第一个机体坐标系作为全局坐标系（记为G）。此外，我们假设这三种传感器被刚性连接在一起，表I中所定义的外参也已预先校准。那么，在第i次惯性测量时的离散状态转移模型为：
$$ \mathbf{x}_{i+1}=\mathbf{x}_{i}\boxplus\left(\Delta t\mathbf{f}\left(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i}\right)\right) $$
其中$\Delta t$为惯性测量单元的采样周期，状态x、输入u、过程噪声w以及函数f的定义如下：
$$ \mathcal{M}\triangleq S O(3)\times\mathbb{R}^{15},~\operatorname{d i m}(\mathcal{M})=18 $$
$$ \mathbf{x}\triangleq\left[\begin{matrix}{^{G}\mathbf{R}_{I}^{T}}&{^{G}\mathbf{p}_{I}^{T}}&{^{G}\mathbf{v}^{T}}&{\mathbf{b}_{\mathbf{g}}^{T}}&{\mathbf{b}_{\mathbf{a}}^{T}}&{^{G}\mathbf{g}^{T}}\\ \end{matrix}\right]^{T}\in\mathcal{M} $$
$$ \mathbf{u}\triangleq\left[\begin{matrix}{\mathbf{\omega}_{m}^{T}}&{\mathbf{a}_{m}^{T}}\\ \end{matrix}\right]^{T},\mathbf{w}\triangleq\left[\begin{matrix}{\mathbf{n_{g}}^{T}}&{\mathbf{n_{a}}^{T}}&{\mathbf{n_{b g}}^{T}}&{\mathbf{n_{b a}}^{T}}\\ \end{matrix}\right]^{T} $$
$$ \mathbf{f}(\mathbf{x},\mathbf{u},\mathbf{w})=\left[\begin{matrix}{\mathbf{\omega}_{m}-\mathbf{b_{g}}-\mathbf{n_{g}}}\\ {^{G}\mathbf{v}+\frac{1}{2}(^{G}\mathbf{R}_{I}\left(\mathbf{a}_{m}-\mathbf{b_{a}}-\mathbf{n_{a}}\right)+^{G}\mathbf{g})\Delta t}\\ {^{G}\mathbf{R}_{I}\left(\mathbf{a}_{m}-\mathbf{b_{a}}-\mathbf{n_{a}}\right)+^{G}\mathbf{g}}\\ {\mathbf{n_{b_{g}}}}\\ {\mathbf{n_{b_{a}}}}\\ {\mathbf{0}_{3\times1}}\\ \end{matrix}\right]\in\mathbb{R}^{18} $$
其中，$^G\mathbf{R}_I$ 和 $^G\mathbf{p}_I$ 分别表示全局坐标系下的IMU姿态和位置，$^G\mathbf{g}$ 是全局坐标系下的重力矢量，$\omega_m$ 和 $\mathbf{a}_m$ 为IMU的原始测量值，$\mathbf{n}_g$ 和 $\mathbf{n}_a$ 分别是 $\omega_m$ 和 $\mathbf{a}_m$ 中的测量噪声，$\mathbf{b}_a$ 和 $\mathbf{b}_g$ 为IMU偏差，它们被建模为由高斯噪声 $\mathbf{n}_{bg}$ 和 $\mathbf{n}_{ba}$ 驱动的随机游走过程。
### C. 前向传播
我们利用前向传播来预测每个IMU输入时刻 $\hat{\mathbf{x}}_{i+1}$ 下的状态 $\hat{\mathbf{P}}_{i+1}$ 及其协方差 $\mathbf{u}_i$。更具体地说，通过将公式(1)中的过程噪声 $\mathbf{w}_i$ 设为零来实现状态的传播：
$$ \begin{array}{r}{\hat{\mathbf{x}}_{i+1}=\hat{\mathbf{x}}_{i}\boxplus\left(\Delta t\mathbf{f}(\hat{\mathbf{x}}_{i},\mathbf{u}_{i},\mathbf{0})\right).}\end{array} $$
$^{3}$ 的协方差按如下方式传播：
$$ \begin{aligned}&\hat{\mathbf{P}}_{i+1}=\mathbf{F}_{\hat{\boldsymbol{\delta}}\hat{\mathbf{x}}}\hat{\mathbf{P}}_{i}\mathbf{F}_{\hat{\boldsymbol{\delta}}\hat{\mathbf{x}}}^{T}+\mathbf{F}_{\mathbf{w}}\mathbf{Q}\mathbf{F}_{\mathbf{w}}^{T}\tag{7}\\&\mathbf{F}_{\hat{\boldsymbol{\delta}}\hat{\mathbf{x}}}=\left.\frac{\partial\hat{\boldsymbol{\delta}}\hat{\mathbf{x}}_{i+1}}{\partial\hat{\boldsymbol{\delta}}\hat{\mathbf{x}}_{i}}\right|_{\hat{\boldsymbol{\delta}}\hat{\mathbf{x}}_{i}=\mathbf{0},\mathbf{w}_{i}=\mathbf{0}},\mathbf{F}_{\mathbf{w}}=\left.\frac{\partial\hat{\boldsymbol{\delta}}\hat{\mathbf{x}}_{i+1}}{\partial\mathbf{w}_{i}}\right|_{\hat{\boldsymbol{\delta}}\hat{\mathbf{x}}_{i}=\mathbf{0},\mathbf{w}_{i}=\mathbf{0}}\\ \end{aligned} $$
其中 $\mathbf{Q}$ 是 $\mathbf{w}$ 的协方差，$\delta\hat{\mathbf{x}}_i \triangleq \mathbf{x}_i \boxminus \hat{\mathbf{x}}_i$，而 $\mathbf{F}_{\delta\hat{\mathbf{x}}}$ 和 $\mathbf{F}_{\mathbf{w}}$ 的具体形式可在 [^11] 和 [^30] 中查看。
状态预测（6）和协方差（7）从接收到最后一次激光雷达或图像测量的时间$t_k$₁开始传播，直到接收到当前激光雷达或图像测量的时间$t_k$为止；在此期间，每次接收到IMU测量数据$u_i$时，也会进行相应更新$t_k$₁和$t_k$。公式（6）和（7）中的初始状态与协方差分别为$\bar{x}_k$₁和$\bar{P}_k$₁，它们是通过融合最后一次激光雷达或图像测量数据得到的（详见第IV-E节）。我们将传播到$t_k$时刻的状态和协方差分别记为$\hat{x}_k$和$\hat{P}_k$。需要注意的是，我们并不假设激光雷达扫描数据和图像是在同一时间接收到的。一旦接收到激光雷达扫描数据或图像，就会如第IV-E节所述对状态进行更新。
### D. 帧到地图的测量模型
1) 激光雷达测量模型：如果在时间 $t_k$ 接收到激光雷达扫描数据，我们首先采用 [^30] 中提出的前向传播方法来补偿运动畸变。扫描中的结果点集 $\{^L\mathbf{p}_j\}$ 可视为在 $t_k$ 时刻同时采样得到，且表示在同一激光雷达局部坐标系L中。在将扫描点 $\{^L\mathbf{p}_j\}$ 配准到地图时，我们假设每个点都位于地图中某个相邻平面上，该平面的法向量为 $\mathbf{u}_j$，中心点为 $\mathbf{q}_j$。也就是说，若利用真实状态（即位姿）$L^L\mathbf{p}_j$ 将激光雷达局部坐标系中测量的点 $\mathbf{x}_k$ 转换到全局坐标系下，残差应当为零：
$$ \mathbf{0}=\mathbf{r}_{l}(\mathbf{x}_{k},{}^{L}\mathbf{p}_{j})=\mathbf{u}_{j}^{T}({}^{G}\mathbf{T}_{I_{k}}{}^{I}\mathbf{T}_{L}{}^{L}\mathbf{p}_{j}-\mathbf{q}_{j}) $$
实际应用中，为了找到相邻平面，我们先利用预测状态中的位姿 $^L\mathbf{p}_j$ 通过 $\hat{\mathbf{x}}_k$ 将 $^G\hat{\mathbf{p}}_j = ^G\hat{\mathbf{T}}_{I_k}^I\mathbf{T}_L^L\mathbf{p}_j$ 转换到全局坐标系下，然后在由增量kd树结构ikd-tree [^17] 组织的激光雷达全局地图中搜索最近的5个点来拟合平面。随后，公式(8)便定义了状态 $\mathbf{x}_k$ 的隐式测量模型。为了考虑 $^L\mathbf{p}_j$ 中的测量噪声，该等式会乘以一个权重因子 $\Sigma_l$。
Fig. 2. Changing the pose of current frame $I_{k}$ in the global frame to minimize the photometric errors between the reference image patches in the visual global map and the corresponding patch in the current frame.
---
2) 稀疏直接视觉对齐测量模型：与[^16]中的帧间图像对齐不同，我们采用稀疏直接帧到地图的图像对齐方式，通过由粗到细的方式最小化光度误差，详见图2。具体而言，如果在时间$t_k$接收到一幅图像，我们会从全局视觉地图中提取位于该图像视场内的地图点$\{G_{\mathbf{p}_i}\}$（参见第V-B.2节）。对于每个地图点$G_{\mathbf{p}_i}$，都关联了在不同先前图像中观测到的图像块（参见第V-B节）；我们选择图像中包含该点的、且与当前图像的观测角度最接近的观测块作为参考图像块（记为$\mathbf{Q}_i$）。同时，将$\mathbf{A}_i$投影到当前图像金字塔时的仿射变换矩阵记为$\mathbf{Q}_i$。接着，利用真实状态（即位姿）$G_{\mathbf{p}_i}$将地图点$\mathbf{I}_k(\cdot)$变换到当前图像$\mathbf{x}_k$上，此时$\mathbf{Q}_i$与当前图像中对应图像块之间的光度误差应为零：
$$ \mathbf{0}=\mathbf{r}_{c}(\mathbf{x}_{k},{}^{G}\mathbf{p}_{i})=\mathbf{I}_{k}(\boldsymbol{\pi}({}^{I}\mathbf{T}_{C}^{-1}{}^{G}\mathbf{T}_{I_{k}}^{-1}{}^{G}\mathbf{p}_{i}))-\mathbf{A}_{i}\mathbf{Q}_{i} $$
其中$\pi(\cdot)$为针孔投影模型。公式(9)定义了针对状态$\mathbf{x}_k$的另一种隐式测量模型，该模型会在三个层级上进行优化（参见第IV-E节）；每个层级中，当前图像和参考图像块都是从上一级别采样得到的。优化从最粗糙的层级开始，某一层级收敛后，优化便进入下一个更精细的层级。为了考虑图像$\mathbf{I}_k$中的测量噪声，该公式会乘以一个权重因子$\Sigma_c$。
### E. 误差状态迭代卡尔曼滤波更新
根据第IV-C节推导出的传播状态$\hat{x}_k$和协方差$\hat{P}_k$，可得到$\mathbf{x}_k$的先验分布如下：
$$ \mathbf{x}_{k}\boxminus\hat{\mathbf{x}}_{k}\sim\mathcal{N}(\mathbf{0},\hat{\mathbf{P}}_{k}). $$
结合公式(10)中的先验分布、公式(8)中的激光雷达测量分布以及公式(9)中的视觉测量分布，我们便可得到$\mathbf{x}_{k}$的最大后验估计（MAP）：
$$ \begin{aligned}\min_{\mathbf{x}_{k}\in\mathcal{M}}&\left(\|\mathbf{x}_{k}\boxminus\hat{\mathbf{x}}_{k}\|_{\hat{\mathbf{p}}_{k}}^{2}+\sum_{j=1}^{m_{l}}\|\mathbf{r}_{l}(\mathbf{x}_{k},^{L}\mathbf{p}_{j})\|_{\boldsymbol{\Sigma}_{l}}^{2}\right.\\&+\sum_{i=1}^{m_{c}}\|\mathbf{r}_{c}(\mathbf{x}_{k},^{G}\mathbf{p}_{i})\|_{\boldsymbol{\Sigma}_{c}}^{2}\end{aligned} $$
其中$\|\mathbf{x}\|_{\Sigma}^2 = \mathbf{x}^T \boldsymbol{\Sigma}^{-1} \mathbf{x}$。注意，如果在$t_k$时刻接收到激光雷达扫描数据，公式(11)仅会将激光雷达残差$\mathbf{r}_l$与惯性测量单元的传播结果（即$m_c = 0$）进行融合。类似地，如果在$t_k$时刻接收到图像，公式(11)仅会将视觉光度误差$\mathbf{r}_c$与惯性测量单元的传播结果（即$m_l = 0$）进行融合。
公式(11)中的优化问题是非凸的，可通过高斯-牛顿法迭代求解。这种迭代优化已被证明等价于迭代卡尔曼滤波[^27]。为了处理流形约束$\mathcal{M}$，在每次优化迭代中，我们会通过第IV-A节所述的$\boxplus$操作，将状态参数化到当前状态估计的切空间（即误差状态）中。求解得到的误差状态会用于更新当前的状态估计，随后进入下一次迭代直至收敛。收敛后的状态估计记为$\bar{\mathbf{x}}_k$，公式(11)在收敛时的海森矩阵记为$\bar{\mathbf{P}}_k$；它们会被用于按照第IV-C节的描述传播传入的惯性测量单元数据。收敛后的状态还会被用于在第V-A节更新新的激光雷达扫描数据与全局地图的对应关系，以及在第V-B节更新视觉全局地图。
### V. 地图管理
我们的地图包含两部分：用于LIO子系统的点云地图（激光雷达全局地图），以及用于VIO子系统的附带图像块的点云地图（视觉全局地图）。
### A. 激光雷达全局地图
我们的激光雷达全局地图采用了FAST-LIO2 [^17]算法，该算法将所有历史3D点组织成一种名为ikd-Tree [^32]的增量式k-d树结构。ikd-Tree提供了点查询、插入和删除接口。它还会按照指定分辨率对点云地图进行内部下采样，持续监测其树结构，并通过重建相应的子树来动态平衡树结构。当接收到新的激光雷达扫描数据时，我们会利用预测位姿在ikd-Tree中查找每个点对应的最近邻点（详见第IV-D.1节）。在将扫描数据与惯性测量单元数据融合得到$\bar{x}_{k}$后（详见第IV-E节），我们便利用该结果将扫描点转换到全局坐标系，并以LIO速率将其插入ikd-Tree中。
### B. 视觉全局地图
视觉全局地图是由以往观测到的激光雷达点组成的集合。每个点都附带了观测到它的多张图像中的图像块。视觉全局地图的数据结构与更新方式如下所述：
1) 数据结构：为了快速找到当前视场角内的视觉地图点，我们使用轴对齐体素来存放视觉全局地图中的点。体素大小相同（$0.5 \times 0.5 \times 0.5$米），通过哈希表进行组织以实现快速索引。存储在体素中的点会保存其位置、从不同参考图像中提取的三级图像块金字塔（$8 \times 8 \times 3$像素）以及每个图像块金字塔对应的相机位姿。
2) 视觉子地图与异常点剔除：尽管体素数量远少于视觉地图点的数量，但确定哪些体素位于当前帧视场角内仍可能非常耗时，尤其是当地图点（即体素）数量较多时。为解决这一问题，我们会筛选出上一次扫描中包含激光雷达点的那些体素。利用已测量的点查询哈希表即可高效完成此操作。如果相机视场角与激光雷达视场角高度重叠，那么位于相机视场角内的地图点很可能就包含在这些体素中。因此，只需提取这些体素中的点并进行视场角检查，就能得到视觉子地图。
视觉子地图中可能包含在当前图像帧中被遮挡或深度不连续的点，这些点会严重降低视觉惯性里程计的精度。为解决这一问题，我们需要对视觉子地图中的所有点进行投影。
---
TARI F II
ABSOLUTE TRANSLATIONAL ERRORS (RMSE, METERS) IN NTU-VIRAL SEQUENCES WITH GOOD QUALITY GROUND TRUTH
| eee_01 | eee_02 | eee_03 | nya_01 | nya_02 | nya_03 | sbs_01 | sbs_02 | sbs_03 |
| FAST-LIVO | 0.19 | 0.13 | 0.19 | 0.12 | 0.18 | 0.11 | 0.25 | 0.13 | 0.13 |
| FAST-LIO2 | 0.21 | 0.17 | 0.21 | 0.14 | 0.21 | 0.13 | 0.18 | 0.16 | 0.14 |
| SVO2.0 (edgelets+prior) | Fail | Fail | 4.12 | 2.29 | 2.91 | 3.32 | 7.84 | Fail | Fail |
| R2LIVE | 0.45 | 0.21 | 0.97 | 0.19 | 0.63 | 0.31 | 0.56 | 0.24 | 0.44 |
| DVL-SLAM (no loop closure) | 2.88 | 1.65 | 3.08 | 2.09 | 1.45 | 1.82 | 1.08 | 2.31 | 2.23 |
Fig. 3. Outlier rejection. (a) shows the diagrammatic drawing of occluded and depth-discontinuous map points. (b) shows the effect visualization of outlier rejection module in real scenes. The red dots are the rejected map points, and the green dots are the accepted map points.
利用$\hat{\mathbf{x}}_k$中预测的位姿将这些点投影到当前帧上，并在大小为$40 \times 40$像素的网格中保留深度最小的点。此外，我们还将最新一次激光雷达扫描中的点投影到当前帧，通过对比它们的深度来判断是否遮挡了$9 \times 9$邻域内的地图点。被遮挡的地图点会被剔除（见图3），剩余的点则用于对齐当前图像（详见第IV-D.2节）。
3) 视觉全局地图更新：在新图像帧对齐之后（详见第IV-D.2节），我们会将当前图像中的图像块附加到视场角内的地图点上，这样地图点就能拥有视角分布均匀的有效图像块。具体而言，我们会选择帧对齐后光度误差较大的地图点；如果该地图点上次添加图像块至今已超过20帧，或者当前帧中的该地图点与其上次添加图像块时的参考帧中的像素位置相差超过40像素，我们就会为其添加新的图像块。新图像块从当前图像中提取，大小为$8 \times 8$像素。除了图像块金字塔外，我们还会将帧位姿附加到该地图点上。
除了在地图点中添加补丁外，我们还需要在视觉全局地图中添加新的地图点。为此，我们将当前图像划分为大小为$40 \times 40$像素的网格，并将最新的激光雷达扫描点投影到该网格上。每个网格中梯度最大的投影激光雷达点会被添加到视觉全局地图中，同时还会加入在该处提取的补丁以及图像位姿信息。为了避免将边缘处的激光雷达点加入视觉地图，我们会跳过局部曲率较大的边缘点[^4] [^5]。
### 六、实验与结果
在本节中，我们在公开数据集和私有数据集上验证了所提出的方法。
### A. 基准数据集
本节在NTU-VIRAL公开数据集的全部九个序列上对该方法进行定量实验[^13]。该数据集使用的传感器包括左摄像头、水平排列的16通道OS1 gen1 $^{4}$激光雷达及其内置惯性测量单元。我们将所提方法与多种开源里程计系统进行比较，包括基于特征的、全激光雷达-惯性-视觉里程计R2LIVE [^11]，直接式激光雷达-惯性里程计FAST-LIO2 [^17]，半直接式视觉-惯性里程计SVO2.0 [^33]，以及直接式激光雷达-视觉SLAM系统DVL-SLAM [^29]。所有这些系统均从其各自的Github网站下载，其中FAST-LIO2、R2LIVE和DVL-SLAM均采用针对户外场景推荐的参数。SVO2.0的参数（包括网格大小、特征选择阈值和滑动窗口大小）经过调试以达到最佳效果。此外，由于除DVL-SLAM外，所有对比系统均为无回环闭合功能的里程计，因此我们移除了DVL-SLAM的回环闭合模块以实现公平比较。修改后的DVL-SLAM我们也已上传至Github $^{5}$，该系统保留了滑动窗口优化机制以尽可能保持精度。
表二展示了所有方法的结果，每种方法在所有序列中均使用相同参数。可以看出，除“sbs_01”序列外，我们的方法在所有序列中均取得了最佳精度；在“sbs_01”序列中，我们系统的误差略高于纯激光雷达-惯性里程计FAST-LIO2。这是因为无人机运动过快导致图像极度模糊，融合这些低质量图像对里程计精度的提升作用有限。除“sbs_01”序列外，由于我们的方法紧密融合了激光雷达、惯性和视觉信息，其性能超越了纯激光雷达-惯性里程计FAST-LIO2.0（提升程度取决于图像质量），也优于其他子系统（如纯视觉-惯性里程计SVO2.0和纯激光雷达-视觉里程计DVL-SLAM）在所有其余序列中的表现。此外，我们的系统精度也高于最先进的基于特征的紧耦合全激光雷达-惯性-视觉里程计系统R2LIVE。有趣的是，R2LIVE的精度偶尔会低于纯激光雷达-惯性里程计FAST-LIO2。原因是R2LIVE基于特征的LIO子系统可配准的激光雷达点较少，而FAST-LIO2采用直接式LIO子系统可配准更多原始激光雷达点。R2LIVE中对图像的融合可以弥补这种精度损失，因此在某些序列中其性能超过了FAST-LIO2。最后需要指出的是，由于无人机快速旋转和强烈图像模糊导致先验信息不足，SVO2.0在eee_01、eee_02、sbs_02和sbs_03序列中无法正常运行，而DVL-SLAM因其视觉里程计模块使用了激光雷达测量数据，在这些序列中仍能正常工作。
---
由于无人机快速旋转以及严重的图像模糊现象导致了相关信息丢失，而DVL-SLAM凭借其视觉里程计模块中对激光雷达测量数据的利用，得以在这些序列中正常工作。
### B. 私有数据集
1) 设备设置：用于数据采集的传感器套件如图4所示，包括一台机载计算机DJI Manifold-2C（配备Intel i7-8550U CPU和8 GB RAM）、两台工业相机（MV-CA013-21UC）以及一台Livox Avia激光雷达。所有传感器通过STM32同步定时器产生的10 Hz触发信号实现硬件同步。需要注意的是，Livox转换器仅支持每秒1 Hz的脉冲触发信号，而Livox SDK会自动将其划分为10 Hz信号。
(b)
Fig. 4. Our platform with hardware synchronization for data acquisition. (a) our hardware system, (b) the hardware synchronization diagram.
2) 激光雷达性能退化实验：在本实验中，我们在激光雷达性能退化的环境下评估我们的系统，该环境为一面长约30米的墙面。结果如图5所示。由于沿墙面方向缺乏约束，基于激光雷达与惯性传感器的里程计FAST-LIO2在该方向上无法给出良好的状态估计，尤其是当视场中只能看到侧面墙面时。同样，墙面上高度重复的视觉特征以及墙面垂直方向上的不敏感视觉约束，会导致视觉惯性里程计SVO2.0出现漂移现象。与FAST-LIO2和SVO2.0相比，我们的系统能够稳健且精确地应对该场景，表现出最佳性能，端到端漂移量仅为0.05米。
Fig. 5. The performance of FAST-LIVO, FAST-LIO2, and SVO2.0 in a LiDAR degeneration scene. The point cloud is colored by LiDAR intensity. (a), (b) and (c) represent the top view of the corresponding position a, b, c respectively.
3) 视觉挑战实验：在本实验中，我们测试了视觉SLAM中极具难度的场景。如图6所示，传感器的运动过程包括从室内到室外、从室外到室内的过程，还包含两次剧烈运动以及面对无纹理的白色墙面。我们提出的算法能够在该极具挑战性的场景中稳定工作，最终在总路径长度79.52米的情况下，返回起点时的端到端误差仅为0.04米。
Fig. 6. The RGB-colored point cloud of the visual challenging scene containing aggressive motions, indoor-to-outdoor and outdoor-to-indoor movements, and a visual texture-less wall. The green path is the computed trajectory.
4) 带彩色点云的高精度建图：在本实验中，我们利用所提算法实时重建了香港大学校园环境的精确、密集、三维且带有RGB颜色的地图，如图7所示。值得一提的是，彩色点云的放大视图具有与实际RGB图像相似的精细视觉细节。
Fig. 7. (a) RGB-colored point cloud, (b) partially enlarged view of the colored point cloud and (c) the corresponding RGB image. TABLE III
MEAN TIME CONSUMPTION IN MILLISECONDS
| | Intel i7 | ARM |
| VIO subsystem | Visual Submap | 1.63 | 3.81 |
| Outlier Rejection | 2.07 | 2.58 |
| Sparse-Direct Visual Alignment | 3.19 | 3.88 |
| ESIKF Update | 0.42 | 0.52 |
| Update Visual Global Map | 2.92 | 3.03 |
| Total Time | 10.23 | 13.82 |
| LIO subsystem | 26.52 | 51.51 |
### C. 时间分析
在本节中，我们评估了完整系统FAST-LIVO的计算效率，并将其与基于特征的激光雷达-惯性-视觉里程计系统R2LIVE进行比较。比较是在所有私有数据集上，于配备8核Intel Core i7-10700U处理器的台式电脑上进行的：R2LIVE的激光雷达与图像帧平均处理时间为LIO和VIO前端45.16毫秒，加上VIO后端滑动窗口优化所需的59.27毫秒。
---
而FAST-LIVO的完整处理时间仅为36.75毫秒。表III进一步展示了FAST-LIVO在台式电脑以及搭载Qualcomm Kryo585 CPU的嵌入式计算平台RB5上的平均时间分解情况。可以看出，FAST-LIVO能够在Intel和ARM处理器上实时运行，且仍有显著的计算冗余。
### VII. 结论
本文提出了一种快速、稳健、稀疏直接、帧到图的激光雷达-惯性-视觉融合框架，其速度优于当前最先进的LIVO算法。我们在误差状态迭代卡尔曼滤波器中融合了激光雷达、惯性传感器和摄像头的测量数据，并采用了基于图像块的光度误差模型。我们的系统在剧烈运动、室内外切换、无纹理白色墙面以及激光雷达性能退化等环境中进行了测试。在公开数据集上的实验表明，该系统在整体性能上优于当前最先进的LIO、VIO和LIVO算法。
## 致谢
作者衷心感谢Livox Technology在整个研究过程中提供的设备支持。本研究部分得到了香港大学教育资助委员会一般研究基金（项目编号17206421）的支持。作者还要感谢林佳荣在实验中的协助。
## 参考文献
[^1]: R. Mur-Artal and J. D. Tardós, “Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras,” IEEE transactions on robotics, vol. 33, no. 5, pp. 1255–1262, 2017.
[^2]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE transactions on pattern analysis and machine intelligence, vol. 40, no. 3, pp. 611–625, 2017.
[^3]: J. Engel, T. Schöps, and D. Cremers, “Lsd-slam: Large-scale direct monocular slam,” in European conference on computer vision. Springer, 2014, pp. 834–849.
[^4]: J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.” in Robotics: Science and Systems, vol. 2, no. 9, 2014.
[^5]: J. Lin and F. Zhang, "Loam livox: A fast, robust, high-precision lidar odometry and mapping package for lidars of small fov," in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 3126–3131.
[^6]: T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 4758–4765.
[^7]: T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
[^8]: C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, "Lins: A lidar-inertial state estimator for robust and efficient navigation," in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 8899–8906.
[^9]: J. HYUN and H. Myung, “Uwb-inertial slam based on tightly-coupled ekf framework,” in Int’l Conf. on Ubiquitous Robots (UR) 2020. Korea Robot Society, 2020.
[^10]: A. Kramer, C. Stahoviak, A. Santamaria-Navarro, A.-A. Aghamohammadi, and C. Heckman, "Radar-inertial ego-velocity estimation for visually degraded environments," in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 5739–5746.
[^11]: J. Lin, C. Zheng, W. Xu, and F. Zhang, "R²live: A robust, real-time, lidar-inertial-visual tightly-coupled state estimator and mapping," IEEE Robotics and Automation Letters, vol. 6, no. 4, pp. 7469–7476, 2021.
[^12]: T. Shan, B. Englot, C. Ratti, and D. Rus, "Lvi-sam: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping," in 2021 IEEE international conference on robotics and automation (ICRA). IEEE, 2021, pp. 5692–5698.
[^13]: T.-M. Nguyen, S. Yuan, M. Cao, Y. Lyu, T. H. Nguyen, and L. Xie, "Ntu viral: A visual-inertial-ranging-lidar dataset, from an aerial vehicle viewpoint," International Journal of Robotics Research (accepted, to appear), 2021.
[^14]: R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “Orb-slam: a versatile and accurate monocular slam system,” IEEE transactions on robotics, vol. 31, no. 5, pp. 1147–1163, 2015.
[^15]: M. Irani and P. Anandan, “All about direct methods,” in Proc. Workshop Vis. Algorithms, Theory Pract, 1999, pp. 267–277.
[^16]: C. Forster, M. Pizzoli, and D. Scaramuzza, “Svo: Fast semi-direct monocular visual odometry,” in 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014, pp. 15–22.
[^17]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “Fast-lio2: Fast direct lidar-inertial odometry,” IEEE Transactions on Robotics, pp. 1–21, 2022.
[^18]: M. Meilland, A. I. Comport, and P. Rives, “Real-time dense visual tracking under large lighting variations,” in British Machine Vision Conference. British Machine Vision Association, 2011, pp. 45–1.
[^19]: T. Tykkälä, C. Audras, and A. I. Comport, “Direct iterative closest point for real-time visual odometry,” in 2011 IEEE International Conference on Computer Vision Workshops (ICCV Workshops). IEEE, 2011, pp. 2050–2056.
[^20]: C. Kerl, J. Sturm, and D. Cremers, “Robust odometry estimation for rgb-d cameras,” in 2013 IEEE international conference on robotics and automation. IEEE, 2013, pp. 3748–3754.
[^21]: J. Graeter, A. Wilczynski, and M. Lauer, “Limo: Lidar-monocular visual odometry,” in 2018 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2018, pp. 7872–7879.
[^22]: X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, "Lic-fusion: Lidar inertial-camera odometry," in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 5848–5854.
[^23]: K. Sun, K. Mohta, B. Pfrommer, M. Watterson, S. Liu, Y. Mulgaonkar, C. J. Taylor, and V. Kumar, “Robust stereo visual inertial odometry for fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 3, no. 2, pp. 965–972, 2018.
[^24]: X. Zuo, Y. Yang, P. Geneva, J. Lv, Y. Liu, G. Huang, and M. Pollefeys, "Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking," in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 5112–5119.
[^25]: W. Shao, S. Vijayarangan, C. Li, and G. Kantor, “Stereo visual inertial lidar simultaneous localization and mapping,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 370–377.
[^26]: D. Wisth, M. Camurri, S. Das, and M. Fallon, “Unified multi-modal landmark tracking for tightly coupled lidar-visual-inertial odometry,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1004–1011, 2021.
[^27]: C. F. W. Bell B.M., “The iterated kalman filter update as a gauss-newton method,” Automatic Control IEEE Transactions, vol. 38, no. 2, pp. 294–297, 1993.
[^28]: Y.-S. Shin, Y. S. Park, and A. Kim, “Direct visual slam using sparse depth for camera-lidar system,” in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018, pp. 5144–5151.
[^29]: —, “Dvl-slam: sparse depth enhanced direct visual-lidar slam,” Autonomous Robots, vol. 44, no. 2, pp. 115–130, 2020.
[^30]: W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” IEEE Robotics and Automation Letters, pp. 1–1, 2021.
[^31]: D. He, W. Xu, and F. Zhang, “Embedding manifold structures into kalman filters,” arXiv preprint arXiv:2102.03804, 2021.
[^32]: Y. Cai, W. Xu, and F. Zhang, "ikd-tree: An incremental kd tree for robotic applications," arXiv preprint arXiv:2102.10808, 2021.
[^33]: C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, “Svo: Semidirect visual odometry for monocular and multicamera systems,” IEEE Transactions on Robotics, vol. 33, no. 2, pp. 249–265, 2016.