2Fast-2Lamaa

This repository contains our open-source implementation of 2Fast-2Lamaaa, a lidar-inertial mapping and localisation framework for large-scale environments.

09/10/2026 EDIT:

• Computational burden divided by 3-4: a series of optimisations of both nodes (caching of the GP and cost computations, cheaper undistortion and registration, and a VDB-like voxel index replacing the i-Octree for the spatial indexing of the map, with slightly more approximate spatial queries).
• Pose at the IMU rate: the gp_map node can now estimate the pose at the frequency of the IMU, published on /imu_rate_odom. It is a sliding-window estimator that fuses the registered poses with the IMU measurements (see Pose at the IMU rate). It is enabled with use_imu_estimator and already turned on in os1_odometry.launch.py, vbr_odometry.launch.py, boreas_odometry.launch.py and boreas_odometry_old.launch.py, where the IMU parameters shared by the two nodes are now defined once at the top of the file.
• Builds and runs on both Ubuntu 24.04 (ROS 2 Jazzy) and Ubuntu 26.04 (ROS 2 Lyrical).

Previous edits are listed at the bottom of this page.

If you are looking to use the undistortion-related code from our IROS publication, or the first version of 2Fast-2Lamaa, please refer to the old branch.

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.

A thourough presentation and performance analysis of 2Fast-2Lamaa is available in our paper’s preprint and journal version.

Corresponding contributor: le.gentil.cedric@gmail.com

Disclaimer

This code is provided as it is. It is research code that is not necessarily optimized for maintainability. We open-sourced this work for the benefit of the robotics community and we are open to suggestions/collaborations to improve this pipeline.

Citing

If you are using this framework or part of it please cite our work as shown at the bottom of this page

Installation

Dependencies

The following dependencies are required to build the package:

• ROS2 (tested with Jazzy on Ubuntu 24.04 and Lyrical on Ubuntu 26.04)
• Ceres (2.2 or newer)
• Eigen
• OpenCV

Please refer to the ROS2 installation guide for Jazzy (Ubuntu 24.04) or Lyrical (Ubuntu 26.04) for the installation of ROS2. The other dependencies can be installed with rosdep from the root of your workspace (once the repository is cloned, see below):

rosdep install --from-paths src --ignore-src -y

or manually using the following command:

sudo apt install libceres-dev libeigen3-dev libopencv-dev

(if the default Ceres version is too old, you need to install Ceres 2.2 from source)

Building

To build the package, clone the repository in your ROS2 workspace and build it using colcon (if you don’t have a workspace yet, create ~/ros2_ws/src):

cd /path/to/your/workspace/src
git clone https://github.com/clegenti/2fast2lamaa.git
cd ..
colcon build --packages-select ffastllamaa
source install/setup.bash

Running

2Fast-2Lamaa is implemented as a ROS2 package named ffastllamaa. Accordingly, you need your data to be in ROS2 bag format to run the package. The lidar and IMU data should be pointcloud2 and imu messages, respectively. You need to know the extrinsic calibration between the lidar and the IMU (the data should also be synchronised ideally) Incorrect knowledge of the extrinsic calibration or bad synchronization will result in poor localisation and mapping performance.

As aforementioned, the core of 2Fast-2Lamaa is made of two main modules/nodes that are launched together via a launch file:

• lidar_scan_odometry, the undistortion and dynamic object filtering node: it uses the IMU measurements to undistort the lidar scans in an optimization-based formulation
• gp_map, the localisation and mapping node: it uses the undistorted scans to perform localisation within a map (global or topometric) that can be built online of mapping, or provided for localisation only.

Thus, 2Fast-2Lamaa can be used in different modes: odometry/mapping or pure localisation. Both these modes can use a global map or a topometric map (set of submaps topologically linked). The mode and map style selection is done through node parameters (detailed below) that can be provided via the launch file.

File truncated at 100 lines see the full file