Eigen Is All You Need: 高效的具有内部关联的连续时间激光惯性里程计

Eigen Is All You Need: 高效的具有内部关联的连续时间激光惯性里程计

Thien-Minh Nguyen, IEEE 会员，Xinhang Xu，Tongxing Jin，Yizhuo Yang，Jianping Li，Shenghai Yuan，Lihua Xie，IEEE 会士

摘要

本文提出了一种名为 SLICT2 的连续时间激光惯性里程计 (CT-LIO) 系统，主要有两个创新点。首先，与传统认知相反，CT-LIO 算法可以通过线性求解器在仅几次迭代内完成优化，比常用的非线性求解器更加高效。其次，CT-LIO 更依赖于正确的关联过程，而非迭代次数。基于这些理念，我们实现了一种自定义求解器，在每一步增量之后立即执行特征关联过程，并在少数几次迭代内收敛。我们的实现能够在高密度控制点下实现实时性能，并在高动态运动场景中表现出竞争力。我们通过与现有的先进 CT-LIO 方法进行对比，展示了我们方法的优势。为了社区的利益，源代码已在 https://github.com/brytsknguyen/slict 上发布。

I. 引言

连续时间优化近年来已成为基于激光雷达的里程计和建图 (LOAM) 系统的新范式。一般来说，连续时间 LOAM (CT-LOAM) 与离散时间系统的主要区别在于，优化过程中直接使用激光雷达点的原始数据，且状态估计直接在激光雷达点的采样时间上进行表达。相比之下，传统的离散时间系统只在离散的时间点创建若干状态估计，需要使用一些近似技术将激光雷达点转换为与状态估计时间匹配的测量值 (如图 1 所示)。连续时间优化中的即时状态估计可以基于分段线性插值 (PLI) [1], [2] 或 B 样条多项式 [3], [4], [5], [6]。实际上，B 样条相较于分段线性模型具有多项优势，尤其是在快速变化轨迹建模、不规则采样时间的数据融合、无需 IMU 预积分以及天然支持在线时间偏移估计等方面。然而，我们注意到，CT-LOAM 的计算效率仍然是一个瓶颈，尚未得到充分解决。这激发了我们在本文中的研究，以获得对该问题的更多见解。

图 1. 离散时间 (上) 与连续时间 (下) 优化方案的对比。在离散情况下，测量值与滑动窗口内的不同状态估计耦合。通常，测量值并非在状态时间戳处获取，因此需要一些技术来强制执行这种严格的状态-测量耦合模式。例如，IMU 测量值 (绿色小方块) 需要进行预积分，以提供耦合两个连续离散状态 (绿色大方块) 的因素。另一方面，对于连续时间优化，测量值可以通过插值状态直接与轨迹估计耦合。插值状态可以理解为控制点的时间加权组合——即我们优化问题中的实际决策变量。在此图示中，我们暗示了样条阶数 N=3，因此每个插值状态与三个控制点耦合 (粉色段)。

对于背景介绍，LOAM 系统通常使用一些非线性求解器 (NLS)，通过 Gauss-Newton 方法优化最小二乘成本函数，其中 Ceres-GTSAM-g2o [7], [8], [9] 是主要的三大求解器。基本上，这些求解器旨在解决大型最小二乘问题，其中每个平方误差项都是局部构建和声明的。使用 NLS 具有多个优势。最重要的是，它们允许以分而治之的方式处理问题，用户只需专注于描述个别因子的残差和雅可比矩阵，而这些残差和雅可比矩阵在全局范围内的排列由求解器自动管理。此外，NLS 还提供了额外的功能，如抑制异常值的鲁棒函数、利用问题稀疏性的选项，以及对优化过程的深入诊断。

我们还注意到，在紧耦合 CT-LOAM 系统中，Ceres 通常被广泛使用。我们认为这是因为 GTSAM 和 g2o 最初是为视觉里程计问题开发的，它们带有复杂的因子图。然而，这些因子应当在图的整个生命周期内保持固定，并可用于短期（里程计）和长期（捆绑调整）优化过程。另一方面，LOAM 问题具有更简单的因子图，专注于短滑动窗口，当地图更新时，每个因子的平面/边缘系数也会改变。因此，每次更新地图时，成本函数都需要重新构建。

SLICT2 的灵感来自我们之前的工作 SLICT [2]，我们的主要目标是将连续时间轨迹的表达从 PLI 升级到 B 样条，以获得前面提到的优势，同时通过多分辨率增量映射和特征关联方案保持竞争性能。遵循传统智慧，我们最初在 B 样条优化过程中使用了 NLS。然而，SLICT 中的非实时问题仍然存在。实际上，实时性问题不仅限于 SLICT，大多数 CT-LOAM 方法也存在这一问题。经过深入调查，我们注意到，NLS 在构建最小二乘问题时会产生显著的时间开销。此外，内部程序为了确保成本函数的单调减少也需要额外时间，使得 NLS 缺乏吸引力。在寻求实时问题的解决方案时，我们发现一个简单直接的方法能带来更好的效果。具体来说，通过放弃 NLS 方法并直接基于 Eigen 库（一个在机器人研究中广泛使用的线性代数操作库）实现流形优化求解器，CT-LIO 问题可以在几次迭代内收敛。事实上，经过第一次迭代后，状态估计已经足够好，可以进行更好的关联。因此，我们将这些过程组合成求解-关联-求解循环，可以在 2 或 3 次迭代内收敛。有效地，我们的 CT-LIO 系统实现了时间效率（实验表明它的速度可以比 NLS 快 8 倍）和竞争精度。此外，即使控制点和激光雷达因子的数量非常密集，也没有性能妥协，而这是我们在其他工作中发现的一个问题。总结来说，SLICT2 的贡献如下：

• 基于 Eigen 库的线性求解器和每次迭代中应用的内部特征到地图的关联，实现了高效的实时 CT-LIO 流水线，迭代次数少。
• 详细的从局部到全局的残差和激光雷达及惯性因子的雅可比矩阵的计算表达，确保了 SLICT2 的计算效率。
• 通过广泛的实验和分析展示了 SLICT2 相较于现有最先进（SOTA）的 CT-LIO 方法的竞争性能，并深入调查了每个子过程的时间成本。
• 我们将本工作开源，惠及社区。

本文其余部分的结构如下：在 Sec. II 中，我们介绍了系统的基本符号和理论背景。Sec. III 提供了 SLICT2 流水线的主要步骤描述，以及激光雷达和惯性因子的残差和雅可比矩阵的计算。在 Sec. IV 中，我们与其他 SOTA 连续时间激光雷达里程计方法进行了比较，随后深入研究了我们的算法，探讨了每个底层过程的时间开销。Sec. V 总结了我们的工作。

II. 预备知识

A. 符号

