2Fast-2Lamaa: Large-Scale Lidar-Inertial Localization and Mapping with Continuous Distance Fields

Abstract

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 over 750km 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.22\% and 0.06 m, respectively. The real-time implementation is publicly available at https://github.com/clegenti/2fast2lamaa.

Published in The International Journal of Robotics Research

References in corpus (21)

• On-Manifold Preintegration for Real-Time Visual-Inertial Odometry
• Voxblox: Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning
• Tightly Coupled 3D Lidar Inertial Odometry and Mapping
• KISS-ICP: In Defense of Point-to-Point ICP -- Simple, Accurate, and Robust Registration If Done the Right Way
• The Newer College Dataset: Handheld LiDAR, Inertial and Vision with Ground Truth
• Efficient Continuous-Time SLAM for 3D Lidar-Based Online Mapping
• Voxgraph: Globally Consistent, Volumetric Mapping using Signed Distance Function Submaps
• PIN-SLAM: LiDAR SLAM Using a Point-Based Implicit Neural Representation for Achieving Global Map Consistency
• Closed-form Preintegration Methods for Graph-based Visual-Inertial Navigation
• Dynablox: Real-time Detection of Diverse Dynamic Objects in Complex Environments
• COIN-LIO: Complementary Intensity-Augmented LiDAR Inertial Odometry
• IDOL: A Framework for IMU-DVS Odometry using Lines
• A Unified Approach for Autonomous Volumetric Exploration of Large Scale Environments under Severe Odometry Drift
• A flexible framework for accurate LiDAR odometry, map manipulation, and localization
• OKVIS2-X: Open Keyframe-based Visual-Inertial SLAM Configurable with Dense Depth or LiDAR, and GNSS
• A Simple Observer for Gyro and Accelerometer Biases in Land Navigation Systems
• RESPLE: Recursive Spline Estimation for LiDAR-Based Odometry
• HiMo: High-Speed Objects Motion Compensation in Point Clouds
• Incorporating Control Inputs in Continuous-Time Gaussian Process State Estimation for Robotics
• Asynchronous Event-Inertial Odometry using a Unified Gaussian Process Regression Framework
• D-LIO: 6DoF Direct LiDAR-Inertial Odometry based on Simultaneous Truncated Distance Field Mapping