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

TL;DR

2Fast-2Lamaa tackles large-scale lidar-inertial localization and mapping by tightly coupling a continuous-time motion-distortion correction module with a Gaussian-process distance-field map for scan-to-map registration. Its 11-DoF continuous state, derived from IMU preintegration over two lidar scans, enables accurate undistortion without reliance on prior pose estimates, while the GP-based map provides smooth, queryable distances to surfaces for robust registration and localization. The framework supports both full mapping and pure localization, offers topometric localization via submaps, and includes offline loop closure to achieve global consistency. Across automotive and handheld datasets, it achieves state-of-the-art or near state-of-the-art accuracy with real-time CPU-based performance and demonstrates strong robustness in challenging environments, including tunnels and snowy conditions.

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 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.

Figures (18)

• Figure 1: 2Fast-2Lamaa performs odometry, mapping, and localization over large-scale environments. It relies on GP-based distance fields for scan-to-map registration. Thanks to efficient data structures, the map can contain details of the environment's geometry while allowing large-scale operations. The images here are visualizations of the map created with an 8-long Suburbs sequence from our self-collected dataset. Despite the length of the trajectory, the map can represent all the observed geometry (a), without sacrificing details (b).
• Figure 2: Most optimization-based lidar state estimation frameworks can be classified into three categories. (a) represents discrete-time estimation where the system's pose is estimated at a finite set of timestamps. The trajectory between timestamps is not estimated. (b) leverages a function over the whole duration of operations to represent the motion continuously. Such a method generally relies on some motion model and is parameterized by a set of supporting points. (c) shows a hybrid approach that locally characterizes the trajectory continuously based on inertial data, but uses discrete state variables at the global scale. This approach does not require an explicit motion model, but the global trajectory shows discontinuities when switching to a new timestamp. 2Fast-2Lamaa is built on the latter paradigm.
• Figure 3: 2Fast-2Lamaa consists of two functional blocks. The first one is an optimization-based motion correction module that undistorts lidar scans using continuous IMU preintegration to characterize the system motion with only 11-DoFs. Once corrected, the scans are used for scan-to-map registration to estimate the global pose of the system.
• Figure 4: Accumulated lidar features (planar in green, edge in magenta) over 3 of data in a Suburbs sequence from our self-collected dataset.
• Figure 5: Example of topometric map obtained on a Suburbs sequence of our self-collected dataset. It consists of a succession of submaps (shown as point clouds of different colours) and a topological graph (shown in red, with the sphere being the submaps' centroids) that connects the submaps. This topometric map does not require global consistency to enable state-of-the-art localization in repeating trajectories.