对于一个量 $\mathbf{x}$，我们使用符号 $\breve{\mathbf{x}}$ 表示 $\mathbf{x}$ 的测量值，使用符号 $\hat{\mathbf{x}}$ 表示其估计值。对于矩阵 $\mathbf{x}$，${\mathbf{x}}^{\top }$ 表示其转置。对于 $\mathbf{r}\in {\mathbb{R}}^n$ 和 $\mathbf{M}\in {\mathbb{R}}^{n\times n}$，我们表示 $\Vert \mathbf{r}{\Vert }_{\mathbf{M}}^2\triangleq {\mathbf{r}}^{\top }\mathbf{Mr}$。对于变换矩阵 $\mathbf{T}\in \mathrm{SE}(3)$，我们也使用表达式 $\mathbf{T}=(\mathbf{R},\mathbf{p})$，其中 $\mathbf{R}\in \mathrm{SO}(3)$ 和 $\mathbf{p}\in {\mathbb{R}}^3$ 分别是 $\mathbf{T}$ 的旋转和位移部分。

我们将李代数群（斜对称矩阵群）记为 $\mathfrak{so}(3)$，并使用 $(\cdot {)}_{\times }$ 和 $(\cdot {)}_{\vee }$ 表示映射 $(\cdot {)}_{\times }:{\mathbb{R}}^3\to \mathfrak{so}(3)$ 和 $(\cdot {)}_{\vee }:\mathfrak{so}(3)\to {\mathbb{R}}^3$。此外，映射 Exp: ${\mathbb{R}}^3\to \mathrm{SO}(3)$ 及其逆映射 Log: $\mathrm{SO}(3)\to {\mathbb{R}}^3$ 也将在后续部分中用到。我们还使用算子 $⊞:\mathcal{M}\times {\mathbb{R}}^n\to \mathcal{N}$ 表示流形上的广义加法运算。

