# SC-FAST-LIVO2：一种用于长期SLAM、可减小漂移的跨模态回环闭合框架 周远 $^{*}$ 邓迪国际学院 中南大学 中国长沙 蒋先坤 $^{*,\dagger}$ 长空学院 国际学院 王宏泽 中国北京 北京邮电大学 南京航空航天大学 中国南京 $^{†}$通讯作者 赵子涵 邓迪国际学院 中南大学 王轩博 中国长沙 机械工程学院 大连理工大学 中国大连 摘要—本文提出了一种用于激光雷达-视觉SLAM系统的混合回环闭合检测框架，将鲁棒的Scan Context描述符与位姿图优化模块集成到名为SC-FAST-LIVO2的框架中 $^{**}$。该模块负责关键帧提取和位姿图构建，还能通过归一化相似度评分实现基于阈值的回环闭合检测，以匹配候选帧。该模块还可协助完成最终全局地图的建立与优化，同时消除漂移以保证全局一致性。位姿图优化模块采用SE(3)约束进行集成，它会订阅点云消息和位姿信息，并发布带有自适应协方差缩放的反馈结果。大量实验表明，所提方法能够降低绝对平移误差，在大规模环境中可有效校正漂移。这种模块化系统设计使得各个组件可以独立升级，同时仍能保持整体SLAM的一致性。 关键词—SLAM、位姿图优化、回环闭合检测、Scan Context ## I. 引言 同步定位与建图（SLAM）[^1]技术是推动自主机器人[^2]、自动驾驶[^3]以及无人机（UAV）[^4]可靠地在周边环境中导航和交互的基础支柱，即便在无法使用GPS的环境中也能正常工作，比如地下隧道[^5]、密集的城市峡谷[^6]或室内设施[^7]，以及完全未探索的环境。在这些外部定位基础设施不可用或不可靠的场景下，SLAM系统必须自主构建环境地图，同时实时精确追踪机器人的位姿。过去十年间，传感器技术的进步和算法创新推动SLAM性能达到了新高度，比如高分辨率激光雷达[^8]和经济型惯性测量单元[^9]，使其成为了现代自主系统的核心组成部分。 虽然最先进的 LiDAR-Inertial Odometry (LIO) [^10] 系统在短期内已实现了令人瞩目的位姿估计精度，但长期导航仍受到持续误差累积的困扰。仅为行进距离 1–2% 的漂移会随时间累积，导致长时间运行时出现显著的位置偏差。例如，在公开数据集 NTU VIRAL 的某些序列中，LIO-SAM [^11] 在最大 [^12] 时产生了 1257.159 m 的漂移，这会使地图不一致、干扰任务规划，甚至危及自主操作的安全性。 这一局限性显著制约了 SLAM 在长时间任务中的适用性。 近期的 LIO 工作可分为两个互补的方向。在紧耦合方面，LIO-SAM [^11] 将 LiDAR 与 IMU 进行融合，并通过稳健的 loop closure 来抑制漂移、保持长期导航的全局一致性，同时应用 Pose Graph Optimization (PGO) 来加速 loop closure 与位姿图的构建，并满足实时性约束。另一方面，FAST-LIVO2 [^10] 已成为实时性能与计算效率方面的标杆。通过采用精心优化的稀疏直接数据处理流水线，以及顺序更新的 error-state iterated Kalman filter (ESIKF) [^10]，FAST-LIVO2 在障碍物或运动物体普遍存在的动态环境，以及小型机器人或计算能力受限的无人机等资源受限平台上均表现出色。然而，这种对速度与效率的追求是有代价的：FAST-LIVO2 --- 缺乏专门的回环检测机制，从而导致长期一致性较差。在大规模应用场景中，当机器人的运动轨迹长达数百米甚至更远时，缺少漂移校正机制便会导致明显的地图扭曲和位姿偏差。 本研究旨在保留FAST-LIVO2的低延迟里程计特性，同时弥补其缺乏专门回环检测机制这一缺陷——该缺陷会在大尺度轨迹上降低长期一致性。因此，我们设计并将其中的稳健回环检测模块与位姿图优化后端集成到FAST-LIVO2系统中，以此补充原有的前端模块，使其转变为能够大规模校正漂移的完整SLAM系统。所提出的SC-FAST-LIVO2系统是一种混合框架，它将基于Scan Context描述符的轻量级且稳健的回环检测机制与位姿图优化技术无缝集成到FAST-LIVO2中。本文的主要贡献包括： 1) 我们提出了一种统一的回环检测与位姿图优化方案，用以增强FAST-LIVO2的前端模块，从而提升大规模环境下的长期一致性。 2) 我们构建了具有自适应协方差和鲁棒损失的SE(3)位姿图优化模型，该模型通过增量平滑与建图算法2（iSAM2）进行迭代求解[^13]，并将修正结果传播到共享体素图中。 3) 我们提出了一种基于跨模态Scan Context描述符的稳健回环检测方案，该方案能够降低绝对平移误差并提升轨迹精度。 本研究推动了自主导航领域的发展，证明了精心的系统集成能够在实时性能与地图精度之间取得平衡。此外，我们架构的模块化设计确保了与现有FAST-LIVO2应用程序的向后兼容性，从而能够轻松应用于现有的机器人系统，同时大幅扩展其应用范围，以支持从工业检测到城市探索等长时间、大规模的任务。 本文其余部分的结构如下：第二节回顾了关于激光雷达-惯性-视觉SLAM及回环检测的相关工作；第三节介绍了所提出的SC-FAST-LIVO2框架的方法论，首先阐述Scan Context描述符的初步知识以及用于位置识别的相似性计算方法，随后详细说明整体系统架构、回环检测模块、位姿图优化及其与FAST-LIVO2前端的交互方式；第四节报告实验设置、数据集、基准测试协议以及定量和定性结果；第五节对全文进行总结并探讨未来的研究方向。 ### 二、相关工作 近年来，由于激光雷达SLAM对光照变化的鲁棒性以及在复杂环境中的精确几何建模能力，其发展十分迅速。这一进步主要得益于三大技术趋势：点密度更高且功耗更低的固态激光雷达的研发、高效的点云处理算法的出现，以及嵌入式平台计算能力的提升。 ### A. 激光雷达里程计方法 开创性的LOAM框架[^8]提出了一种将高频里程计与低频建图分离的两层方法，通过基于特征的数据配准提升了实时性能与精度。然而，LOAM由于缺乏显式的回环检测机制，在长时间任务中会出现累积漂移问题。对于地面机器人而言，LeGO-LOAM[^14]通过地面分割和稳健的优化方案进一步提升了性能，但这两个框架仍属于松散耦合，无法在大型且重复的环境中保持全局一致性。 紧密耦合的直接配准方法代表了重要的进步。FAST-LIO[^15]采用针对原始激光雷达点的迭代卡尔曼滤波器，无需进行特征提取；后续的FAST-LIO2[^16]引入了增量式KD树地图维护机制，从而实现轻量、高频且精确的局部里程计。尽管有这些进展，这些系统主要仍作为里程计前端使用，在缺乏回环检测的情况下，长期部署时仍会出现累积漂移问题。 ### B. 多传感器融合 为了进一步提升鲁棒性，尤其是在传感器环境恶劣的场景下，多源SLAM[^17] [^18] [^19]框架如VINS-Mono[^9]、FI-SLAM[^20]和FAST-LIVO[^21]等，融合了视觉、激光雷达和惯性数据。值得注意的是，这些框架中的紧密耦合视觉-惯性子系统能够在激光雷达短暂失效期间维持跟踪功能。FAST-LIVO2[^10]便是典型代表，它通过轻量级前端和高效的ESIKF来联合优化这些模态。不过，这些系统仍然缺乏全局回环检测机制，在长轨迹下仍容易出现累积误差。 ### C. 迈向全局一致性的方法 为了解决长期漂移问题，图SLAM框架已成为主流。LIO-SAM [^11] 通过融合激光雷达里程计、IMU预积分、GPS信息以及基于图因子的回环闭合技术，实现了全局图优化。不过，在大规模场景中，实时性能可能会受到影响，因为维护和优化全局地图会带来显著的计算开销。 在回环闭合机制中，Scan Context描述符 [^22] 能生成旋转不变的描述符，从而实现高效的激光雷达位置识别；而在视觉领域，词袋（BoW）方法 [^23] 则借助分层词汇树对二进制描述符进行处理，实现了可扩展的视觉回环闭合。这两种方法都利用了紧凑且具区分度的特征以及高效的搜索机制，极大地有助于漂移校正和全局一致性维护，并对许多近期的SLAM系统产生了重要影响。 总体而言，尽管诸如FAST-LIVO2这类前沿的激光雷达-惯性-视觉里程计（LIVO）系统在实时性能和动态或资源受限环境中的鲁棒性方面表现出色， --- 但由于缺乏全局回环闭合机制，它们在长时间运行过程中仍会不可避免地出现累积漂移误差。这一限制在大规模应用中尤为突出：即便微小的漂移也会不断累积，最终导致严重的位姿误差和不一致的地图。 ### III. 方法论 本节我们将介绍所提出的SC-FAST-LIVO2框架。该节详细阐述了所提出的SC-FAST-LIVO2框架，重点说明了如何将基于描述符的回环闭合模块集成到FAST-LIVO2中。 ### A. 预备知识 Scan Context是一种轻量级的、非直方图的全局描述符，在基于激光雷达的回环闭合中已被证明十分有效，它已被SC-A-LOAM [^24]、SC-LIO-SAM [^25]、SC-LeGO-LOAM [^26] 以及FAST-LIO-SLAM [^27] 所采用。在本研究中，我们将Scan Context [^22] 集成到FAST-LIVO2中，从而形成了SC-FAST-LIVO2，该模块被嵌入到将在下一部分介绍的SC-PGO模块中。Scan Context的基本原理总结如下： 1) 点云划分：设点云通过 $N_{r}$ 个同心圆环和 $N_{s}$ 个方位扇区进行划分： $$ \mathcal{P}=\bigcup_{\substack{i\in[N_{r}]\\ j\in[N_{s}]}}\mathcal{P}_{i j}, $$ 其中 $\mathcal{P}_{ij}$ 表示第i个圆环和第j个扇区中的子云。 $$ \phi\colon\mathcal{P}_{i j}\to\mathbb{R},\;\phi\left(\mathcal{P}_{i j}\right)=\operatorname*{m a x}_{\mathbf{p}\in\mathcal{P}_{i j}}z(\mathbf{p}). $$ 2) 分箱编码：每个分箱通过以下方式映射为实数值： 其中 $z(\cdot)$ 为z坐标。 3) Scan Context矩阵：将各分箱的值堆叠起来便得到SC矩阵： $$ I=(a_{i j})\in\mathbb{R}^{N_{r}\times N_{s}},a_{i j}=\phi\big(\mathcal{P}_{i j}\big). $$ 可采用根移位增强来提高鲁棒性。 4) 相似性：给定查询向量 $I^{q}$ 和候选向量 $I^{c}$，定义列向量 $c_{i}^{q}$ 和 $c_{i}^{c}$。余弦距离为： $$ d(I^{q},I^{c})=\frac{1}{N_{s}}\sum_{j=1}^{N_{s}}\left(1-\frac{c_{j}^{q}\cdot c_{j}^{c}}{\|c_{j}^{q}\|\|c_{j}^{c}\|}\right). $$ 为了处理偏航角问题，需选择 $I^{c}$ 的最佳环形列移位： $$ D\big(I^{q},I^{c}\big)=\min_{n\in[N_{s}]}d\big(I^{q},I^{c}_{n}\big),n^{*}=\underset{n\in[N_{s}]}{\mathrm{argmin}}d\big(I^{q},I^{c}_{n}\big). $$ 5) 两阶段搜索：根据行的稀疏性计算环形键： $$ \begin{align*}k=\big(\psi(r_{1}),\ldots,\psi(r_{N_{r}})\big),\quad&\mathrm{where}\ \psi\colon r_{i}\to\mathbb{R},\\\psi(r_{i})&=\frac{\|r_{i}\|_{0}}{N_{s}}.\end{align*} $$ 其中 $r_i$ 为I的第i行。首先在KD树中检索与查询键最接近的K个邻居，然后通过 $D(I_q, I_c)$ 验证每个候选结果。若满足条件则接受该回环。 $$ c^{*}=\underset{c_{k}\in\mathcal{C}}{\mathrm{a r g m i n}}D\big(I^{q},I^{c_{k}}\big),\quad\mathrm{s.t.}D<\tau. $$ 最佳偏移量 $n^{*}$ 可给出粗略的偏航角估计。更详细的信息可从Scan Context [^22] 中获取。 ### B. 系统概述 SC-FAST-LIVO2的整体架构如图1所示，包含6个组件：传感器数据输入、前向传播、反向传播、状态估计、地图构建和输出层。 FAST-LIVO2的主要部分包括传感器输入、前向传播、状态估计、地图构建和输出层。相关细节已在FAST-LIVO2 [^10] 中讨论过。下文将详细描述反向传播过程。 ![Image](images/d3d631b0a3a6bc5871963d6e0b3fd837f916b4a9fcf3d801b33705a46685822a.jpg)
Fig. 1: Basic Architecture of SC-FAST-LIVO2
### C. Scan Context位姿图优化（SC-PGO） Scan Context位姿图优化模块是确保全局定位一致性的核心组件，SC-PGO模块的完整结构以伪代码形式展示在算法1中。它能够修正激光雷达-惯性-视觉里程计在长时间运行过程中产生的累积漂移误差。该过程包含五个关键步骤：关键帧选择、回环检测与验证、位姿图构建、增量式位姿图优化，以及地图更新与漂移优化。通过将局部位姿整合到一个全局一致的框架中， --- ```pseudo // Algorithm 1: SC-PGO Module for Global Consistency Require: Keyframe pose $\mathbf{T}_k$; point cloud $\mathcal{P}_k$; translation threshold $\tau_i$; rotation threshold $\tau_r$; similarity threshold $\tau_s$; fitness threshold $\tau_f$ Ensure: Optimized global poses $\{\mathbf{T}_k^*\}$; updated global map $\mathcal{M}$ 1: Keyframe Selection: 2: if $d_{\text{trans}} > \tau_i$ OR $d_{\text{rot}} > \tau_r$ then 3: Create new keyframe with $\mathbf{T}_k$, $\mathcal{P}_k$ 4: Add node $\mathbf{T}_k$ to pose graph 5: Loop Closure Detection (Coarse): 6: Extract Scan Context descriptor $\mathbf{SC}_k$ from $\mathcal{P}_k$ 7: Compute similarity $s(\mathbf{SC}_k, \mathbf{SC}_j)$ for all past keyframes 8: if $\exists j$ such that $s(\mathbf{SC}_k, \mathbf{SC}_j) > \tau_s$ then 9: Add candidate $(k, j)$ to loop closure list $\mathcal{L}$ 10: Loop Closure Verification (Fine): 11: for all $(k, j) \in \mathcal{L}$ do 12: Align $\mathcal{P}_k$ and $\mathcal{P}_j$ using ICP: 13: $\min_{\mathbf{R}, t} \sum \left\| \mathbf{R} \cdot \mathbf{p}_i^{\text{world}} + \mathbf{t} - \mathbf{q}_i^{\text{body}} \right\|^2$ 14: Compute fitness score: fitness = $\frac{1}{M} \sum \|\mathbf{R} \cdot \mathbf{p}_i + \mathbf{t} - \mathbf{q}_i\|$ 15: if fitness $< \tau_f$ then 16: Add loop closure factor $\mathbf{f}_{\text{loop}}(\mathbf{T}_k, \mathbf{T}_j)$ to graph 17: Pose Graph Optimization: 18: Construct graph with factors: prior, odometry, loop closure 19: Solve: $\{\mathbf{T}_k^*\} = \arg\min_{\{\mathbf{T}_k\}} \sum \|\mathbf{f}(\mathbf{T}_i, \mathbf{T}_j)\|^2_\Sigma$ using iSAM2 20: Propagate corrections: $\Delta\mathbf{T}_k = \mathbf{T}_k^* \cdot \mathbf{T}_k^{-1}$ 21: Map Update: 22: for all keyframes do 23: Transform point cloud: $\mathbf{p}_i^{\text{world}} = \mathbf{T}_k^* \cdot \mathbf{q}_i^{\text{body}}$ 24: Fuse into global voxel map $\mathcal{M}$ with downsampling 25: return $\{\mathbf{T}_k^*\}$, $\mathcal{M}$ ``` 该模块能够提升复杂环境下的建图精度与鲁棒性，因此在机器人技术、自主导航和大规模建图应用中发挥着至关重要的作用。 $$ \mathbf{T}_{k}=\left[\begin{matrix}{\mathbf{R}_{k}}&{\mathbf{t}_{k}}\\ {\mathbf{0}^{T}}&{1}\\ \end{matrix}\right], $$ 1) 关键帧选择：为了在计算速度和精度之间取得平衡，系统采用了一种智能的关键帧选择方法。该方法实时跟踪机器人的运动，在满足特定几何条件时将当前位姿标记为关键帧。系统使用因子图来表示关键帧之间的约束关系。每个关键帧的位姿都是SE(3)中的一个节点，其变换矩阵为： 其中 $\mathbf{R}_k \in \mathrm{SO}(3)$ 表示当前的旋转矩阵，$\mathbf{t}_k \in \mathbb{R}^3$ 表示当前的平移向量。 评估运动程度时采用两项几何准则：对于平移，欧几里得距离表示为： $$ \begin{array}{r}{d_{\mathrm{t r a n s}}=\|\mathbf{t}_{k}-\mathbf{t}_{\mathrm{l a s t}}\|_{2},}\end{array} $$ 其中，当前平移向量 $\mathbf{t}_{k}$ 与上一个关键帧的平移向量 $\mathbf{t}_{\text{last}}$ 之间的欧几里得距离被计算出来。 对于旋转，旋转矩阵 $\mathbf{R}_k$ 与 $\mathbf{R}_{\text{last}}$ 之间的角度差通过以下方式测量： $$ d_{\mathrm{r o t}}=\operatorname{a r c c o s}\left(\frac{\mathrm{t r}(\mathbf{R}_{k}\cdot\mathbf{R}_{\mathrm{l a s t}}^{T})-1}{2}\right)\times\frac{180^{\circ}}{\pi}. $$ 当运动或旋转超过阈值时，便会创建新的关键帧，以捕捉主要的位姿变化。每个关键帧包含完整的位姿数据、点云和特征描述符，从而为位姿优化提供强有力的约束。这种方法在处理成本和轨迹精度之间取得了平衡。 2) 回环检测与验证：回环检测用于识别机器人重新访问先前区域的情况，为位姿图优化提供关键的全局约束。系统采用从粗到精的两阶段架构来确保效率与可靠性。 在粗匹配阶段，具有旋转不变性的Scan Context描述符通过将点云中的三维点投影到极坐标网格上并分析每个单元格的高度分布，从而从激光雷达点云中提取全局特征，形成二维环境特征矩阵。关键帧之间的场景相似度通过以下方式计算： $$ s(\mathbf{S}\mathbf{C}_{k},\mathbf{S}\mathbf{C}_{j})=1-\frac{\sum_{i,j}|\mathbf{S}\mathbf{C}_{k}(i,j)-\mathbf{S}\mathbf{C}_{j}(i,j)|}{N_{r}\cdot N_{\theta}\cdot h_{\operatorname*{m a x}}}. $$ 其中，$\mathbf{SC}_k$ 和 $\mathbf{SC}_j$ 是关键帧的特征矩阵，$N_r$ 和 $N_\theta$ 分别是径向和角度网格尺寸，而 $h_{max}$ 则是激光雷达的最大探测范围。若相似度得分高于阈值，则表明存在闭环，此时会触发精细验证。 在精细验证阶段，系统采用迭代最近点（ICP）算法 [^28] 来对候选帧进行精确配准。ICP通过迭代建立点对对应关系并优化位姿估计，从而有效捕捉几何细节，最终收敛到最优变换。其优化目标为： $$ \min_{\mathbf{R},\mathbf{t}}\sum_{i=1}^{M}\left\|\mathbf{R}\cdot\mathbf{p}_{i}^{\mathrm{world}}+\mathbf{t}-\mathbf{q}_{i}^{\mathrm{body}}\right\|_{2}^{2}, $$ 其中，$\mathbf{p}_{i}^{\text{world}}$ 表示当前帧中的一个点，而 $\mathbf{q}_{i}^{\text{body}}$ 则表示候选传感器帧中对应的点。 $\mathbf{R}$ 和 $\mathbf{t}$ 分别是待确定的旋转矩阵和平移向量，M 表示参与配准计算的点对数量。配准质量通过平均拟合误差来评估： $$ fitness=\frac{1}{M}\sum_{i=1}^{M}\left\|\mathbf{R}\cdot\mathbf{p}_{i}^{world}+\mathbf{t}-\mathbf{q}_{i}^{body}\right\|_{2}. $$ 只有当误差低于阈值时，闭环才会被接受，同时相应的相对位姿会被加入位姿图中。系统中还加入了时间一致性检查，以排除不稳定的匹配结果，从而进一步提升鲁棒性。 --- 3) 位姿图构建：约束框架由三种因子类型构成，具体如下： 先验因子用于锚定初始关键帧： $$ \mathbf{f}_{\mathrm{p r i o r}}(\mathbf{T}_{0})=\log\left(\mathbf{T}_{\mathrm{p r i o r}}^{-1}\cdot\mathbf{T}_{0}\right). $$ 其中，$\log: \text{SE}(3) \mapsto \text{se}(3)$ 表示从李群到李代数的映射，而 $\mathbf{T}_{\text{prior}} = \mathbf{I}$、$\mathbf{T}_0$ 则是初始帧的位姿。 里程计因子利用相对位姿 $\hat{T}_{k,k+1}$ 来关联连续的关键帧： $$ \mathbf{f}_{\mathrm{o d o m}}(\mathbf{T}_{k},\mathbf{T}_{k+1})=\log\left(\hat{\mathbf{T}}_{k,k+1}^{-1}\cdot\left(\mathbf{T}_{k}^{-1}\cdot\mathbf{T}_{k+1}\right)\right). $$ 其中，$\hat{\mathbf{T}}_{k,k+1} \in \mathrm{SE}(3)$ 是根据里程计测量得到的从帧 k 到帧 $k+1$ 的相对位姿观测值。 $$ \mathbf{f}_{\mathrm{l o o p}}(\mathbf{T}_{k},\mathbf{T}_{j})=\log\left(\hat{\mathbf{T}}_{k,j}^{-1}\cdot\left(\mathbf{T}_{k}^{-1}\cdot\mathbf{T}_{j}\right)\right). $$ 当检测到闭环时，闭环因子会施加强约束： 其中，$\hat{\mathbf{T}}_{k,j}$ 是通过点云配准计算得出的相对位姿观测值。 4) 增量式位姿图优化：位姿图优化的目标是找到能使全局约束误差最小化的最优位姿集合，这相当于最大后验估计： $$ \{\mathbf{T}_{k}^{*}\}=\operatorname{a r g}\operatorname*{m i n}_{\{\mathbf{T}_{k}\}}\sum_{\mathrm{f a c t o r s}}\left\|\mathbf{f}(\mathbf{T}_{i},\mathbf{T}_{j})\right\|_{\mathbf{\Sigma}}^{2}. $$ 其中，$\{\mathbf{T}_k^*\}$ 是所有最优关键帧位姿的集合，求和符号 $\Sigma$ 遍历图中所有的约束因子（先验、里程计和闭环），$\|\cdot\|_{\Sigma}^2$ 是马氏距离，而 $\Sigma$ 则是与每个约束相关的协方差矩阵。注意，k 索引所有关键帧，而每个因子 $\mathbf{f}$ 涉及一对特定的关键帧 $(i, j)$。 系统采用 iSAM2 [^13] 算法，该算法通过维护贝叶斯树，仅在新数据到来时对受影响的子图进行优化。它通过边缘化操作保持稀疏性，从而实现实时运算。整个过程以融合后的里程计数据初始化，最终输出校正后的全局位姿集合。 5) 地图更新与漂移补偿：在位姿图优化之后，系统会利用优化后的位姿来更新全局地图并补偿漂移。对于每个关键帧，位姿校正计算如下： $$ \Delta\mathbf{T}_{k}=\mathbf{T}_{k}^{*}\cdot\mathbf{T}_{k}^{-1} $$ 此处，$\Delta\mathbf{T}_k$ 表示在优化前将位姿 $\mathbf{T}_k$ 转换为优化后的位姿 $\mathbf{T}_k^*$ 所需的修正量。随后，原始点云会通过以下方式被转换到全局坐标系： $$ \mathbf{p}_{i}^{\mathrm{w o r l d}}=\mathbf{T}_{k}^{*}\cdot\mathbf{q}_{i}^{\mathrm{b o d y}}. $$ 该过程通过优化后的位姿，将传感器坐标系中的点云转换到全局坐标系。 最后，通过体素网格滤波对点云进行下采样并融合修正后的点云，从而构建出全局一致的稠密地图。这一步骤消除了累积漂移现象，确保了地图的全局一致性。 D. SC-PGO模块与其他组件之间的交互 SC-PGO模块与其他系统组件的交互对于提升定位精度、减少里程计漂移至关重要。 1) 订阅来自FAST-LIVO2的前向信息：PGO模块接收外部数据以构建约束条件。通过订阅与激光雷达里程计相关的主题，PGO模块能够获取LIO以及视觉惯性里程计（VIO）在前向传播中计算出的实时位姿，包括三维空间中的位置以及横滚角、俯仰角和偏航角。这些订阅数据用于提取关键帧并构建初始的位姿图。 PGO模块也会订阅完整的点云消息。原始点云信息会先经过预处理，随后被送入前向传播部分的点云配准模块以提取更多特征。完成该流程后，点云会被送入激光雷达里程计模块进行后续处理，最终由PGO模块将其作为关键帧存储和ICP配准的约束条件。 2) 将优化后的信息发布给FAST-LIVO2：一旦从PGO模块接收到新的关键帧，Scan Context算法便会生成环形键和扇形键。系统会构建KD树，根据相似度评分筛选出候选关键帧。检测到的闭环会被加入位姿图中。Scan Context与PGO的结合通过iSAM2算法增强了全局位姿优化效果，之后可视化消息会带着同步的关键帧索引被发布给FAST-LIVO2。后端的PGO模块会重点处理前端FAST-LIVO2产生的累积里程计误差，具体包括关键帧提取、位姿图构建、闭环检测以及全局优化。 ### IV. 实验结果 本节将介绍实验设置与结果，包括与基准方法的对比，以及在绝对平移误差和地图质量方面的评估。 ### A. 实现方式与系统配置 所提出的SC-FAST-LIVO2系统在ROS环境下实现。系统采用了FAST-LIVO2的默认配置，启用了曝光时间估计和参考块更新功能，但禁用了法向量细化功能。每次扫描中的激光雷达点会以1:3的比例进行下采样。体素地图的根体素大小设为0.5米，内部八叉树的最大层数为3。图像对齐所用的图像块大小为$8 \times 8$，法向量细化所用的大小为$11 \times 11$。在顺序ESIKF设置中，相机光度噪声被设定为常量100。PGO模块的更新频率设为20。在所有具有相同传感器配置的序列和数据集中，我们的系统均使用相同的参数。实验平台配备了Intel i9-13900H CPU和32 GB内存。 --- ![Image](images/74e6ac787c6e255105f305a81b632bddefeb30a02c6453bed995e6c9429809b7.jpg)
Fig. 2: Loop closure comparison results of colored point cloud and path constructed by the original FAST-LIVO2 system and SC-FAST-LIVO2. The point clouds from top to bottom correspond to "HKU Lecture Center" and "HKU Cultural Center".
### B. 数据集 1) Hilti数据集：Hilti'22数据集[^29]和Hilti'23数据集[^30]包含了在室内外不同场景中采集的数据序列，这些场景涵盖稀疏和常规建筑工地、走廊、房间以及牛津的谢尔登尼亚剧院画廊。这些序列呈现出多种具有挑战性的环境特征，比如无纹理表面、多变的照明条件以及不足的激光雷达平面约束。对于每个序列，都提供了通过动作捕捉系统（MoCap）或全站仪[^31]获取的毫米级精度的真实值。需要注意的是，Hilti数据集的真实值并未公开提供。因此，在这些数据集上的算法结果需通过官方的Hilti评估平台进行评定。 2) FAST-LIVO2私有数据集：为了评估PGO模块的性能，我们使用FAST-LIVO2构建的私有数据集开展了额外实验。该私有数据集包含20个在不同环境中采集的数据序列，这些环境包括校园建筑、走廊、地下室和采矿隧道，其特点为结构稀疏、杂物繁多、照明不佳、光线变化显著且纹理薄弱。每个序列均设计为返回起点轨迹，以便准确评估漂移情况。 ### C. 基准实验 本次实验中，我们对Hilti'22和Hilti'23数据集中的序列进行了评估。我们的方法分别与几种最先进的里程计系统进行了对比，包括：SDV-LOAM [^32]，一种半直接式激光雷达-视觉里程计；FAST-LIO2 [^16]，一种直接式激光雷达-惯性里程计；FL2BIPS [^33]，一种采用贝叶斯ICP位姿优化的激光雷达-惯性里程计，它将FAST-LIO2滤波与批量位姿图优化相结合；FL2BIPS-GTSFM [^33]，一种多模态SLAM系统，它在FL2BIPS基础上扩展了视觉束调整功能，采用了由佐治亚理工学院运动结构算法（GTSAM）驱动的分布式运动结构流水线；Leapmotor，一种基于FAST-LIO、采用实时滑动窗口优化的激光雷达-惯性里程计；FrankenPharos，一种激光雷达-惯性SLAM系统，它能够同时优化位姿和IMU偏差，还集成了在线去畸变和基于X-ICP的退化感知功能；FAST-LIVO [^21]，一种快速且紧密耦合的稀疏直接式激光雷达-惯性-视觉里程计；以及FAST-LIVO2 [^10]，FAST-LIVO的改进版本，采用了顺序更新的ESIKF。 对于FAST-LIO2、FAST-LIVO和FAST-LIVO2，我们采用了针对室内和室外环境推荐的参数设置。对于SDV-LOAM，我们使用了将视觉模块与LeGO-LOAM结合的松散耦合集成框架的开源实现。相应的评估结果直接取自FAST-LIVO2论文[^10]。此外，我们利用官方Hilti'22和Hilti'23排行榜上公开的FL2BIPS、FL2BIPS-GTSFM、Leapmotor和FrankenPharos的结果进行客观对比。 这些实验的结果如表I和表II所示，结果表明我们的系统在大多数序列上的表现均优于其他所有方法，平均RMSE仅为0.035米，比基线方法FAST-LIVO2低6.58%。 ---
TABLE I: Absolute Translational Errors (RMSE, meters) in Hilt $^{1}$'22 Sequences
DatasetSequenceSDV-LOAM $^{{*}}$FAST-LIO2 $^{{*}}$FL2BIPS $^{{**}}$FL2BIPS-GTSFM $^{{**}}$FAST-LIVO $^{{*}}$FAST-LIVO2 $^{{*}}$Ours
Hilti'22Construction Ground25.1210.0130.0150.0510.0220.0100.009
Construction Multilevel12.5610.0440.0370.1220.0520.0200.019
Construction Stairs9.2120.3201.3922.6740.2410.0160.014
Long Corridor19.5310.0640.1660.3870.0650.0670.058
Cupola9.2310.2500.9154.1800.1820.1800.161
Lower Gallery11.2320.0240.2310.8000.0220.0070.006
Attic to Upper Gallery4.5510.7200.3000.1210.6210.0690.073
Outside Building2.6220.0280.0320.1180.0520.0350.034
Average11.7580.1830.3861.0740.1570.5050.468
$^{*}$ 这些模型和数据由FAST-LIVO2提供[^10] $^{**}$ 这些模型和数据由Hilti平台提供[^29]
TABLE II: Absolute Translational Errors (RMSE, meters) in Hilt $^{23}$ Sequences
DatasetSequenceSDV-LOAM $^{{*}}$FAST-LIO2 $^{{*}}$Leapmotor $^{{**}}$FrankenPharos $^{{**}}$FAST-LIVO $^{{*}}$FAST-LIVO2 $^{{*}}$Ours
Hilti $^{{*23}}$Floor 04.6210.0310.0310.0250.0210.0220.019
Floor 17.9510.0310.0410.0230.0220.0230.021
Floor 27.9120.0830.0310.0370.0480.0220.021
Underground6.1510.0380.0250.0170.0350.0160.015
Stairs9.0320.1700.0880.0110.1520.0180.015
Parking 3x floors down19.9520.3200.5530.4000.3560.0320.026
Floor 1 Large room16.7810.0280.0660.0480.0310.0260.028
Floor 2 Large room - dark15.0120.0400.0650.0440.0530.0460.047
Average10.9270.0930.1130.0760.0900.0260.024
$^{*}$ 这些模型和数据由FAST-LIVO2提供 [^10] $^{**}$ 这些模型和数据由Hilti平台提供 [^30] 我们的系统在大多数序列中均取得了最佳性能。值得注意的是，在“Outside Building”序列中，与基线FAST-LIVO2框架相比，我们的系统在定位精度方面并未展现出显著提升。这一现象可归因于该序列的真实轨迹中缺乏闭环信息。因此，我们的系统无法通过闭环检测来提升轨迹精度。排除该序列后，我们的方法在定位性能上仍优于基线FAST-LIVO2。这种提升得益于SC-FAST-LIVO2所采用的精准闭环检测与位姿图优化技术，它们能有效减少长期建图过程中的累积漂移误差，从而实现更精确的轨迹重建。 ### D. 高精度建图 本实验利用FAST-LIVO2私有数据集，评估了我们所提出系统的高精度定位与建图能力。如图2所示，SC-FAST-LIVO2在室内和室外环境中均表现出优异的定位与建图性能。在相机与场景间视角差异较大的区域，我们的方法通过位姿校准和局部地图重建技术显著提升了建图质量。在轨迹生成方面，我们的方法有效消除了基线FAST-LIVO2中出现的闭环伪影，并在轨迹精度和位姿优化方面取得了显著进步。 ### V. 结论与未来工作 本文提出了SC-FAST-LIVO2，这是一个后端集成了混合闭环检测与优化功能的框架，它在FAST-LIVO2前端的基础上，加入了基于Scan Context的地点识别技术和基于iSAM2的位姿图优化功能。 在Hilt $^{1}$22、Hilt $^{1}$23以及私有数据集上的实验表明，该方法在复杂场景下能稳定降低RMSE值，且地图一致性明显提升，性能优于基线模型。不过仍存在一定局限性：在回环较少或没有回环的轨迹中，闭环带来的优势自然受限，因为无法建立可靠的地点识别约束；此外，在视角变化极端的情况下，前端鲁棒性可能会下降。 在未来的工作中，我们将探索更多闭环检测方法以进一步提升性能，包括融合激光雷达与视觉信息的方法。我们还会优化Scan Context描述符和位姿图优化模块，以更好地利用多模态融合技术，进而提高系统的鲁棒性和全局一致性。 ## 致谢 所有作者感谢王森教授就此课题给予的深刻讨论，这些讨论为闭环检测和位姿图优化方法以及我们的后续研究提供了重要启发。 --- ## 参考文献 [^1]: C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. Leonard, “Past, present, and future of simultaneous localization and mapping: Towards the robust-perception age,” IEEE Transactions on Robotics, vol. 32, no. 6, p. 1309–1332, 2016. [^2]: H. Carrillo, P. Dames, V. Kumar, and J. A. Castellanos, “Autonomous robotic exploration using occupancy grid maps and graph slam based on shannon and rényi entropy,” in 2015 IEEE international conference on robotics and automation (ICRA). IEEE, 2015, pp. 487–494. [^3]: A. Singandhupe and H. M. La, “A review of slam techniques and security in autonomous driving,” in 2019 third IEEE international conference on robotic computing (IRC). IEEE, 2019, pp. 602–607. [^4]: G. Sonugür, “A review of quadrotor uav: Control and slam methodologies ranging from conventional to innovative approaches,” Robotics and Autonomous Systems, vol. 161, p. 104342, 2023. [^5]: F. D. Lauro, D. G. Sorrenti, and M. A. Sotelo, “Multi-robot LiDAR SLAM: A practical case study in underground tunnel environments,” arXiv preprint arXiv:2507.21553, 2025. [^6]: Y. Zhang, F. Dong, Q. Sun, and W. Song, "Laser SLAM matching localization method for subway tunnel point clouds," Sensors, vol. 25, no. 12, p. 3681, 2025. [^7]: R. Murai, E. Dexheimer, and A. J. Davison, “Mast3r-slam: Real-time dense slam with 3d reconstruction priors,” in Proceedings of the Computer Vision and Pattern Recognition Conference, 2025, pp. 16695–16705. [^8]: J. Zhang, S. Singh et al., “Loam: Lidar odometry and mapping in real-time.” in Robotics: Science and systems, vol. 2, no. 9. Berkeley, CA, 2014, pp. 1–9. [^9]: 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. [^10]: C. Zheng, W. Xu, Z. Zou, T. Hua, C. Yuan, D. He, B. Zhou, Z. Liu, J. Lin, F. Zhu, Y. Ren, R. Wang, F. Meng, and F. Zhang, "Fast-livo2: Fast, direct lidar-inertial-visual odometry," IEEE Transactions on Robotics, vol. 41, pp. 326–346, 2025. [^11]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in 2020 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2020, pp. 5135–5142. [^12]: Y. Gao and L. Zhao, “Ve-liom: A versatile and efficient lidar-inertial odometry and mapping system,” Remote Sensing, vol. 16, no. 15, p. 2772, 2024. [^13]: M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping using the bayes tree,” The International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235, 2012. [^14]: 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. [^15]: 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. [^16]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, "Fast-lio2: Fast direct lidar-inertial odometry," IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2053–2073, 2022. [^17]: P. Chen, X. Zhao, L. Zeng, L. Liu, S. Liu, L. Sun, Z. Li, H. Chen, G. Liu, Z. Qiao et al., "A review of research on slam technology based on the fusion of lidar and vision," Sensors, vol. 25, no. 5, p. 1447, 2025. [^18]: X. Xu, L. Zhang, J. Yang, C. Cao, W. Wang, Y. Ran, Z. Tan, and M. Luo, "A review of multi-sensor fusion slam systems based on 3d lidar," Remote Sensing, vol. 14, no. 12, p. 2835, 2022. [^19]: Z. Fan, L. Zhang, X. Wang, Y. Shen, and F. Deng, “Lidar, imu, and camera fusion for simultaneous localization and mapping: a systematic review,” Artificial Intelligence Review, vol. 58, no. 6, pp. 1–59, 2025. [^20]: Z. Song, X. Zhang, R. Wang, S. Zhang, S. Wu, Y. Wang, and J. Yuan, "Fi-slam: Feature information-based robust and efficient lidar slam," IEEE Transactions on Instrumentation and Measurement, vol. 74, pp. 1–12, 2025. [^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]: G. Kim and A. Kim, "Scan context: Egocentric spatial descriptor for place recognition within 3d point cloud map," in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 4802–4809. [^23]: D. Gálvez-López and J. D. Tardos, “Bags of binary words for fast place recognition in image sequences,” IEEE Transactions on robotics, vol. 28, no. 5, pp. 1188–1197, 2012. [^24]: gisbi kim, “Sc-a-loam,” , 2021, accessed: 2025/7/30. [^25]: —, “Sc-lio-sam,” , 2021, accessed: 2025/7/30. [^26]: —, “Sc-lego-loam,” , 2021, accessed: 2025/7/30. [^27]: —, “Fast_lio_slam,” , 2021, accessed: 2025/7/30. [^28]: A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in Robotics: science and systems, vol. 2, no. 4. Seattle, WA, 2009, p. 435. [^29]: L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scaramuzza, and M. Fallon, “Hilti-oxford dataset: A millimeter-accurate benchmark for simultaneous localization and mapping,” IEEE Robotics and Automation Letters, vol. 8, no. 1, pp. 408–415, 2022. [^30]: L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scaramuzza, and M. F. Fallon, “Hilti-oxford dataset: A millimeter-accurate benchmark for simultaneous localization and mapping,” IEEE Robotics Autom. Lett., vol. 8, no. 1, pp. 408–415, 2023. [Online]. Available: [^31]: C. Klug, C. Arth, D. Schmalstieg, and T. Gloor, “Measurement uncertainty analysis of a robotic total station simulation,” in IECON 2018-44th Annual Conference of the IEEE Industrial Electronics Society. IEEE, 2018, pp. 2576–2582. [^32]: Z. Yuan, Q. Wang, K. Cheng, T. Hao, and X. Yang, “Sdv-loam: Semi-direct visual–lidar odometry and mapping,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 45, no. 9, pp. 11203–11220, 2023. 男性 ., 2022.