2Fast-2Lamaa: Large-Scale Lidar-Inertial Localization and Mapping with Continuous Distance Fields
作者: Cedric Le Gentil, Raphael Falque, Daniil Lisus, Timothy D. Barfoot
分类: cs.RO
发布日期: 2024-10-07 (更新: 2025-12-08)
🔗 代码/项目: GITHUB
💡 一句话要点
2Fast-2Lamaa：基于连续距离场的大规模激光雷达惯性里程计与建图
🎯 匹配领域: 支柱七：动作重定向 (Motion Retargeting)
关键词: 激光雷达 惯性里程计 SLAM 连续距离场 高斯过程 运动畸变校正 状态估计
📋 核心要点
- 现有激光雷达惯性里程计在处理大规模、高动态场景时，往往面临精度下降和计算复杂度高的挑战。
- 2Fast-2Lamaa通过连续IMU预积分和高斯过程距离场，实现了高效且精确的激光雷达扫描去畸变和地图构建。
- 实验结果表明，该框架在多种数据集上达到了最先进的性能，里程计和定位误差显著降低。
📝 摘要（中文）
本文提出了一种名为2Fast-2Lamaa的激光雷达惯性状态估计框架，用于里程计、建图和定位。其首要关键组件是基于优化的激光雷达扫描去畸变，该组件利用连续IMU预积分来建模系统在每个激光雷达点时间戳的位姿。在100-200ms内的连续轨迹仅由初始扫描条件（线速度和重力方向）和IMU偏差参数化，产生11个状态变量。通过最小化激光雷达提取的特征之间的点到线和点到面距离来估计这些变量，而不依赖于先前的估计，从而产生一种无先验的运动畸变校正策略。由于该方法执行局部状态估计，因此直接提供扫描到扫描的里程计。为了保持更长时间内的几何一致性，使用去畸变的扫描进行扫描到地图的配准。地图表示采用高斯过程来形成连续距离场，从而能够在空间中的任何位置进行点到表面距离查询。通过非线性最小二乘优化最小化这些距离来细化去畸变扫描的位姿。对于里程计和建图，地图以实时方式增量构建；对于纯定位，重用现有地图。增量地图构建还包括移除动态对象的机制。我们在来自汽车和手持系统的250公里（超过10小时）的公共和自收集数据集上对2Fast-2Lamaa进行了基准测试。该框架在各种具有挑战性的场景中实现了最先进的性能，里程计和定位误差分别低至0.27%和0.06米。实时实现可在https://github.com/clegenti/2fast2lamaa公开获取。
🔬 方法详解
问题定义：现有激光雷达惯性里程计方法在大规模场景中，由于累积误差和动态环境的影响，难以保证定位精度和地图质量。此外，传统的点云地图表示方法在进行距离查询时效率较低，限制了实时性应用。
核心思路：2Fast-2Lamaa的核心思路是利用连续IMU预积分进行精确的激光雷达扫描去畸变，并采用高斯过程构建连续距离场地图，从而实现高精度、高效率的里程计和建图。通过无先验的运动畸变校正策略，减少了对先前估计的依赖，提高了鲁棒性。
技术框架：2Fast-2Lamaa框架主要包含以下几个阶段：1) 激光雷达数据采集和IMU数据预积分；2) 基于优化的激光雷达扫描去畸变，利用连续IMU预积分估计扫描期间的位姿；3) 特征提取，提取点到线和点到面的特征；4) 局部状态估计，通过最小化特征距离进行里程计计算；5) 基于高斯过程的连续距离场地图构建；6) 扫描到地图的配准，通过最小化点到表面距离优化位姿；7) 动态对象移除，维护地图的静态性。
关键创新：该方法最重要的创新点在于：1) 采用连续IMU预积分进行激光雷达扫描去畸变，能够更精确地建模运动过程；2) 使用高斯过程构建连续距离场地图，实现了高效的点到表面距离查询，提高了配准效率；3) 提出了一种无先验的运动畸变校正策略，增强了系统的鲁棒性。与现有方法相比，该方法在精度和效率上都有显著提升。
关键设计：在连续IMU预积分中，轨迹由初始扫描条件（线速度和重力方向）和IMU偏差参数化，仅使用11个状态变量，降低了计算复杂度。高斯过程距离场的核函数选择和参数优化对地图质量和查询效率至关重要。动态对象移除机制的设计需要权衡移除效果和计算成本。
🖼️ 关键图片
📊 实验亮点
2Fast-2Lamaa在250公里（超过10小时）的公共和自收集数据集上进行了基准测试，包括汽车和手持系统的数据。实验结果表明，该框架在各种具有挑战性的场景中实现了最先进的性能，里程计和定位误差分别低至0.27%和0.06米。这些结果表明，2Fast-2Lamaa在精度和鲁棒性方面具有显著优势。
🎯 应用场景
2Fast-2Lamaa具有广泛的应用前景，可应用于自动驾驶、机器人导航、三维重建、虚拟现实等领域。该框架能够提供高精度、实时的定位和地图信息，为自动驾驶车辆提供可靠的环境感知能力，也可用于机器人自主导航和环境探索。此外，该技术还可用于构建高精度的三维地图，为城市规划、文物保护等领域提供支持。
📄 摘要（原文）
This paper introduces 2Fast-2Lamaa, a lidar-inertial state estimation framework for odometry, mapping, and localization. Its first key component is the optimization-based undistortion of lidar scans, which uses continuous IMU preintegration to model the system's pose at every lidar point timestamp. The continuous trajectory over 100-200ms is parameterized only by the initial scan conditions (linear velocity and gravity orientation) and IMU biases, yielding eleven state variables. These are estimated by minimizing point-to-line and point-to-plane distances between lidar-extracted features without relying on previous estimates, resulting in a prior-less motion-distortion correction strategy. Because the method performs local state estimation, it directly provides scan-to-scan odometry. To maintain geometric consistency over longer periods, undistorted scans are used for scan-to-map registration. The map representation employs Gaussian Processes to form a continuous distance field, enabling point-to-surface distance queries anywhere in space. Poses of the undistorted scans are refined by minimizing these distances through non-linear least-squares optimization. For odometry and mapping, the map is built incrementally in real time; for pure localization, existing maps are reused. The incremental map construction also includes mechanisms for removing dynamic objects. We benchmark 2Fast-2Lamaa on 250km (over 10h) of public and self-collected datasets from both automotive and handheld systems. The framework achieves state-of-the-art performance across diverse and challenging scenarios, reaching odometry and localization errors as low as 0.27% and 0.06 m, respectively. The real-time implementation is publicly available at https://github.com/clegenti/2fast2lamaa.