我们回顾了 $\mathrm{SO}(3)$ 的右雅可比矩阵 $J_r$ 和逆右雅可比矩阵 $J_r^{-1}$，定义见文献 [10], [11]。它们的闭式公式如下：
$$J_r(\mathbf{\varphi })=\mathbf{I}-\frac{1-\cos (\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^2}{\mathbf{\varphi }}_{\times }+\frac{\Vert \varphi \Vert -\sin (\Vert \varphi \Vert )}{\Vert \varphi {\Vert }^3}({\varphi }_{\times }{)}^2,$$
$$J_r^{-1}(\mathbf{\varphi })=\mathbf{I}+\frac{1}{2}{\mathbf{\varphi }}_{\times }+\left(\frac{1}{\Vert \mathbf{\varphi }{\Vert }^2}-\frac{1+\cos (\Vert \mathbf{\varphi }\Vert )}{2\Vert \mathbf{\varphi }\Vert \sin (\Vert \mathbf{\varphi }\Vert )}\right)({\mathbf{\varphi }}_{\times }{)}^2,$$
对于 $\varphi \in {\mathbb{R}}^3$。

B. 基于 B 样条的插值

$B$ 样条是由以下参数定义的分段多项式：节点间隔 $\Delta t$，样条阶数 $N$（或其等价的次数 $D\triangleq N-1$），以及控制点集合 { T m } m = 0 M , T m ∈ SE ⁡ ( 3 ) \left\{\mathbf{T}_{m}\right\}_{m=0}^{M}, \mathbf{T}_{m} \in \operatorname{SE}(3) {Tm​}m=0M​,Tm​∈SE(3)。每个控制点 ${\mathbf{T}}_m$ 对应一个节点，记为 $t_m$，其中 $t_m=t_0+m\Delta t$。给定 $N$，可计算混合矩阵 ${\mathbf{B}}^{(N)}$ 及其累积形式 ${\tilde{\mathbf{B}}}^{(N)}$，如下所示：

B ( N ) = [ b m , n ] ∈ R N × N , m , n ∈ { 0 , … N − 1 } , (1) \mathbf{B}^{(N)} = \left[ b_{m,n} \right] \in \mathbb{R}^{N \times N}, m,n \in \{ 0, \dots N-1 \}, \tag{1} B(N)=[bm,n​]∈RN×N,m,n∈{0,…N−1},(1)
$${\tilde{\mathbf{B}}}^{(N)}=\left[{\tilde{b}}_{m,n}\right],{\tilde{b}}_{m,n}=\sum _{j=m}^D{b}_{j,n},\text{\hspace{1em}(2)}$$
$$b_{m,n}=\frac{1}{n!(D-n)!}\sum _{l=m}^D(-1{)}^{l-m}{C}_N^{l-m}(D-l{)}^{D-n},\text{\hspace{1em}(3)}$$

其中 $C_D^n=\frac{D!}{n!(D-n)!}$ 表示二项式系数。

从控制点和节点集合出发，对于任意时间 $t\in \left[t_0,t_{M-N+2}\right]$，即某个测量的采样时间，我们可以通过以下过程插值出此时的位姿 $\mathbf{T}(t)=(\mathbf{R}(t),\mathbf{p}(t))$：

s = ( t − t i ) / Δ t , t ∈ [ t i , t i + 1 ) , t i ∈ { t m } m = 0 M , (4) s = \left(t - t_{i}\right) / \Delta t, \quad t \in \left[t_{i}, t_{i+1}\right), \quad t_{i} \in \left\{ t_{m} \right\}_{m=0}^{M}, \tag{4} s=(t−ti​)/Δt,t∈[ti​,ti+1​),ti​∈{tm​}m=0M​,(4)
$${\left[{\lambda }_0,{\lambda }_1,\dots ,{\lambda }_{N-1}\right]}^{\top }={\mathbf{B}}^{(N)}{\left[1,s,\dots ,s^{N-1}\right]}^{\top },\text{\hspace{1em}(5)}$$
$${\left[{\tilde{\lambda }}_0,{\tilde{\lambda }}_1,\dots ,{\tilde{\lambda }}_{N-1}\right]}^{\top }={\tilde{\mathbf{B}}}^{(N)}{\left[1,s,\dots ,s^{N-1}\right]}^{\top },\text{\hspace{1em}(6)}$$
$$\mathbf{R}(t)={\mathbf{R}}_i\prod _{j=1}^{N-1}\mathrm{Exp}\left({\tilde{\lambda }}_j\log \left({\mathbf{R}}_{i+j-1}^{-1}{\mathbf{R}}_{i+j}\right)\right),\text{\hspace{1em}(7)}$$
$$\mathbf{p}(t)={\mathbf{p}}_i+\sum _{j=1}^{N-1}{\lambda }_j{\mathbf{p}}_{i+j}.\text{\hspace{1em}(8)}$$

换句话说，给定时间 $t$，我们首先找到包含 $t$ 的节点 $t_i$ 及对应的区间 $\left[t_i,t_{i+1}\right)$，然后计算归一化时间 $s$，如步骤 (4) 所述。接下来，通过混合矩阵和 $s$ 的幂次计算缩放系数 ${\lambda }_i,{\tilde{\lambda }}_i,i=0,\dots ,N-1$，如步骤 (5) 和 (6) 所述。最后，通过缩放系数和节点 $t_i,\dots ,t_{i+N-1}$ 处的 $N$ 个控制点计算时间 $t$ 时的插值位姿 $(\mathbf{R}(t),\mathbf{p}(t))$，如 (7) 和 (8) 所述。

注释 1：注意，对于一个预定义的样条，上述过程仅适用于采样时间 $t$ 不超过 $t_{M-N+2}$ 的情况。反之，为了表示在区间 $\left[t_{\min },t_{\max }\right]$ 上的轨迹，采用节点间隔 $\Delta t$ 的 $B$ 样条，我们设定 $t_0=t_{\min }$，最后一个节点的索引为 $M=\lceil (t_{\max }-t_{\min })/\Delta t\rceil +N-2$。在图 1 中，展示了一个滑动窗口，$M=4$ 且 $N=3$。由于 $N=3$，必须在 $t_{k+1}$ 处添加一个控制点，才能在包含所有测量的区间 $\left[t_w,t_k\right]$ 上完全定义 $B$ 样条。在我们的工作中，使用了 basalt ${}^2$ 库 [12] 来辅助 $B$ 样条的构建。

为了方便后续的残差和雅可比计算，我们还定义了以下量：

$${\mathbf{d}}_j=\log \left({\mathbf{R}}_{i+j-1}^{-1}{\mathbf{R}}_{i+j}\right),j=1,\dots ,N-1,\text{\hspace{1em}(9)}$$
$${\mathbf{f}}_j={\tilde{\lambda }}_j{\mathbf{d}}_j,j=1,\dots ,N-1.\text{\hspace{1em}(10)}$$

C. 观测模型

大多数传感模式可以用以下观测模型表述：

$$h(\mathcal{X},\mathcal{Z},\mathcal{M},\eta )=0\text{\hspace{1em}(15)}$$

其中，$\mathcal{X}$ 是需要估计的状态，$\mathcal{Z}$ 是传感器的某些测量值，$\mathcal{M}$ 是先验信息，$\eta$ 表示测量中的噪声或建模误差。

¹ 如果我们用 $\hat{\mathcal{X}}$ 表示 $\mathcal{X}$ 的估计值，$h(\hat{\mathcal{X}},\mathcal{Z},\mathcal{M},0)$ 可以称为残差。在某些情况下，$\mathcal{M}$ 可以省略或视为观测模型 $h(\cdot )$ 的一部分。因此，我们可以写作 $h(\mathcal{X},\mathcal{Z},\eta )=0$ 和 $h(\hat{\mathcal{X}},\mathcal{Z},0)$ 分别作为观测模型和残差。

在 CT-LIO 的情况下，IMU 和激光雷达的观测模型可以表示如下：

$$\begin{array}{rl}& {\left({\mathbf{R}}_t^{-1}{\dot{\mathbf{R}}}_t\right)}_{\vee }+{\mathbf{b}}_{\omega }+{\mathbf{\eta }}_{\omega }-{\breve{\mathbf{\omega }}}_t=0\\ & {\mathbf{R}}_t^{-1}\left({\ddot{\mathbf{p}}}_t+\mathbf{g}\right)+{\mathbf{b}}_a+{\mathbf{\eta }}_a-{\breve{\mathbf{a}}}_t=0\\ & {\mathbf{n}}^{\top }\left({\mathbf{R}}_t{}^{B_t}\breve{\mathbf{f}}+{\mathbf{p}}_t\right)+\mu -{\eta }_l=0,\end{array}\text{\hspace{1em}(16)(17)(18)}$$

其中，$\mathbf{R}$ 和 $\mathbf{p}$ 分别是姿态和位置状态；${\breve{\mathbf{\omega }}}_t$、${\breve{\mathbf{a}}}_t$ 和 ${}^{B_t}\mathbf{f}$ 分别是时间点 $t$ 的角速度、加速度和激光雷达测量值；${\mathbf{b}}_{\omega }$ 和 ${\mathbf{b}}_a$ 分别是 IMU 的偏置；${\mathbf{\eta }}_{\omega }$、${\mathbf{\eta }}_a$ 和 ${\eta }_l$ 分别是测量噪声；$\mathbf{g}$ 是重力常数；$\mathbf{n}$ 和 $\mu$ 是与 $\breve{\mathbf{f}}$ 关联的地图中的平面系数。

为简化表示，在后续部分中我们将分别使用 $r_{\mathcal{L}}\triangleq h\left(\hat{\mathcal{X}},{}^B\mathbf{f},0\right)$ 和 $r_{\mathcal{I}}\triangleq h\left(\hat{\mathcal{X}},\left({\mathbf{\omega }}_t,{\mathbf{a}}_t\right),0\right)$ 来表示激光雷达和 IMU 的残差。

D. 状态估计

机器人的轨迹由样条表示，起始于时间 $t_0$，并随着新数据的获取而扩展。为保持问题有界，我们仅优化对应于最近时间区间 $\left[t_s,t_e\right]$ 的滑动窗口内的控制点。具体来说，待估计的状态为：

$$\mathcal{X}=\left({\mathbf{T}}_w,{\mathbf{T}}_{w+1},\dots {\mathbf{T}}_{k+N-2},{\mathbf{b}}_{\omega },{\mathbf{b}}_a\right)\text{\hspace{1em}(19)}$$

其中，${\mathbf{T}}_w,{\mathbf{T}}_{w+1},\dots {\mathbf{T}}_{k+N-2}$ 是滑动窗口内的控制点，且 $\left[t_s,t_e\right]\subset \left[t_w,t_{k+N-2}\right]$，${\mathbf{b}}_{\omega }$ 和 ${\mathbf{b}}_a$ 是 IMU 的偏置。

与离散时间情况下通常包含速度状态不同，在 CT-LIO 中，速度隐含在位置状态的一阶导数中。需要注意的是，用户还可以包含其他状态，例如局部重力常数、激光雷达与 IMU 的偏移以及时间戳偏移。然而，这些额外特性超出了本文的讨论范围，我们相信用户可以根据需要轻松修改 SLICT2 的基本管道来处理这些问题。

III. 方法论

A. 主要工作流程

图 2 展示了 SLICT2 的主要工作流程。这些块的编号与下面的部分相对应。蓝色的块 $(1,2.1,2.2,4)$ 受 SLICT 的启发，简要回顾了背景信息。SLICT2 的原创贡献以红色突出显示 (3.1, 3.2, 3.3)，将详细阐述。

1. 同步：在同步过程中，IMU 和激光雷达数据被缓冲并组织成所谓的捆包。每个捆包由一组原始激光雷达点 $\left\{{}^{B_t}\mathbf{f},\dots \right\}$ 和一组 IMU 测量 { ( ω ˘ t , a ˘ t ) , … } \left\{\left(\breve{\omega}_{t}, \breve{\mathbf{a}}_{t}\right), \ldots\right\} {(ω˘t​,a˘t​),…} 组成。我们还仔细跟踪时间戳，以保持滑动窗口的时间限制。
   
   图 2. SLICT2 是一个 CT-LIO 系统，具有简单而高效的管道。$i$ 是内循环的索引，从 0 开始。我们将序列 $3.1-3.2-3.3-2.2-3.1$ 称为内循环，外循环则指 $2.1-$ (内循环) - 2.1。每个块的描述见第 III-A 节。

2. 传播-去偏-关联：大多数 LIO 方法采用点到平面观察模型 $\mathbf{nf}+\mu =0$。为了获取每个特征 $\breve{\mathbf{f}}$ 的平面系数 $(\mathbf{n},\mu )$，需要进行以下所谓的 PDA（传播-去偏-关联）过程：

• IMU 传播：当获得一组新的测量值 ( { B t f ˘ f , … } , { ( ω ˘ t , a ˘ t ) , … } ) \left(\left\{{ }^{B_{t}} \breve{\mathbf{f}}_{\mathbf{f}}, \ldots\right\},\left\{\left(\breve{\boldsymbol{\omega}}_{t}, \breve{\mathbf{a}}_{t}\right), \ldots\right\}\right) ({Bt​f˘f​,…},{(ω˘t​,a˘t​),…}) 时，我们将使用 IMU 测量 { ( ω ˘ t , a ˘ t ) , … } \left\{\left(\breve{\omega}_{t}, \breve{a}_{t}\right), \ldots\right\} {(ω˘t​,a˘t​),…} 来传播状态，并获得每个 IMU 采样时间内的姿态。这一过程依赖于第一个采样的状态初始猜测。这也是我们在每次迭代后重新进行传播的原因，因为可以获得更好的状态估计。

• 去偏（运动去失真）：在此步骤中，${}_{\breve{B_t}}{}_{\mathbf{f}}$ 的世界参考坐标 ${}_{\breve{\mathbf{R}}}{}_{\mathbf{f}}$ 通过变换 ${}^{\text{W}}\breve{\mathbf{f}}={\breve{\mathbf{R}}}_t{}^{B_t}\breve{\mathbf{f}}+{\breve{\mathbf{p}}}_t$ 获取，其中 $\left({\breve{\mathbf{R}}}_t,{\breve{\mathbf{p}}}_t\right)$ 是激光雷达点 $\breve{\mathbf{f}}$ 在采样时间 $t$ 的姿态估计。该姿态估计可以通过对样条进行采样（对于旧捆包）或使用 IMU 传播的姿态（对于最新捆包）来获取。同样，由于传播步骤的不准确性，去偏过程可能并不完美。因此，在获得更好的状态估计后，重新进行该过程是有益的。

• 关联：值 ${}^{\mathrm{W}}\mathbf{f}$ 用于在地图中查找邻域（可以通过 kNN 搜索 [13], [14] 或接近 surfel [15], [2] 来完成）。基于邻域中点的分布，可以计算平面系数 $(\mathbf{n},\mu )$ 并与 $\breve{\mathbf{f}}$ 关联，以构建点到平面的因子。由于这两个步骤需要同时完成，因此我们将上述两个步骤统称为关联过程。

备注 2：显然，PDA 过程中的所有步骤都依赖于某种初始轨迹估计，该估计在彻底优化之前可能并不准确。解决此问题的一种方法是添加所有可能可关联的平面 [16]。然而，这种方法在计算上可能比较繁重，错误的关联仍会对整体解产生影响。因此，在 SLICT2 中，我们选择在每次更新后简单地重新进行关联。经过研究发现，在更新步骤能够更高效地完成的情况下，这种重新进行 PDA 过程的方式是可行的。

3. 构建、求解与更新：当滑动窗口中的所有测量值 $\mathcal{Z}$ 都已考虑后，我们可以开始所谓的构建-求解-更新（BSU）过程。本质上，这一过程通过调整状态估计 $\hat{\mathcal{X}}$ 来最小化代价函数 $f$，如下所示：

$$\begin{array}{rl}f=\sum & {\Vert r_{\mathcal{L}}\left(\hat{\mathcal{X}},{}^{B_t}\breve{\mathbf{f}}\right)\Vert }_{{\mathbf{w}}_{\mathcal{L}}}^2\\ & +\sum {\Vert r_{\mathcal{I}}\left(\hat{\mathcal{X}},{\breve{\omega }}_t,{\breve{\mathbf{a}}}_t\right)\Vert }_{{\mathbf{W}}_{\mathcal{I}}}^2\triangleq \Vert h(\hat{\mathcal{X}},\mathcal{Z}){\Vert }_{\mathbf{W}}^2\end{array}\text{\hspace{1em}(20)}$$

其中 ${\mathbf{r}}_{\mathcal{I}}$ 和 ${\mathbf{r}}_{\mathcal{L}}$ 分别是 IMU 和激光雷达的残差，${\mathbf{W}}_{\mathcal{I}}$，${\mathbf{W}}_{\mathcal{L}}$，$\mathbf{W}$ 是一些权重矩阵（通常为协方差的逆），$h(\hat{\mathcal{X}},\mathcal{Z})$ 是全局范围内的联合残差向量。

为了最小化 (20)，我们从初始猜测 ${\hat{\mathcal{X}}}^0$ 开始，然后通过小扰动 ${\delta }^{\ast }\triangleq$ ${\mathrm{argmin}}_{\delta }f\left({\hat{\mathcal{X}}}^i⊞\delta \right),i=1,2,\dots$ 迭代更新 ${\hat{\mathcal{X}}}^i$ 多个步骤。为了在每次迭代中求解问题 ${\delta }^{\ast }\triangleq {\mathrm{argmin}}_{\delta }f\left({\hat{\mathcal{X}}}^i⊞\delta \right)$，我们在 ${\hat{\mathcal{X}}}^i$ 处进行线性化，如下所示：

$$\begin{array}{rl}f\left({\hat{\mathcal{X}}}^i⊞\delta \right)& ={\Vert h\left({\hat{\mathcal{X}}}^i⊞\delta ,\mathcal{Z}\right)\Vert }_{\mathbf{W}}^2\approx \Vert \mathbf{r}+\mathbf{J}\delta {\Vert }_{\mathbf{W}}^2\\ & ={\delta }^{\top }{\mathbf{J}}^{\top }\mathbf{WJ}\delta +2{\delta }^{\top }{\mathbf{J}}^{\top }\mathbf{Wr}+{\mathbf{r}}^{\top }\mathbf{Wr}\end{array}\text{\hspace{1em}(21)}$$

其中 $\mathbf{r}=h\left({\hat{\mathcal{X}}}^i,\mathcal{Z}\right)$，$\mathbf{J}$ 是雅可比矩阵 ${\frac{\partial h\left({\hat{\mathcal{X}}}^i⊞\delta ,\mathcal{Z}\right)}{\partial \delta }|}_{\delta =0}$。关于 $\mathbf{r}$ 和 $\mathbf{J}$ 的详细信息将在第 III-B 节中讨论。对于最小化 (21)，可以得到线性系统的解：

$$\begin{array}{c}{\mathbf{J}}^{\top }\mathbf{WJ}\delta =-{\mathbf{J}}^{\top }\mathbf{Wr}\end{array}\text{\hspace{1em}(22)}$$

通过最小化该系统中的 $f$，我们得到新的状态估计 ${\hat{\mathcal{X}}}^{i+1}$，即 ${\hat{\mathcal{X}}}^{i+1}\triangleq {\hat{\mathcal{X}}}^i⊞{\delta }^{\ast }$。

4. 恢复：在进行完 BSU 过程后，我们可以对状态进行恢复。为此，我们从时间 $t=0$ 开始提取每个 IMU 的测量值，通过已优化的 IMU 状态对 IMU 的 IMU 姿态进行恢复。恢复的过程如下：从 $t=0$ 开始，通过从后向前（${\hat{\mathbf{p}}}_t-{\hat{\mathbf{p}}}_{t-1}$）和前向传播（${\hat{\mathbf{p}}}_{t-1}+{\mathbf{p}}_t-{\hat{\mathbf{p}}}_t$）获取每个时间步骤 $t$ 的所有状态，从而获得初始位置。实际上，这些状态也是在设计过程中作为初始点的，但在跟踪操作中使用。

B. 基于B样条的因子的残差和雅可比矩阵

在本节中，我们提供了激光雷达和惯性因子的残差和解析雅可比矩阵的详细计算，这对于确保该方法的实时性能至关重要。

1. 激光雷达因子：对于每个激光雷达特征 ${}^{B_t}\breve{\mathbf{f}}$，基于观察模型（18）和第IIB节中的插值规则，激光雷达因子的残差可以明确地表示为状态估计（控制点）和测量值的函数，如下所示：

$$\begin{array}{rl}{r}_{\mathcal{L}}={\mathbf{n}}^{\top }[{\hat{\mathbf{R}}}_i& \prod _{j=1}^{N-1}\mathrm{Exp}\left({\tilde{\lambda }}_i\log \left({\hat{\mathbf{R}}}_{i+j-1}^{-1}{\hat{\mathbf{R}}}_{i+j}\right)\right){}^{B_t}\breve{\mathbf{f}}\\ & +{\hat{\mathbf{p}}}_i+\sum _{j=1}^{N-1}{\lambda }_i\left({\hat{\mathbf{p}}}_{i+j}-{\hat{\mathbf{p}}}_{i+j-1}\right)]+\mu \end{array}$$

其中 $t\in \left[t_i,t_{i+1}\right)$。为了更方便处理，我们可以将（23）写为：

$$\begin{array}{c}{r}_{\mathcal{L}}={\mathbf{n}}^{\top }\left({\hat{\mathbf{R}}}_t{}^{{\mathrm{B}}_t\breve{\mathbf{f}}}+{\hat{\mathbf{p}}}_t\right)+\mu \end{array}\text{\hspace{1em}(23)}$$

其中 $\left({\hat{\mathbf{R}}}_t,{\hat{\mathbf{p}}}_t\right)$ 是一个辅助状态估计，是与 $\breve{\mathbf{f}}$ 相关的决策变量（控制点）的组合。显然，每个因子 $r_{\mathcal{L}}$ 都与 $N$ 个控制点相关，因此其对状态估计 $\hat{\mathcal{X}}$ 的雅可比矩阵具有以下形式：

$$\frac{\partial r_{\mathcal{L}}}{\partial \hat{\mathcal{X}}}=\left[\begin{array}{llll}\cdots & \frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{R}}}_{i+j}}& \frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{p}}}_{i+j}}& \cdots \end{array}\right]\text{\hspace{1em}(24)}$$

