# R $^{3}$LIVE：一个鲁棒、实时、RGB彩色、激光雷达-惯性-视觉紧耦合的状态估计与建图软件包 林佳荣、张福 摘要—本文提出了一种全新的激光雷达-惯性-视觉传感器融合框架，名为R³LIVE。该框架利用激光雷达、惯性及视觉传感器的测量数据，实现鲁棒且精准的状态估计。R³LIVE包含两个子系统：激光雷达-惯性里程计（LIO）和视觉-惯性里程计（VIO）。其中，LIO子系统（FAST-LIO）利用激光雷达与惯性传感器的测量数据来构建地图的几何结构（即三维点的坐标）；VIO子系统则利用视觉-惯性传感器的数据来渲染地图的纹理（即三维点的颜色）。更具体地说，VIO子系统通过最小化帧到地图的光度误差，直接且高效地融合视觉数据。所提出的R³LIVE系统是在我们之前的工作R²LIVE基础上开发的，采用了完全不同的VIO架构设计。整个系统能够实时重建周围环境精确、稠密、三维且带RGB颜色的地图（详见附带的视频¹）。实验结果表明，该系统在状态估计方面的鲁棒性和精度均优于现有的同类系统。为了分享我们的研究成果并为社区做出贡献，我们在Github²上开源了R³LIVE。 ## I. 引言 近年来，激光雷达传感器在自动驾驶车辆[^1]、无人机[^2] [^3]等各类机器人应用中得到越来越广泛的使用。尤其是随着低成本固态激光雷达（如[^4]）的出现，基于这类激光雷达的更多应用[^5] [^6] [^7] [^8] [^9]推动了机器人领域的发展。然而，对于基于激光雷达的SLAM系统而言，在几何特征不足的场景中很容易失效，尤其是视场角（FoV）有限的固态激光雷达[^10]。为解决这一问题，将激光雷达与其他传感器（如摄像头[^11] [^12] [^13] [^14]和超宽带（UWB）[^15] [^16]）融合，可提升系统的鲁棒性与精度。近期，机器人领域提出了多种激光雷达-视觉融合框架[^17]。张和辛格提出的V-LOAM[^18]是早期的激光雷达-惯性-视觉系统之一，它采用松耦合的视觉-惯性里程计（VIO）作为初始化激光雷达建图子系统的运动模型。类似地，在[^19]中，作者提出了一种立体视觉-惯性激光雷达SLAM系统，该系统将紧耦合的立体视觉-惯性里程计与激光雷达建图、激光雷达增强的视觉回环闭合模块相结合。更近期，王等人提出了DV-LOAM[^20]，这是一种直接的视觉-激光雷达融合框架。该系统首先利用两级视觉里程计模块来获取高效的粗略状态估计，随后用激光雷达建图模块对粗略位姿进行优化，最后借助回环闭合模块修正累积的漂移。上述系统均在松耦合层面融合激光雷达-惯性-视觉传感器，其中激光雷达测量数据并未与视觉或惯性测量数据一起进行联合优化。 ![Image](images/2fb05df4d105310218d4d511ac9fe01a8c345d45649878b90a33d7a060df4529.jpg)
Fig. 1: Our developed R $^{3}$LIVE is able to reconstruct a dense, 3D, RGB-colored point cloud of the traveled environment in real-time. The white path is the figure is our traveling trajectory for collecting the data.
视觉里程计模块用于高效获取粗略的状态估计，之后再用激光雷达建图模块对粗略位姿进行优化，最终利用回环闭合模块来修正累积的漂移。上述系统均在松耦合层面融合激光雷达-惯性-视觉传感器，其中激光雷达测量数据并未与视觉或惯性测量数据一起进行联合优化。 近期，人们提出了耦合度更高的激光雷达-惯性-视觉融合框架。例如，左等人提出的LIC-fusion [^13] 就是一种紧密耦合的激光雷达-惯性-视觉融合框架，它在多状态约束卡尔曼滤波（MSCKF）框架内结合了惯性测量单元数据、稀疏视觉特征以及激光雷达特征，并同时进行在线时空校准。为了进一步提升激光雷达扫描匹配的鲁棒性，他们后续的研究成果LIC-Fusion 2.0 [^14] 提出了一种在滑动窗口内跨多个激光雷达扫描的平面特征跟踪算法，并对窗口内的位姿轨迹进行优化。单等人在 [^12] 中提出的LVI-SAM则在因子图基础上构建了一个紧密耦合的同步定位与建图框架，融合了激光雷达、视觉和惯性传感器。当其中一个子系统出现故障时，LVI-SAM的激光雷达-惯性和视觉-惯性子系统可以独立运行；一旦检测到足够多的特征，它们便能协同工作。我们之前的工作 $R^{2}LIVE$ [^11] 将激光雷达-惯性-视觉传感器的数据紧密融合，提取激光雷达和稀疏视觉特征，通过在误差状态迭代卡尔曼滤波框架内最小化特征重投影误差来估计系统状态，从而实现实时性能，同时利用滑动窗口优化提升了整体视觉建图精度。$R^{2}LIVE$ 能够在各种复杂场景中稳定运行。 --- 剧烈机动 诱饵，甚至是狭窄隧道—— 小型物体 为了进一步提升我们之前的工作 $R^{2}$LIVE 的实时性能、鲁棒性和精度，我们提出了一种名为 $R^{3}$LIVE 的新型传感器融合框架，其VIO子系统与 $R^{2}$LIVE 完全不同。本文的主要贡献如下： • 我们提出了一种实时同步定位、建图与着色框架。该框架由用于重建几何结构的激光雷达-惯性里程计（LIO）和用于纹理渲染的视觉-惯性里程计（VIO）组成。整个系统能够实时重建出环境的密集、三维、带RGB颜色的点云（见图1(a)）。 - 我们基于RGB彩色点云地图提出了一种新型VIO系统。该VIO通过最小化观测到的地图点的RGB颜色与当前图像中测得颜色的像素差异来估计当前状态。这一过程无需环境中存在显著的视觉特征，也省去了相应的处理时间（如特征检测与提取），因此我们的系统在纹理较少的环境中尤为鲁棒。 - 得益于精心的架构设计与实现，我们提出的系统在状态估计方面比现有成果具有更高的鲁棒性和精度。该系统已在多种室内外环境中得到验证。结果显示，在行驶距离达1.5公里后，系统的平移漂移量仅为0.16米，旋转偏差为$3.9^{\circ}$。 - 为了分享我们的研究成果并为社区做出贡献，我们在Github $^{3}$ 上开源了R $^{2}$LIVE，其中包括所有代码以及手持设备的机械设计资料。 ### 二、系统概述 ![Image](images/35e7e8d665a7dd9421d7730e5fc5792d35296c17d57dd73e13e52aa61b97da8e.jpg)
Fig. 2: The overview of our proposed system.
我们系统的概览如图2所示。我们提出的框架包含两个子系统：LIO子系统（上部）和VIO子系统（下部）。LIO子系统负责构建全局地图的几何结构，该结构用于配准输入的激光雷达扫描数据，并通过最小化点面残差来估计系统状态。VIO子系统则负责构建地图的纹理，它利用输入图像为每个点赋予RGB颜色，同样通过最小化点面残差来更新系统状态。
TABLE I: NOMENCLATURE
NotationExplanation
Expression
田/⊆The encapsulated “boxplus” and “boxminus” operations on manifold
$G(\cdot)$The value of (·) expressed in global frame
$C(\cdot)$The value of (·) expressed in camera frame
$\text{Exp}(\cdot)/\text{Log}(\cdot)$The Rodrigues' transformation between the rotation matrix and rotation vector
$\delta(\cdot)$The estimated error of (·). It is the minimum parameterization that characterized in the tangent space.
$\Sigma_{(\cdot)}$The covariance matrix of vector (·)
Variable
$b_g, b_a$The bias of gyroscope and accelerometer
$G_g$The gravitational acceleration in global frame
$G_v$The linear velocity in global frame
$(^{C}\mathbf{R}_I, ^{G}\mathbf{p}_I)$The attitude and position of the IMU w.r.t. global frame
$(^{I}\mathbf{R}_C, ^{I}\mathbf{p}_C)$The extrinsic value between camera and IMU
xThe full state vector
$\hat{x}$The prior estimation of x
$\hat{x}$The current estimate of x in each ESIKF iteration
帧间PnP投影误差以及帧到地图的光度误差。 ### 三、符号说明 本文采用表I中所示的符号，这些符号已在先前的工作R $^{2}$LIVE [^11]中介绍过。 ### A. 状态向量 在我们的工作中，将完整状态向量定义为$\mathbf{x} \in \mathbb{R}^{29}$： \mathbf{x}=\left[\mathbf{G}_{\mathbf{R},I}^{T},\mathbf{G}_{\mathbf{p},I}^{T},\mathbf{G}_{\mathbf{v},T}^{T},\mathbf{b}_{\mathbf{g}}^{T},\mathbf{b}_{\mathbf{a}}^{T},\mathbf{G}_{\mathbf{g},T}^{T},\mathbf{I}\mathbf{R}_{C}^{T},\mathbf{I}\mathbf{p}_{C}^{T},\mathbf{I}_{t_{C}},\mathbf{\phi}^{T}\right]^{T}\tag{1} 其中符号${}^{G}\mathbf{R}_{I},{}^{G}\mathbf{p}_{I},{}^{G}\mathbf{v},b_{\mathbf{g}},b_{\mathbf{a}},{}^{G}\mathbf{g},{}^{I}\mathbf{R}_{C},{}^{I}\mathbf{p}_{C}$的解释见表I，$t_{C}$表示惯性测量单元与相机之间的时间偏移，而激光雷达默认已与惯性测量单元同步，$\phi=\left[f_{x},f_{y},c_{x},c_{y}\right]^{T}$为相机内参，其中$(f_{x},f_{y})$代表相机焦距，$(c_{x},c_{y})$表示主点相对于图像平面左上角的偏移量。 ### B. 地图表示 我们的地图由体素和点构成，点包含在体素中，是地图的最小元素。 1) 体素：为了在我们的VIO子系统中快速查找用于渲染和跟踪的地图中的点（详见第V-C节和第V-D节），我们设计了一种固定大小（例如$0.1 \, \text{m} \times 0.1 \, \text{m} \times 0.1 \, \text{m}$）的容器，称为体素。如果一个体素最近添加了点（例如近一秒内），我们就将该体素标记为激活状态；否则，将其标记为非激活状态。 2) 点：在我们的工作中，一个点P是一个大小为6的向量： $$ \mathbf{P}=\left[\mathbf{\Sigma}^{G}\mathbf{p}_{x},\mathbf{\Sigma}^{G}\mathbf{p}_{y},\mathbf{\Sigma}^{G}\mathbf{p}_{z},\mathbf{c}_{r},\mathbf{c}_{g},\mathbf{c}_{b}\right]^{T}=\left[\mathbf{\Sigma}^{G}\mathbf{p}^{T},\mathbf{c}^{T}\right]^{T} $$ 其中头部$3 \times 1$子向量$^G\mathbf{p} = \left[^G\mathbf{p}_x, ^G\mathbf{p}_y, ^G\mathbf{p}_z\right]^T$表示该点在全局坐标系下的三维位置，尾部$3 \times 1$向量$\mathbf{c} = [\mathbf{c}_r, \mathbf{c}_g, \mathbf{c}_b]^T$为该点的RGB颜色。此外，我们还记录该点的其他信息，比如$3 \times 3$协方差矩阵$\Sigma_\mathbf{p}$和$\Sigma_c$，它们分别表示对$^G\mathbf{p}$和$\mathbf{c}$的估计误差，以及该点的创建和渲染时间戳。 --- ### 四、激光雷达-惯性导航子系统 如图2所示，$R^{3}LIVE$的LIO子系统负责构建全局地图的几何结构。对于传入的激光雷达扫描数据，由于帧内连续运动产生的运动失真会通过IMU反向传播来补偿，具体过程如[^5]所示。随后，我们利用误差状态迭代卡尔曼滤波器（ESIKF）来最小化点到平面的残差，从而估计系统的状态。最终，在状态收敛后，该扫描中的点会被添加到全局地图中，同时标记对应的体素为激活或失活状态。全局地图中累积的3D点构成了几何结构，该结构也会为我们的VIO子系统提供深度信息。关于$R^{3}LIVE$中LIO子系统的详细实现，读者可参考我们之前的相关工作[^11] [^21]。 ### V. 视觉-惯性里程计子系统 我们的VIO子系统负责渲染全局地图的纹理，通过最小化光度误差来估计系统状态。具体来说，我们将一定数量的点（即被跟踪的点）从全局地图投影到当前图像上，然后在ESIKF框架内通过最小化这些点的光度误差来迭代估计系统状态。为了提高效率，被跟踪的地图点是稀疏分布的。针对这些稀疏地图点最小化光度误差时，通常需要构建输入图像的金字塔。然而，这种金字塔对平移和旋转都不具备不变性，而这正是我们需要估计的参数。在我们提出的框架中，我们利用单个地图点的颜色来计算光度误差。在VIO中同时渲染出的这种颜色是地图点的固有属性，对相机的平移和旋转都具有不变性。为确保鲁棒且快速的收敛，我们设计了图2所示的两步流程：首先利用帧间光流来跟踪地图点，并通过最小化透视-点（PnP）投影误差来优化系统状态（见第V-A节）；接着，通过最小化被跟踪点之间的帧到地图光度误差来进一步精化系统状态估计（见第V-B节）。借助收敛后的状态估计结果和原始输入图像，我们进行纹理渲染，以更新全局地图中点的颜色（见第V-C节）。 ### A. 帧间视觉-惯性里程计 假设我们已经在上一帧图像 $\mathcal{P} = \{\mathbf{P}_1, ..., \mathbf{P}_m\}$ 中跟踪了 m 个地图点 $\mathbf{I}_{k-1}$，它们在 $\mathbf{I}_{k-1}$ 上的投影为 $\{\rho_{1_{k-1}}, ..., \rho_{m_{k-1}}\}$，我们利用 Lucas–Kanade 光流法找出它们在当前帧图像 $\mathbf{I}_k$ 中的位置，记为 $\{\rho_{1_k}, ..., \rho_{m_k}\}$。然后，我们通过 ESIKF 计算并优化其投影误差以估计状态。 1) 透视-点投影误差：如图3所示，我们以第s个点$\mathbf{P}_s = \begin{bmatrix} G \mathbf{p}_s^T, \mathbf{c}_s^T \end{bmatrix}^T \in \mathcal{P}$为例，其投影误差$\mathbf{r}\left(\tilde{\mathbf{x}}_k, \boldsymbol{\rho}_s^k, G \mathbf{p}_s\right)$的计算方式如下： ![Image](images/f07b095a9b0421ea81edbb36711e32a7060a9239a43254f4a77dfd88966132a4.jpg)
Fig. 3: Our frame-to-frame ESIKF VIO update the system's state with minimizing the PnP projection error.
$$ \begin{aligned}^{C}\mathbf{p}_{s}=&\left[{}^{C}\mathbf{p}_{x_{s}},{}^{C}\mathbf{p}_{y_{s}},{}^{C}\mathbf{p}_{z_{s}}\right]^{T}=\left({}^{G}\check{\mathbf{R}}_{I_{k}}\cdot{}^{I}\check{\mathbf{R}}_{C_{k}}\right)^{T}\cdot{}^{G}\mathbf{p}_{s}\\&-{}^{I}\check{\mathbf{R}}_{C_{k}}^{T}\cdot{}^{I}\check{\mathbf{p}}_{C_{k}}-\left({}^{G}\check{\mathbf{R}}_{I_{k}}\cdot{}^{I}\check{\mathbf{R}}_{C_{k}}\right)^{T}\cdot{}^{G}\check{\mathbf{p}}_{I_{k}}\\\mathbf{r}&\left(\check{\mathbf{x}}_{k},\boldsymbol{\rho}_{s_{k}},{}^{G}\mathbf{p}_{s}\right)=\boldsymbol{\rho}_{s_{k}}-\boldsymbol{\pi}({}^{C}\mathbf{p}_{s},\check{\mathbf{x}}_{k})\end{aligned} $$ 其中，$\dot{\mathbf{x}}_k$是每次ESIKF迭代中x的当前状态估计值，$\pi(^C\mathbf{p}_s, \dot{\mathbf{x}}_k) \in \mathbb{R}^2$的计算方式如下： $$ \begin{aligned}\boldsymbol{\pi}^{\left(\begin{array}{c}C\\\mathbf{p}_{s},\check{\mathbf{x}}_{k}\\\end{array}\right)}&=\left[\check{f}_{x_{k}}\frac{^{C}\mathbf{p}_{x_{s}}}{^{C}\mathbf{p}_{z_{s}}}+\check{c}_{x_{k}},\check{f}_{y_{k}}\frac{^{C}\mathbf{p}_{y_{s}}}{^{C}\mathbf{p}_{z_{s}}}+\check{c}_{y_{k}}\right]^{T}\\&+\frac{^{I}\check{t}_{C_{k}}}{\Delta t_{k-1,k}}(\boldsymbol{\rho}_{s_{k}}-\boldsymbol{\rho}_{s_{k-1}})\\ \end{aligned} $$ 这里$\Delta t_{k-1,k}$是上一图像帧$\mathbf{I}_{k-1}$与当前图像帧$\mathbf{I}_{k}$之间的时间间隔。注意在公式(5)中，第一项是针孔投影函数，第二项是在线时间校正因子[^22]。 公式(4)中残差所对应的测量噪声有两个来源：一个是$\rho_{s_k}$中的像素跟踪误差，另一个则是地图点位置误差$^G p_s$。 $$ \mathbf{\Sigma}^{G}\mathbf{p}_{s}=\mathbf{\Sigma}^{G}\mathbf{p}_{s}^{\mathsf{g t}}+\mathbf{n}_{\mathbf{p}_{s}},\mathbf{n}_{\mathbf{p}_{s}}\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_{\mathbf{n}_{\mathbf{p}_{s}}}) $$ $$ \begin{array}{r}{\rho_{s_{k}}=\rho_{s_{k}}^{\mathrm{g t}}+\mathbf{n}_{\rho_{s_{k}}},\mathbf{n}_{\rho_{s_{k}}}\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_{\mathbf{n}_{\rho_{s_{k}}}})}\end{array} $$ 其中，$^G\mathbf{p}_s^{\mathrm{gt}}$ 和 $\rho_{s_k}^{\mathrm{gt}}$ 分别是 $^G\mathbf{p}_s$ 和 $\rho_{s_k}$ 的真实值。接着，我们得到真实零残差 $\mathbf{r}(\mathbf{x}_k, \rho_{s_k}^{\mathrm{gt}}, ^{G}\mathbf{p}_s^{\mathrm{gt}})$ 的一阶泰勒展开式： $$ \mathbf{0}=\mathbf{r}\left(\mathbf{x}_{k},\boldsymbol{\rho}_{s_{k}}^{\mathrm{gt}},\mathbf{\Phi}^{G}\mathbf{p}_{s}^{\mathrm{gt}}\right)\approx\mathbf{r}\left(\mathbf{\check{x}}_{k},\boldsymbol{\rho}_{s_{k}},\mathbf{\Phi}^{G}\mathbf{p}_{s}\right)+\mathbf{H}_{s}^{r}\delta\mathbf{\check{x}}_{k}+\boldsymbol{\alpha}_{s} $$ 其中 $\alpha_s \sim \mathcal{N}(0, \Sigma_{\alpha_s})$ 以及 $$ \mathbf{H}_{s}^{r}=\left.\frac{\partial\mathbf{r}_{c}\left(\mathbf{\check{x}}_{k}\boxplus\delta\mathbf{\check{x}}_{k},\boldsymbol{\rho}_{s_{k}},\mathbf{\Lambda}^{G}\mathbf{p}_{s}\right)}{\partial\delta\mathbf{\check{x}}_{k}}\right|_{\delta\mathbf{\check{x}}_{k}=\mathbf{0}} $$ $$ \mathbf{\Sigma}_{\alpha_{s}}=\mathbf{\Sigma}_{\mathbf{n}_{\rho_{s_{k}}}}+\mathbf{F}_{\mathbf{p}_{s}}^{r}\mathbf{\Sigma}_{\mathbf{p}_{s}}\mathbf{F}_{\mathbf{p}_{s}}^{r^{\mathrm{~T~}}},\quad\mathbf{F}_{\mathbf{p}_{s}}^{r}=\frac{\partial\mathbf{r}\big(\mathbf{\check{x}}_{k},\mathbf{\rho}_{s_{k}},\mathbf{\Lambda}^{G}\mathbf{p}_{s}\big)}{\partial^{G}\mathbf{p}_{s}} $$ 2) 帧间视觉惯性里程计 ESIKF 更新：公式 (8) 给出了 $\mathbf{x}_k$（或等价地 $\delta\mathbf{x}_k \triangleq \mathbf{x}_k \boxminus \tilde{\mathbf{x}}_k$）的观测分布，该分布可与 IMU 传播得到的先验分布结合，从而得到 $\delta\tilde{\mathbf{x}}_k$ 的最大后验概率（MAP）估计： $$ \begin{aligned}\min_{\delta\tilde{\mathbf{x}}_{k}}&\left(\left\|\tilde{\mathbf{x}}_{k}\boxminus\hat{\mathbf{x}}_{k}+\mathcal{H}\delta\check{\mathbf{x}}_{k}\right\|_{\boldsymbol{\Sigma}_{\delta\hat{\mathbf{x}}_{k}}}^{2}\right.\\&\left.+\sum_{s=1}^{m}\left\|\mathbf{r}\left(\check{\mathbf{x}}_{k},\boldsymbol{\rho}_{s_{k}},\boldsymbol{}^{G}\mathbf{p}_{s}\right)+\mathbf{H}_{s}^{r}\delta\check{\mathbf{x}}_{k}\right\|_{\boldsymbol{\Sigma}_{\alpha_{s}}}^{2}\right)\end{aligned} $$ 其中 $\|\mathbf{x}\|_{\Sigma}^2 = \mathbf{x}^T \boldsymbol{\Sigma}^{-1} \mathbf{x}$ 是协方差为 $\boldsymbol{\Sigma}$ 的马氏距离平方，$\hat{\mathbf{x}}_k$ 是 IMU 传播得到的状态估计，$\boldsymbol{\Sigma}_{\delta \hat{\mathbf{x}}_k}$ 是 IMU 传播得到的状态协方差，而 $\mathcal{H}$ 是将状态误差从 $\hat{\mathbf{x}}_k$ 的切空间投影到 $\hat{\mathbf{x}}_k$ 的切空间时的雅可比矩阵。公式 (11) 中第一项的具体推导过程可参见 R $^2$LIVE [^11] 的第 E 节。 --- ![Image](images/46bdfec181e9c2a1c7ab91984d3d57ffcccd58e2fc93979a2be44c091832b7ef.jpg)
Fig. 4: Our frame-to-map ESIKF VIO updates the system state by minimizing the photometric error between the map point color and its measured color in the current image.
记： $$ \mathbf{H}=\left[\mathbf{H}_{1}^{r T},\ldots,\mathbf{H}_{m}^{r T}\right]^{T},\quad\mathbf{R}=\mathrm{d i a g}\big(\mathbf{\Sigma}_{\alpha_{1}},\ldots,\mathbf{\Sigma}_{\alpha_{m}}\big) $$ $$ \check{\mathbf{z}}_{k}=\left[\mathbf{r}\left(\check{\mathbf{x}}_{k},\boldsymbol{\mathbf{\rho}_{1_{k}}},{}^{G}\mathbf{p}_{1}\right)\ldots,\mathbf{r}\left(\check{\mathbf{x}}_{k},\boldsymbol{\mathbf{\rho}_{m_{k}}},{}^{G}\mathbf{p}_{m}\right)\right]^{T} $$ $$ \mathbf{P}=\left(\pmb{\mathscr{H}}\right)^{-1}\pmb{\Sigma}_{\delta\hat{\mathbf{x}}_{k}}(\pmb{\mathscr{H}})^{-T} $$ 根据 [^5] 的描述，卡尔曼增益的计算方式如下： $$ \mathbf{K}=\left(\mathbf{H}^{T}\mathbf{R}^{-1}\mathbf{H}+\mathbf{P}^{-1}\right)^{-1}\mathbf{H}^{T}\mathbf{R}^{-1} $$ $$ \mathbf{\check{x}}_{k}=\mathbf{\check{x}}_{k}\boxplus\left(-\mathbf{K}\mathbf{\check{z}}_{k}-\left(\mathbf{I}-\mathbf{K}\mathbf{H}\right)\left(\mathbf{\mathcal{H}}\right)^{-1}\left(\mathbf{\check{x}}_{k}\boxminus\mathbf{\hat{x}}_{k}\right)\right) $$ 上述过程（从第 V-A.1 节到第 V-A.2 节）会反复迭代直至收敛（即更新量小于给定阈值）。需要注意的是，这种迭代式卡尔曼滤波器等价于高斯-牛顿优化方法 [^23]。 ### B. 帧到地图的视觉惯性里程计 1) 帧到地图的光度更新：在完成帧间视觉惯性里程计更新后，我们已得到较为精确的状态估计 $\tilde{\mathbf{x}}_k$，随后通过最小化跟踪点的光度误差来进行帧到地图的视觉惯性里程计更新，以降低漂移。如图 4 所示，以第 s 个跟踪点 $\mathbf{P}_s \in \mathcal{P}$ 为例，其帧到地图的光度误差 $\mathbf{o}(\tilde{\mathbf{x}}_k,^G\mathbf{p}_s, \mathbf{c}_s)$ 计算如下： $$ \mathbf{o}\left(\check{\mathbf{x}}_{k},{}^{G}\mathbf{p}_{s},\mathbf{c}_{s}\right)=\mathbf{c}_{s}-\gamma_{s} $$ 其中 $\mathbf{c}_s$ 是保存在全局地图中的点的颜色，$\gamma_s$ 是当前图像 $\mathbf{I}_k$ 中观测到的颜色。为了获取观测颜色 $\gamma_s$ 及其协方差 $\Sigma_{\mathbf{n}_{\gamma_s}}$，我们先预测该点在当前图像 $\tilde{\rho}_{s_k} = \pi(^C\mathbf{p}_s, \tilde{\mathbf{x}}_k)$ 上的位置 $\mathbf{I}_k$，然后对邻近像素的 RGB 颜色进行线性插值。 我们同时考虑了 $\gamma_{s}$ 和 $c_{s}$ 的测量噪声： $$ \begin{array}{r}{\gamma_{s}=\gamma_{s}^{g t}+\mathbf{n}_{\gamma_{s}},\;\mathbf{n}_{\gamma_{s}}\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_{\mathbf{n}_{\gamma_{s}}})}\end{array} $$ $$ \mathbf{c}_{s}=\mathbf{c}_{s}^{g t}+\mathbf{n}_{\mathbf{c}_{s}}+\eta_{\mathbf{c}_{s}},\mathbf{n}_{\mathbf{c}_{s}}\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_{\mathbf{n}_{\mathbf{c}_{s}}}) $$ $$ \eta_{\mathbf{c}_{s}}\sim\mathcal{N}(\mathbf{0},\sigma_{s}^{2}\cdot\Delta t_{\mathbf{c}_{s}}) $$ 其中 $\gamma_{s}^{gt}$ 是 $\gamma_{s}$ 的地面真实值，$\mathbf{c}_{s}^{gt}$ 是 $\mathbf{c}_{s}$ 的真实值，$\Delta t_{c_{s}}$ 是当前帧与 $\mathbf{P}_{s}$ 上次渲染时刻之间的时间间隔。注意在公式 (19) 中，$\mathbf{c}_{s}$ 的测量噪声由两部分组成：上一次渲染时产生的估计误差 $\mathbf{n}_{c_{s}}$（详见第 VC 节），以及用于描述环境光照变化的脉冲随机游走过程噪声 $\eta_{c_{s}}$。 结合公式 (17)、(18) 和 (19)，我们得到真实零残差 $\mathbf{o}(\mathbf{x}_{k}, G\mathbf{p}_{s}^{gt}, G\mathbf{c}_{s}^{gt})$ 的一阶泰勒展开式： $$ \mathbf{0}=\mathbf{o}(\mathbf{x}_{k},{}^{G}\mathbf{p}_{s}^{g t},\mathbf{c}_{s}^{g t})\approx\mathbf{o}(\check{\mathbf{x}}_{k},{}^{G}\mathbf{p}_{s},\mathbf{c}_{s})+\mathbf{H}_{s}^{o}\delta\check{\mathbf{x}}_{k}+\mathbf{\beta}_{s} $$ 其中 $\boldsymbol{\beta}_s \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_{\boldsymbol{\beta}_s})$ 以及： $$ \mathbf{H}_{s}^{o}=\left.\frac{\partial\mathbf{o}\big(\mathbf{\check{x}}_{k}\boxplus\delta\mathbf{\check{x}}_{k},{}^{G}\mathbf{p}_{s},\mathbf{c}_{s}\big)}{\partial\delta\mathbf{\check{x}}_{k}}\right|_{\delta\mathbf{\check{x}}_{k}=\mathbf{0}} $$ $$ \mathbf{\Sigma}_{\beta_{s}}=\mathbf{\Sigma}_{\mathbf{n}_{\mathbf{c}_{s}}}+\mathbf{\sigma}_{s}^{2}\cdot\Delta t_{\mathbf{c}_{s}}+\mathbf{\Sigma}_{\mathbf{n}_{\gamma_{s}}}+\mathbf{F}_{\mathbf{p}_{s}}^{o}\mathbf{\Sigma}_{\mathbf{p}_{s}}\mathbf{F}_{\mathbf{p}_{s}}^{o\mathrm{~\tiny~T~}} $$ $$ \mathbf{F^{o}_{\mathbf{p}_{s}}}=\frac{\partial\mathbf{o}\big(\check{\mathbf{x}}_{k},{^{G}\mathbf{p}_{s}},\mathbf{c}_{s}\big)}{\partial{^{G}\mathbf{p}_{s}}} $$ 2) 帧到地图的视觉惯性里程计 ESIKF 更新：公式 (20) 给出了 $\delta\ddot{x}_{k}$ 的另一观测分布，该分布可与 IMU 传播得到的先验分布结合，从而得到 $\delta\ddot{x}_{k}$ 的最大后验概率（MAP）估计： $$ \begin{aligned}\min_{\delta\tilde{\mathbf{x}}_{k}}&\left(\|\tilde{\mathbf{x}}_{k}\boxminus\hat{\mathbf{x}}_{k}+\mathcal{H}\delta\tilde{\mathbf{x}}_{k}\|_{\boldsymbol{\Sigma}_{\delta\hat{\mathbf{x}}_{k}}}^{2}\right.\\+\left.\sum_{s=1}^{m}\left\|\mathbf{o}\big(\tilde{\mathbf{x}}_{k},{}^{G}\mathbf{p}_{s},\mathbf{c}_{s}\big)+\mathbf{H}_{s}^{o}\delta\tilde{\mathbf{x}}_{k}\right\|_{\boldsymbol{\Sigma}_{\boldsymbol{\beta}_{s}}}^{2}\right)\end{aligned} $$ 记 $\mathbf{H}, \mathbf{R}, \check{\mathbf{z}}_k$ 和 $\mathbf{P}$，其定义类似于公式 (12) $\sim$ 和 (14)： $$ \mathbf{H}=\left[\mathbf{H}_{1}^{o T},\ldots,\mathbf{H}_{m}^{o T}\right]^{T},\quad\mathbf{R}=diag(\mathbf{\Sigma}_{\beta_{1}},\ldots,\mathbf{\Sigma}_{\beta_{m}}) $$ $$ \check{\mathbf{z}}_{k}=\left\lceil\mathbf{o}\big(\check{\mathbf{x}}_{k},{}^{G}\mathbf{p}_{1},\mathbf{c}_{1}\big)\ldots,\mathbf{o}\big(\check{\mathbf{x}}_{k},{}^{G}\mathbf{p}_{m},\mathbf{c}_{m}\big)\right\rceil^{T} $$ $$ \mathbf{P}=\left(\mathcal{H}\right)^{-1}\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{k}}(\mathcal{H})^{-T} $$ 接着我们执行与公式(15)和(16)类似的状态更新。这种帧到地图的VIO ESIKF更新过程（见第V-B.1节至第V-B.2节）会反复迭代直至收敛。收敛后的状态估计将用于：(1)渲染地图纹理（见第V-C节）；(2)更新下一帧所使用的当前跟踪点集$\mathcal{P}$（见第V-D节）；(3)作为下一帧LIO或VIO更新中IMU传播的起点。 $$ \begin{array}{r}{\hat{\mathbf{x}}_{k}=\check{\mathbf{x}}_{k},\quad\mathbf{\Sigma}_{\delta\hat{\mathbf{x}}_{k}}=\left(\mathbf{I}-\mathbf{K}\mathbf{H}\right)\mathbf{\Sigma}_{\delta\check{\mathbf{x}}_{k}}}\end{array} $$ ### C. 渲染全局地图的纹理 在帧到地图的VIO更新完成后，我们便能得到当前图像的精确位姿，随后执行渲染功能来更新地图点的颜色。 首先，我们获取所有已激活体素中的所有点（详见第IV节）。假设点的总数为n个，对应的点集为$\zeta = \{\mathbf{P}_1, ..., \mathbf{P}_n\}$。以第s个点的颜色更新过程$\mathbf{P}_s = \left[^G\mathbf{p}_s^T, \mathbf{c}_s^T\right]^T \in \zeta$为例：如果$\mathbf{P}_s$位于当前图像$\mathbf{I}_k$内，我们就可以通过线性插值$\gamma_s$上邻近像素的RGB值，得到其观测颜色$\Sigma_{\mathbf{n}_{\gamma_s}}$以及协方差$\mathbf{I}_k$。新观测到的点颜色会通过贝叶斯更新方式与地图中已记录的原有颜色$\mathbf{c}_s$进行融合，从而得到更新后的颜色及其相关协方差$\mathbf{c}_s$，具体过程如下： $$ \mathbf{\Sigma}_{\mathbf{n}_{\tilde{\mathbf{c}}_{s}}}=\left(\left(\mathbf{\Sigma}_{\mathbf{n}_{\mathbf{c}_{s}}}+\boldsymbol{\sigma}_{s}^{2}\cdot\Delta t_{\mathbf{c}_{s}}\right)^{-1}+\mathbf{\Sigma}_{\mathbf{n}_{\gamma_{s}}}^{-1}\right)^{-1} $$ $$ \begin{array}{r}{\tilde{\mathbf{c}}_{s}=\left(\left(\mathbf{\Sigma}_{\mathbf{n}_{\mathbf{c}_{s}}}+\pmb{\sigma}_{s}^{2}\cdot\Delta t_{\mathbf{c}_{s}}\right)^{-1}\mathbf{c}_{s}+\mathbf{\Sigma}_{\mathbf{n}_{\pmb{\gamma}_{s}}}^{-1}\pmb{\gamma}_{s}\right)^{-1}\mathbf{\Sigma}_{\mathbf{n}_{\tilde{\mathbf{c}}_{s}}}}\end{array} $$ $$ \begin{array}{r}{\mathbf{c}_{s}=\tilde{\mathbf{c}}_{s},\quad\mathbf{\Sigma}_{\mathbf{n}_{\mathbf{c}_{s}}}=\mathbf{\Sigma}_{\mathbf{n}_{\tilde{\mathbf{c}}_{s}}}}\end{array} $$ --- ![Image](images/a5cfe7d38e665e3d6ec8edfd5ba6996eaf28072f116f1c2acc4c50633340f51b.jpg)
Fig. 5: Our handheld device for data collection. (a) shows our minimum system. (b) shows an additional D-GPS RTK system and an ArUco marker board used to evaluate the system accuracy.
### D. VIO子系统的跟踪点更新 完成纹理渲染后，我们便开始更新跟踪点集$\mathcal{P}$。首先，若某点在公式(4)中的投影误差或公式(17)中的光度误差过大，或者该点未落在$\mathcal{P}$内，就将其从$\mathbf{I}_k$中移除。接着，我们将$\zeta$中的每个点投影到当前图像$\mathbf{I}_k$上，如果附近没有其他已跟踪的点（例如半径50像素范围内），就将其加入$\mathcal{P}$。 ### VI. 实验与结果 ### A. 实验设置 1) 用于数据采集的手持设备：我们的数据采集手持设备如图5(a)所示，它包含电源单元、机载DJI Manifold-2C计算平台（配备Intel i7-8550U CPU和8 GB RAM）、FLIR Blackfly BFS-U3-13Y3C全局快门相机以及LiVOX AVIA激光雷达。相机的视场角为82.9°×66.5°，激光雷达的视场角为70.4°×77.2°。为了定量评估算法的精度（见第VI-D节），我们在设备上安装了差分GPS（DGPS）实时动态定位（RTK）系统[^24]，如图5(b)所示。在GPS信号受限的环境中（见第VI-B节和第VI-C节），我们使用ArUco标记[^25]来计算里程计的漂移量。需要注意的是，该设备的所有机械模块均采用FDM打印设计；为了方便读者复现相关工作，我们已在Github上开源了所有原理图及采集的数据集$^{2}$。 2) 参数配置：本文中的所有实验均采用相同的参数设置（详见[^26]处的配置文件）。在初始化公式(1)中的$\mathbf{x}$时，我们将内参和外参值（即$\phi$、$^I\mathbf{R}_C^T$、$^I\mathbf{p}_C^T$）设为离线标定结果，其余状态分量则初始化为零（位姿、速度和偏差）或取首次加速度计读数（重力）。 ### B. 实验1：在激光雷达性能退化且视觉纹理缺失环境下的鲁棒性评估 在本实验中，我们挑战了SLAM领域中最难的场景之一。我们在激光雷达性能退化且视觉纹理匮乏的环境中进行了鲁棒性评估。如图6所示，我们的传感器在穿过狭窄的“T”形通道时会偶尔面对侧壁。当面对仅提供单一平面约束的墙壁时，激光雷达在估计完整位姿方面的能力会显著下降。此外，白色墙壁上的视觉纹理非常有限（图6(a)）。 ![Image](images/6874d3a2ea1532c797cae4f41d85a7d8bde8a6244a8d539130fb05e142989b5b.jpg)
Fig. 6: We test our algorithm in simultaneously LiDAR degenerated and visual texture-less scenarios.
以及图6(c)所示），尤其是针对墙-1而言，其唯一的变化仅为光照条件。由于缺乏可用的激光雷达和视觉特征，这类场景对基于激光雷达和基于视觉的SLAM方法来说都极具挑战性。 通过利用原始图像的像素颜色信息，我们提出的算法能够在此类极端困难的场景中“存活”下来。图8展示了我们估算出的位姿，其中穿过“墙-1”和“墙-2”的阶段分别用紫色和黄色标出。图8还显示了估算出的协方差，该协方差在整个估算轨迹上均被限定在一定范围内，这表明我们的估算质量在整个过程中都保持稳定。传感器被移回起点，此处使用ArUco标记板来获取起点与终点之间的真实相对位姿。与真实的终点位姿相比，我们的算法在旋转方向上的偏差为$1.62^\circ$，在平移方向上的偏差为$4.57 \, \text{cm}$。我们建议读者观看YouTube上的配套视频$^1$，以便更直观地了解实验过程。 ### C. 实验二：高精度绘制大规模室内外城市环境地图 在本实验中，我们展示了算法实时重建大规模校园环境的精确、密集、3D彩色地图的能力。我们在香港科技大学（HKUST）校园内分别沿着不同的行驶轨迹（即轨迹1~4）采集了4次数据，这四条轨迹的总长度分别为1191米、1372米、1524米和1192米。这些轨迹的鸟瞰图（即投影到X-Y平面上）如图7(f)所示。无需任何额外处理（例如闭环检测），这四条轨迹都能自行闭合（见图7(e)）。利用放置在起点的ArUco标记板，里程计偏差数据如表II所示，这表明我们提出的方法精度很高，在长轨迹和复杂环境中产生的偏差很小。最后，我们在图7中展示了“轨迹-1”下重建的地图。更多可视化结果可在项目页面[^27]查看。
TABLE II: Odometry drift in 4 trajectories of Experiment-2.
Traj-1Traj-2Traj-3Traj-4
Length of trajectory (m)1317137215241191
Drift in translation (m)0.0930.1640.1540.102
Drift in rotation (deg)2.1402.3420.2853.925
--- ![Image](images/ce8f85153a7d5372b464914415553364d17cec84648a921d29cc3e038db77605.jpg) ![Image](images/6c00fe2a5aa08dd2b0efe29c713419a85f7fb745d7fcc48f8358d59f349a63db.jpg)
Fig. 7: We show the dense 3D RGB-colored maps reconstructed in real-time in Experiment-2. In (a), we merge our maps with the Google-Earth satellite image and find them aligned well. Details of the map are selectively shown in (b), (c), and (d), respectively. In (e), we show that our algorithm can close the loop itself without any additional processing (e.g., loop-detection and closure), with the return trajectory can return back to the origin. We refer readers to the video on our YouTube $^{1}$. (f) plot the birdview of 4 traveling trajectories.
![Image](images/90190ceb1c1be81ca46dc10877042152a1c701f4af214f2f47d945c2e54eea51.jpg)
Fig. 8: The estimated poses and their 3- $\sigma$ bound with 5 times amplification for better visualization (the light-colored area around the trajectory) of Experiment-1. The shaded areas in purple and yellow are the phases of the sensors facing against the white “wall-1” and “wall-2”, respectively.
![Image](images/ee0ad7d5316835abc0d1005e6023a92985730437507e5c2c08c875fc411eccee.jpg)
Fig. 9: Comparison of the estimated trajectories in Experiments-3.
D. Experiment-3: Quantitative evaluation of precision using D-GPS RTK
在这项实验中，我们在海港（贝尔彻湾海滨步道 [^28]）采集了两段数据序列，利用实时差分GPS（D-GPS）动态定位（RTK）系统提供地面真实轨迹。在这两段序列中，传感器经常面临众多行人，偶尔还会遇到开阔海域，此时激光雷达的测量数据极少。我们将R $^{3}$LIVE算法在不同配置下的估计轨迹（“R3LIVE-HiRES”和“R3LIVE-RT”，详见表III）与“LVI-SAM” [^12]（对其激光雷达前端进行了针对Livox Avia激光雷达的修改）、“R2LIVE” [^11]、“VINS-Mono” [^29]、“Fast-LIO2” [^21]算法的估计轨迹进行对比，并与图9中的地面真实轨迹相比较。可以看出，在两段序列中我们的估计轨迹与地面真实轨迹的吻合度最高。为了进行更定量的比较，我们计算了相对旋转误差（RPE）和相对平移误差（RTE） [^30]，所有长度分别为50、100、150直至300米的子序列对应的结果均展示在表III中。 ### E. 运行时间分析 我们研究了该系统在两种不同平台上的所有实验中的平均耗时：台式电脑（配备Intel i7-9700K CPU和32GB内存）以及无人机机载计算机（“OB”，配备Intel i7-8550u CPU）。
TABLE III: The relative pose error in Experiment-3. In configuration “R3LIVE-HiRES”, we set the input image resolution as $1280 \times 1024$ and the minimum distance (i.e., the resolution of point cloud map) between two points in maps as 0.01 m. In configuration “R3LIVE-RT”, we set the input image resolution as $320 \times 256$ and the minimum distance between two points in maps as 0.10 m.
Relative pose error in Experiments-3 (a)
Sub-sequence RRE/RTE50 m deg / %100 m deg / %150 m deg / %200 m deg / %250 m deg / %300 m deg / %
R3LIVE-HiRES0.99 / 2.250.53 / 1.020.46 / 0.600.29 / 0.310.24 / 0.310.21 / 0.17
R3LIVE-RT1.48 / 2.210.54 / 1.060.49 / 0.600.31 / 0.410.25 / 0.310.22 / 0.23
LVLSAM2.11 / 13.731.04 / 6.690.83 / 5.070.60 / 3.860.47 / 2.980.43 / 2.40
R2LIVE1.21 / 2.470.61 / 1.020.59 / 0.600.34 / 0.340.37 / 0.330.34 / 0.21
FAST-LIO21.36 / 2.350.66 / 0.820.47 / 0.600.37 / 0.360.37 / 0.310.21 / 0.20
VINS-Mono3.03 / 10.411.70 / 7.631.15 / 6.360.89 / 4.340.73 / 3.080.59 / 2.31
Relative pose error in Experiments-3 (b)
Sub-sequence RRE/RTE50 m deg / %100 m deg / %150 m deg / %200 m deg / %250 m deg / %300 m deg / %
R3LIVE-HiRES1.06 / 1.980.58 / 1.340.43 / 0.830.32 / 0.590.27 / 0.270.25 / 0.16
R3LIVE-RT0.98 / 2.080.55 / 1.610.47 / 0.990.39 / 0.650.33 / 0.330.25 / 0.14
LVLSAM2.04 / 3.331.05 / 2.370.81 / 1.530.57 / 1.380.49 / 1.130.44 / 1.01
R2LIVE1.13 / 2.060.65 / 1.450.49 / 0.880.31 / 0.570.26 / 0.300.29 / 0.16
FAST-LIO21.31 / 2.220.69 / 1.340.49 / 0.890.31 / 0.590.26 / 0.310.25 / 0.29
VINS-Mono2.44 / 9.351.31 / 7.280.99 / 4.630.66 / 3.680.56 / 3.240.48 / 2.73
and 8GB RAM). The detailed statistics are listed in Table IV. The time consumption of our VIO subsystem is affected by two main settings: the size of an image ("Image size") and the resolution of point cloud map ("Pc res").
TABLE IV: The average time consumption on two different platforms with different configurations.
VIO per-frame cost timeLIO per-frame cost time
Image size320\times256640\times5121280\times1024
Pc res (m)0.100.050.010.100.050.010.100.050.01
On PC (ms)7.088.4316.7810.3811.7420.6712.1313.1923.1718.40
On OB (ms)15.3021.6943.7923.7134.7047.9930.3842.4361.3439.56
### VII. 结论与未来工作 在本文中，我们提出了一种名为R³LIVE的新型激光雷达-惯性-视觉传感器融合框架，它由两个子系统组成：LIO子系统和VIO子系统。LIO子系统负责构建全局地图的几何结构，而VIO子系统则负责渲染地图的纹理。我们在激光雷达数据退化且视觉纹理缺失的环境中验证了所提系统的鲁棒性，并在多种大规模室内外环境中评估了其精度。实验结果表明，与现有的最先进SLAM系统相比，R³LIVE能够具备更高的鲁棒性和精度。未来，我们将通过加入在线光度校准来进一步提升R³LIVE的性能与鲁棒性。 --- [^1]: J. Levinson, J. Askeland, J. Becker, J. Dolson, D. Held, S. Kammel, J. Z. Kolter, D. Langer, O. Pink, V. Pratt, et al., “Towards fully autonomous driving: Systems and algorithms,” in 2011 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2011, pp. 163–168. [^2]: F. Gao, W. Wu, W. Gao, and S. Shen, "Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments," Journal of Field Robotics, vol. 36, no. 4, pp. 710–733, 2019. [^3]: F. Kong, W. Xu, Y. Cai, and F. Zhang, “Avoiding dynamic small obstacles with onboard sensing and computation on aerial robots,” IEEE Robotics and Automation Letters, vol. 6, no. 4, pp. 7869–7876, 2021. [^4]: Z. Liu, F. Zhang, and X. Hong, "Low-cost retina-like robotic lidars based on incommensurable scanning," IEEE Transactions on Mechatronics, 2021, in press. [^5]: 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, 2021, in press. [^6]: J. Lin, X. Liu, and F. Zhang, “A decentralized framework for simultaneous calibration, localization and mapping with multiple lidars,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 4870–4877. [^7]: Z. Liu and F. Zhang, “Balm: Bundle adjustment for lidar mapping,” IEEE Robotics and Automation Letters, 2021, in press. [^8]: X. Liu and F. Zhang, “Extrinsic calibration of multiple lidars of small fov in targetless environments,” IEEE Robotics and Automation Letters, 2021, in press. [^9]: C. Yuan, X. Liu, X. Hong, and F. Zhang, "Pixel-level extrinsic self calibration of high resolution lidar and camera in targetless environments," arXiv preprint arXiv:2103.01627, 2021. [^10]: 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. [^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,” arXiv preprint arXiv:2104.10831, 2021. [^13]: 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. [^14]: X. Zuo, Y. Yang, J. Lv, Y. Liu, G. Huang, and M. Pollefeys, "Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking," in IROS 2020, 2020. [^15]: W. Zhen and S. Scherer, “Estimating the localizability in tunnel-like environments using lidar and uwb,” in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 4903–4908. [^16]: H. Zhou, Z. Yao, and M. Lu, “Uwb/lidar coordinate matching method with anti-degeneration capability,” IEEE Sensors Journal, vol. 21, no. 3, pp. 3344–3352, 2020. [^17]: C. Debeunne and D. Vivet, “A review of visual-lidar fusion based simultaneous localization and mapping,” Sensors, vol. 20, no. 7, p. 2068, 2020. [^18]: J. Zhang and S. Singh, “Laser-visual-inertial odometry and mapping with high robustness and low drift,” Journal of Field Robotics, vol. 35, no. 8, pp. 1242–1264, 2018. [^19]: W. Shao, S. Vijayarangan, C. Li, and G. Kantor, “Stereo visual inertial lidar simultaneous localization and mapping,” arXiv preprint arXiv:1902.10741, 2019. [^20]: W. Wang, J. Liu, C. Wang, B. Luo, and C. Zhang, "Dv-loam: Direct visual lidar odometry and mapping," Remote Sensing, vol. 13, no. 16, p. 3340, 2021. [^21]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “Fast-lio2: Fast direct lidar-inertial odometry,” arXiv preprint arXiv:2107.06829, 2021. [^22]: T. Qin and S. Shen, “Online temporal calibration for monocular visual-inertial systems,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 3662–3669. [^23]: B. M. Bell and F. W. Cathey, “The iterated kalman filter update as a gauss-newton method,” IEEE Transactions on Automatic Control, vol. 38, no. 2, pp. 294–297, 1993. [^24]: A differential-GPS real-time kinematic (RTK) system: . [^25]: S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez, “Automatic generation and detection of highly reliable fiducial markers under occlusion,” Pattern Recognition, vol. 47, no. 6, pp. 2280–2292, 2014. [^26]: Configuration: . [^27]: Github: . [^28]: Google Map: . [^29]: 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. [^30]: Z. Zhang and D. Scaramuzza, “A tutorial on quantitative trajectory evaluation for visual (-inertial) odometry,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 7244–7251.