对于 $i\in [w,k+N-2]$，使得 $\left[t_i,t_i+1\right)\ni t,t_i\in$ { t s , … t k + N − 2 } , j ∈ { 0 , N − 1 } \left\{t_{s}, \ldots t_{k+N-2}\right\}, j \in\{0, N-1\} {ts​,…tk+N−2​},j∈{0,N−1}。图3中提供了该雅可比矩阵的示意图。要计算 $r_{\mathcal{L}}$ 对于 ${\hat{\mathbf{R}}}_{i+j}$ 和 ${\hat{\mathbf{p}}}_{i+j}$ 的雅可比矩阵，我们应用链式法则如下：

$$\frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{R}}}_{i+j}}=\frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{R}}}_t}\frac{\partial {\hat{\mathbf{R}}}_t}{\partial {\hat{\mathbf{R}}}_{i+j}},\frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{p}}}_{i+j}}=\frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{p}}}_t}\frac{\partial {\hat{\mathbf{p}}}_t}{\partial {\hat{\mathbf{p}}}_{i+j}}$$

然后，现在的任务是计算雅可比矩阵 $\frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{R}}}_t},\frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{p}}}_t}$，$\frac{\partial {\hat{\mathbf{R}}}_t}{\partial {\hat{\mathbf{R}}}_m}$ 和 $\frac{\partial {\hat{\mathbf{p}}}_t}{\partial {\hat{\mathbf{p}}}_m}$。前两者可以很容易地计算为：

$$\begin{array}{c}\frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{R}}}_t}={\mathbf{n}}^{\top }{\hat{\mathbf{R}}}_t^{{\mathrm{B}}_t}{\breve{\mathbf{f}}}_{\times },\frac{\partial r_{\mathcal{L}}}{\partial {\hat{\mathbf{p}}}_t}={\mathbf{n}}^{\top }\end{array}\text{\hspace{1em}(25)}$$

而 $\frac{\partial {\hat{\mathbf{R}}}_t}{\partial {\hat{\mathbf{R}}}_m}$ 和 $\frac{\partial {\hat{\mathbf{p}}}_t}{\partial {\hat{\mathbf{p}}}_m}$ 可以通过以下方式找到：

$$\begin{array}{rl}\frac{\partial {\mathbf{R}}_t}{\partial {\mathbf{R}}_{i+j}}=& {\tilde{\lambda }}_j{\mathbf{P}}_j{J}_r\left({\tilde{\lambda }}_j{\mathbf{d}}_j\right)J_r^{-1}\left({\mathbf{d}}_j\right)\\ & -{\tilde{\lambda }}_{j+1}{\mathbf{P}}_{j+1}{J}_r\left({\tilde{\lambda }}_{j+1}{\mathbf{d}}_{j+1}\right)J_r^{-1}\left(-{\mathbf{d}}_{j+1}\right)\\ \frac{\partial {\mathbf{p}}_t}{\partial {\mathbf{p}}_{i+j}}=& {\lambda }_{j}I\end{array}\text{\hspace{1em}(26)(27)}$$

方程（26）和（27）是根据Sommer等人的研究成果[12]以更简洁和自包含的公式进行修订的。请注意，对于边界情况 $j=0$，我们保持（26）可以通过定义 ${\mathbf{A}}_0={\mathbf{R}}_i$ ${}_{\tilde{\lambda }}^{\text{和 }}{\mathbf{d}}_0=\log \left({\mathbf{R}}_i\right)$ 来应用；而当 $j=N-1$ 时，我们定义 ${\tilde{\lambda }}_N=0$，因此可以将（26）中的第二项归零。

2. 惯性因子：惯性因子的残差定义为：

$$r_{\mathcal{I}}=\left[\begin{array}{c}{r}_{\omega }\\ r_a\\ r_{b_{\omega }}\\ r_{b_a}\end{array}\right]=\left[\begin{array}{c}{\hat{\mathbf{\omega }}}_t+{\hat{\mathbf{b}}}_{\omega }-{\breve{\mathbf{\omega }}}_t\\ {\hat{\mathbf{a}}}_t+{\hat{\mathbf{b}}}_a-{\breve{\mathbf{a}}}_t\\ {\hat{\mathbf{b}}}_{\omega }-{\overline{\mathbf{b}}}_{\omega }\\ {\hat{\mathbf{b}}}_a-{\overline{\mathbf{b}}}_a\end{array}\right]\in {\mathbb{R}}^{12}\text{\hspace{1em}(28)}$$

其中 ${\hat{\mathbf{\omega }}}_t$ 和 ${\hat{\mathbf{a}}}_t$ 是预测的角速度和机体参考加速度，${\bar{b}}_{\omega }$ 和 ${\bar{b}}_a$ 是一些偏置的先验值，它们与前一个外部循环结束时的估计值 ${\hat{\mathbf{b}}}_{\omega }$ 和 ${\hat{\mathbf{b}}}_a$ 相同。请注意，${\hat{\omega }}_t$ 可以通过公式（14）和（13）计算，${\hat{\mathbf{a}}}_t$ 可以从（10）和（8）获得。

我们看到 $r_{\mathcal{I}}$ 与 $N$ 个控制点和两个偏置状态相关。因此其对状态估计（19）的雅可比矩阵将具有以下形式：

$$\frac{\partial r_{\mathcal{I}}}{\partial \hat{\mathcal{X}}}=\left[\begin{array}{cccccc}\frac{\partial r_{\omega }}{\partial {\hat{\mathbf{R}}}_{i+j}}& \frac{\partial r_{\omega }}{\partial {\hat{\mathbf{p}}}_{i+j}}& \frac{\partial r_{\omega }}{\partial {\hat{b}}_{\omega }}& \frac{\partial r_{\omega }}{\partial {\hat{b}}_a}& & \\ \frac{\partial r_a}{\partial {\hat{\mathbf{R}}}_{i+j}}& \frac{\partial r_a}{\partial {\hat{\mathbf{p}}}_{i+j}}& 0& 0& & \\ \dots & \dots & \dots & \dots & & \\ \frac{\partial r_{b_{\omega }}}{\partial {\hat{\mathbf{R}}}_{i+j}}& \frac{\partial r_{b_{\omega }}}{\partial {\hat{\mathbf{p}}}_{i+j}}& \dots & \frac{\partial r_{b_{\omega }}}{\partial {\hat{b}}_{\omega }}& \frac{\partial r_{b_a}}{\partial {\hat{b}}_a}& \\ \dots & \frac{\partial r_a}{\partial {\hat{\mathbf{R}}}_{i+j}}& \frac{\partial r_a}{\partial {\hat{\mathbf{p}}}_{i+j}}& \dots & 0& 0\end{array}\right]$$

其中雅可比矩阵 $\frac{\partial r_{\mathcal{I}}}{\partial {\hat{\mathbf{R}}}_t}$ 和 $\frac{\partial r_{\mathcal{I}}}{\partial {\hat{\mathbf{p}}}_t}$ 可以如下计算：

$$\frac{\partial r_{\omega }}{\partial {\hat{\mathbf{R}}}_t}=\left[\begin{array}{cc}{\hat{\mathbf{R}}}_t^{\mathrm{B}}& 0\\ 0& 0\end{array}\right]\text{\hspace{1em}(29)}$$

$$\frac{\partial r_a}{\partial {\hat{\mathbf{R}}}_t}=\left[\begin{array}{cc}0& {\hat{\mathbf{R}}}_t^{\mathrm{B}}\\ 0& 0\end{array}\right]\text{\hspace{1em}(30)}$$

然后我们可以得到

$$\frac{\partial r_{\mathcal{I}}}{\partial \hat{\mathcal{X}}}=\left[\begin{array}{cc}\frac{\partial r_{\mathcal{I}}}{\partial {\hat{\mathbf{R}}}_t}& \frac{\partial r_{\mathcal{I}}}{\partial {\hat{\mathbf{p}}}_t}\\ \frac{\partial r_{\mathcal{I}}}{\partial {\hat{b}}_{\omega }}& \frac{\partial r_{\mathcal{I}}}{\partial {\hat{b}}_a}\end{array}\right]\text{\hspace{1em}(31)}$$

C. 并行化

前一部分中残差和雅可比矩阵的计算只是第一步。为了形成方程 (22)，我们仍然需要将这些局部计算映射到全局残差和雅可比矩阵。这显然是NLS提供的便利之一。然而，这可以通过并行化方案更高效地完成。

首先，从IMU和LiDAR因子的数量以及状态估计的数量中，我们可以推导出分配$\mathbf{r}$和$\mathbf{J}$矩阵的适当维度。然后，我们创建并发线程来计算并分配$\mathbf{r}$和$\mathbf{J}$的子块值，使用适当的索引。

下面的示例用以说明这一过程，参考图3。我们用$K_{\mathcal{I}}$和$K_{\mathcal{L}}$表示IMU和LiDAR因子的数量，$S=k-w+1+N-2$表示控制点的数量（如图1所示），$X=6S+6$为所有状态估计的维度（由$S$个控制点和2个偏差向量组成）。然后，我们可以将$\mathbf{J}$和$\mathbf{r}$中的条目索引为$\mathbf{J}=\left[J_{m,n}\right]$和$\mathbf{r}=\left[r_m\right]$，其中 m ∈ { 0 , … K I + K L − 1 } m \in\left\{0, \ldots K_{\mathcal{I}}+K_{\mathcal{L}}-1\right\} m∈{0,…KI​+KL​−1}， n ∈ { 0 , … X − 1 } n \in\{0, \ldots X-1\} n∈{0,…X−1}。在我们的例子中，IMU因子的残差和雅可比矩阵被堆叠在LiDAR因子之上。

现在，对于一个LiDAR因子，索引 a ∈ { 0 , … , K L − 1 } a \in\left\{0, \ldots, K_{\mathcal{L}}-1\right\} a∈{0,…,KL​−1}，根据(23)和(29)，我们可以发现它的残差是向量$\mathbf{r}$中的单个条目$r_{12.K_{\mathcal{I}}+a}$，而它的雅可比矩阵的非零条目位于 J 12. K I + a , n , n ∈ { 6 i , … 6 i + 6 N − 1 } J_{12 . K_{\mathcal{I}}+a, n}, n \in\{6 i, \ldots 6 i+6 N-1\} J12.KI​+a,n​,n∈{6i,…6i+6N−1}，其中$i$是包含$t$的区间$\left[t_i,t_{i+1}\right)$的节点的索引，$t$是LiDAR测量的采样时间。

同样，对于一个IMU因子，索引 b ∈ { 0 , … , K I − 1 } b \in\left\{0, \ldots, K_{\mathcal{I}}-1\right\} b∈{0,…,KI​−1}，它的残差包括条目 r 12 b + m , m ∈ { 0 , … 12 } r_{12 b+m}, m \in\{0, \ldots 12\} r12b+m​,m∈{0,…12}，而它的非零雅可比矩阵分布在条目$J_{12b+m,n}$上，其中 ( m , n ) ∈ { 0 , … 6 } × { 6 i , … 6 i + 6 N − 1 } ∪ { 0 , 12 } × { 6 S , … 6 S + 6 − 1 } (m, n) \in\{0, \ldots 6\} \times\{6 i, \ldots 6 i+6 N-1\} \cup\{0,12\} \times\{6 S, \ldots 6 S+6-1\} (m,n)∈{0,…6}×{6i,…6i+6N−1}∪{0,12}×{6S,…6S+6−1}。

上述分析对于在几毫秒内计算数千个因子的雅可比矩阵和残差至关重要。该过程的时间效率将在第IV-B节中报告。

*所有值均以米为单位。各方法中的最佳APE用粗体标出，第二好的用下划线标出。‘$x$’表示发散实验。FLIO是FAST-LIO2的缩写，SLICT22 R表示内循环中重新关联的点云数量。

IV. 实验

A. 精度测试

在第一部分中，我们将比较SLICT2与其他最新的LIO方法在流行的公共数据集中的表现。

所选方法包括：FAST-LIO2 [17]、CT-ICP[1]、SLICT[2]和CLIC[18]。FAST-LIO2是一个开创性的基于离散时间的LIO方法，开创了LIO中的迭代扩展卡尔曼滤波器（iEKF）。近年来，它也激励了许多其他工作 [19]、[20]、[21]、[22]。对于连续时间，CT-ICP是首批公开发布的CT-LO方法之一，使用了PLI。同样，SLICT使用了PLI的形式，但在激光雷达扫描的子区间级别上。最后，CLIC是CLINS [4]的改进版本，这是一个使用NLS基础的B样条连续时间公式的LIO方法。

在数据集方面，我们选择NTU VIRAL [23]作为第一个基准，在其中我们使用安装在空中飞行器上的水平16通道激光雷达、高精度高频IMU以及由激光跟踪器捕获的厘米级真实数据。由于NTU VIRAL序列的速度相对较低，我们选择在多校园数据集（MCD）[24]中进行下一个基准测试，其中包含高速度序列。此外，在MCD基准测试中，我们还使用了非重复激光雷达（livox），由于其低成本和高精度而在近年来获得了显著的普及，但也由于视场较窄而带来了挑战。

在所有SLICT2实验中，我们将节点长度设置为0.01秒，样条顺序为4，窗口大小为3，内部迭代次数为3，重新关联点云为2，激光雷达因子的最大数量限制为8000。

图4. FAST-LIO（左）和SLICT2（右）在xxx_day_01序列中的地图和轨迹（红色）。我们可以看到，FAST-LIO在垂直方向上有轻微的漂移，导致其在该序列中的APE较大。

表II
MCD数据集上LO和LIO方法的APE。

*所有值均以米为单位。最佳APE的值用粗体表示，第二最佳的值用下划线表示。'x’表示实验发散。
所有实验均在具有32线程的英特尔i9 CPU的PC上进行。每种方法运行5次，并记录由evo包[25]报告的每个序列的最佳APE（绝对位置误差）指标，记录在表I和表II中。

对于NTU VIRAL基准测试，我们可以看到SLICT2在APE最低的序列数量方面表现最佳，其次是SLICT。FAST-LIO2和CLIC的性能相似，而CT-ICP的性能最低。SLICT和SLICT2之间的接近性能是意料之中的。这是因为SLICT2与SLICT共享类似的前端，且序列速度相对较低，因此SLICT2中B样条轨迹表示的优势尚未明显优于SLICT的PLI方法。然而，我们可以看到，SLICT2并未出现SLICT所出现的发散，这是由于内部关联度量有助于该方法在挑战性环境中更快、更准确地收敛（图5）。我们还注意到，即使仅使用16通道激光雷达，SLICT也无法实时运行。

图5. NTU VIRAL 数据集中 tnp 02 序列的环境，受到阳光眩光和玻璃墙的显著噪声影响。此外，当无人机飞得较高时（无人机的轨迹用红色表示），可约束垂直漂移的特征较少。这导致了SLICT的发散。

对于MCD基准测试，我们得到了更明确的结果，SLICT2在大多数序列中具有最小的APE。第二名是SLICT，接下来是FLIO。图4显示了FAST-LIO和SLICT2在一个序列中的地图和轨迹输出。这展示了密集控制点数量的B样条连续时间公式的好处，它可以更好地捕捉高速序列和无人机运动的动态。相反，CT-ICP作为一种仅使用激光雷达的PLI方法，在高速序列中表现不佳，并且在一些长序列中容易发散。最后，CLIC略好于CTICP，但其性能仍不如FASTLIO。还应注意的是，尽管SLICT的性能相对良好，但即使在稀疏的livox点云下，它也无法实时运行。此外，CT-ICP和CLIC在SLICT2设置的如此高数量的激光雷达因子下也无法实时运行，当我们尝试将控制点密集设置为SLICT2时，CLIC经常崩溃。

B. 处理时间

为了研究SLICT2的效率，我们在程序中仔细添加了多个计时器，以记录在xxx_day_01序列中每个步骤和子步骤的处理时间。

参考图6，我们将$t_{\text{outer\_loop }}$定义为一个外循环的处理时间（图2）。接下来，$t_{\text{pda }}$被定义为内部循环中三个PDA迭代和一个外部循环中的总时间（图2中的步骤2.2和2.1）。同样，$t_{\text{bsu }}$是内部循环中三个BSU过程迭代的总时间。最后，$t_{\text{other }}$指的是其他过程，例如数据复制、特征选择、可视化、制作诊断报告等。

从图中可以看出，$t_{\text{loop }}$的最大值为100毫秒，平均值约为83毫秒，这证实了SLICT2可以实时运行。值得注意的是，$t_{\mathrm{pda}}$的变化相当显著，而$t_{\mathrm{bsu}}$和$t_{\text{other }}$相对稳定。这是由于输入点云的大小变化，直接影响了下采样、去畸变和关联过程的处理时间。在完成PDA过程后，优化问题的大小固定为8000个激光雷达因子，因此$t_{\text{bsu }}$在每次循环中消耗的时间多多少少相同。

图7显示了一次PDA迭代的子过程的处理时间，其中$t_{\text{pda }}$指的是第1次内部迭代的PDA过程时间，$t_{\text{prop }}$、$t_{\text{desk }}$、$t_{\text{assoc }}$分别是传播、去畸变和关联的处理时间。我们可以看到，大多数处理时间都花在将激光雷达点与地图关联上，而IMU传播耗时不到1毫秒，去畸变不到2毫秒。

同样，我们查看了图8中的一次BSU迭代。这里$t_{1\text{ bsu }}$是第一次内部循环中BSU序列的处理时间。步骤3.1的BSU序列的去畸变约为1.2毫秒，步骤3.2的BSU序列的优化大约为3.4毫秒。我们发现，BSU过程在BSU序列内部的优化过程中花费了更多时间。
综上所述，SLICT2在不同数据集上表现良好，具有实时处理能力，能够在大多数时间保持良好的精度。总之，SLICT2不仅提高了全局轨迹的准确性，也通过更高效的处理时间提高了计算效率。这种综合能力使SLICT2在动态环境下成为一种有效的连续时间LIO解决方案。

V. 结论

在本文中，我们开发了一种使用基本而高效优化方法的CT-LIO系统，命名为SLICT2。我们方法的关键创新在于使用一种简单的求解器，消除了传统NLS的许多计算开销，但仍能在仅几次迭代中实现收敛结果。通过在每次迭代之间进行关联，性能得到了进一步提升。我们还对SLICT2管道中每个步骤的处理时间进行了深入分析，以深入了解其效率。与其他最新方法相比，SLICT2的性能具有明显的竞争力，并且可以轻松扩展以解决在线校准、时间偏差和多模态传感器融合等其他问题。我们相信，SLICT2为未来LIO系统的更高效实现提供了有力的依据。

参考文献

[1] P. Dellenbach, J.-E. Deschaud, B. Jacquet, and F. Goulette, “Cticp: Real-time elastic lidar odometry with loop closure,” in 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022, pp. 5580-5586.
[2] T.-M. Nguyen, D. Duberg, P. Jensfelt, S. Yuan, and L. Xie, “Slict: Multi-input multi-scale surfel-based lidar-inertial continuoustime odometry and mapping,” IEEE Robotics and Automation Letters, vol. 8, no. 4, pp. 2102-2109, 2023.
[3] J. Quenzel and S. Behnke, “Real-time multi-adaptive-resolution-surfel 6d lidar odometry using continuous-time trajectory optimization,” in 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2021, pp. 5499-5506.
[4] J. Lv, K. Hu, J. Xu, Y. Liu, X. Ma, and X. Zuo, “Clins: Continuoustime trajectory estimation for lidar-inertial system,” in 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2021, pp. 6657-6663.
[5] B. He, W. Dai, Z. Wan, H. Zhang, and Y. Zhang, “Continuoustime lidar-inertial-vehicle odometry method with lateral acceleration constraint,” in 2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2023, pp. 3997-4003.
[6] X. Zheng and J. Zhu, “Traj-lo: In defense of lidar-only odometry using an effective continuous-time trajectory,” IEEE Robotics and Automation Letters, 2024.
[7] S. Agarwal and K. Mierle, “Ceres solver: Tutorial & reference.” [Online]. Available: http://ceres-solver.org/
[8] F. Dellaert, R. Roberts, V. Agrawal, A. Cunningham, C. Beall, D.-N. Ta, F. Jiang, lucacarlone, nikai, J. L. Blanco-Claraco, S. Williams, ydjian, J. Lambert, A. Melim, Z. Lv, A. Krishnan, J. Dong, G. Chen, K. Chande, balderdash devil, DiffDecisionTrees, S. An, mpaluri, E. P. Mendes, M. Bosse, A. Patel, A. Baid, P. Furgale, matthewbroadwaynavenio, and roderick koehle, “borglab/gtsam,” May 2022. [Online]. Available: https://doi.org/10.5281/zenodo. 5794541
[9] G. Grisetti, R. Kümmerle, H. Strasdat, and K. Konolige, “g2o: A general framework for (hyper) graph optimization,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2011, pp. 9-13.
[10] J. Sola, J. Deray, and D. Atchuthan, “A micro lie theory for state estimation in robotics,” arXiv preprint arXiv:1812.01537, 2018.
[11] T.-M. Nguyen, M. Cao, S. Yuan, Y. Lyu, T. H. Nguyen, and L. Xie, “Viral-fusion: A visual-inertial-ranging-lidar sensor fusion approach,” IEEE Transactions on Robotics, vol. 38, no. 2, pp. 958-977, 2022.
[12] C. Sommer, V. Usenko, D. Schubert, N. Demmel, and D. Cremers, “Efficient derivative computation for cumulative b-splines on lie groups,” in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, 2020, pp. 11 148-11 156.
[13] T.-M. Nguyen, S. Yuan, M. Cao, Y. Lyu, T. H. Nguyen, and L. Xie, “Miliom: Tightly coupled multi-input lidar-inertia odometry and mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 3, pp. 55735580, May 2021.
[14] 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, vol. 6, no. 2, pp. 3317-3324, 2021.
[15] C. Park, P. Moghadam, S. Kim, A. Elfes, C. Fookes, and S. Sridharan, “Elastic lidar fusion: Dense map-centric continuous-time slam,” in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018, pp. 1206-1213.
[16] K. Koide, M. Yokozuka, S. Oishi, and A. Banno, “Voxelized gicp for fast and accurate 3d point cloud registration,” in 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021, pp. $11054-11059$.
[17] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “Fast-lio2: Fast direct lidarinertial odometry,” IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2053 - 2073, 2022.
[18] J. Lv, X. Lang, J. Xu, M. Wang, Y. Liu, and X. Zuo, “Continuoustime fixed-lag smoothing for lidar-inertial-camera slam,” IEEE/ASME Transactions on Mechatronics, 2023.
[19] J. Lin and F. Zhang, “R3live: A robust, real-time, rgb-colored, lidar-inertial-visual tightly-coupled state estimation and mapping package,” in 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022, pp. $10672-10678$.
[20] C. Bai, T. Xiao, Y. Chen, H. Wang, F. Zhang, and X. Gao, “Fasterlio: Lightweight tightly coupled lidar-inertial odometry using parallel sparse incremental voxels,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 4861-4868, 2022.
[21] C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, “Fast-livo: Fast and tightly-coupled sparse-direct lidar-inertial-visual odometry,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022, pp. 4003-4009.
[22] Z. Chen, Y. Xu, S. Yuan, and L. Xie, “ig-lio: An incremental gicpbased tightly-coupled lidar-inertial odometry,” IEEE Robotics and Automation Letters, 2024.
[23] 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,” The International Journal of Robotics Research, vol. 41, no. 3, pp. 270-280, 2022.
[24] T.-M. Nguyen, S. Yuan, T. H. Nguyen, P. Yin, H. Cao, L. Xie, M. Wozniak, P. Jensfelt, M. Thiel, J. Ziegenbein, and N. Blunder, “Mcd: Diverse large-scale multi-campus dataset for robot perception,” in Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition, Jun. 2024. [Online]. Available: https://mcdviral.github.io/
[25] M. Grupp, “evo: Python package for the evaluation of odometry and slam.” https://github.com/MichaelGrupp/evo, 2017.