2Fast-2Lamaa: Large-scale lidar-inertial localization and mapping with continuous distance fields

2.1. Lidar state estimation

There are many different approaches to state estimation in the robotics literature. In this section, we mainly consider optimization-based lidar(-inertial) frameworks. The majority of such methods can be classified into three distinct state estimation categories illustrated in Figure 2: (a) discrete, (b) continuous, and (c) hybrid. As mentioned in the introduction, motion distortion is addressed differently depending on the state representation. The first one (a) dissociates the act of undistortion from state estimation. Thus, motion correction is an open-loop process that preprocesses the scans based on previous estimates, and state estimation is discretely solved at the starting timestamp of each scan. With (b) and (c), state estimation and motion distortion are coupled in a single problem that estimates the continuous trajectory of the sensor (at least locally). Fully continuous approaches (b) generally assume a certain motion model to represent the sensor’s movement with a continuous function. IMU measurements can be used as residuals or control inputs of the system’s trajectory. Hybrid approaches (c) attempt to leverage the best of both (a) and (b) by locally parameterizing the trajectory based on inertial information, while keeping discrete variables at the global scale for simplicity and computational efficiency. The rest of this subsection provides a brief literature review of the three types of methods.OPEN IN VIEWER

Considered as state-of-the-art odometry frameworks, Fast-LIO2 (Xu et al., 2022) and DLIO (Chen et al., 2023) are examples of one-off undistortion processes based on discrete-state estimation. The former uses an iterated Kalman filter, with the core registration step consisting of optimizing point-to-plane residuals between the open-loop-undistorted scans and a global map built incrementally. DLIO introduces a more precise way to undistort the point clouds using a constant-jerk motion model for continuous integration of the IMU measurements. After motion-distortion correction, DLIO performs a variant of Generalized-ICP (Segal et al., 2009) and uses a ‘hierarchical geometric observer’ (Lopez, 2023) to estimate the system’s pose efficiently. Note that DLIO and Fast-LIO2 only optimize for the last sensor pose. Other works leverage a similar undistortion process but optimize over a window of poses in a factor-graph formulation (Shan et al., 2020; Ye et al., 2019). Note that the aforementioned frameworks require sufficient geometric features in the system’s surroundings. To address geometrically degenerated environments, COIN-LIO (Pfreundschuh et al., 2024) proposes to leverage intensity information from the lidar data to extract distinctive features even on flat surfaces. More discrete-state lidar-inertial pipelines are discussed in a recent survey paper (Lee et al., 2024).

As shown in another recent survey paper (Talbot et al., 2025), there is a wide variety of continuous-time state formulations for robotics. Some of the early lidar(-inertial) work built upon continuous-time representations that make strong assumptions about the motion. For example, Zebedee (Bosse et al., 2012) uses piece-wise linear functions to model the system’s trajectory. This corresponds to a strict assumption of constant velocity between control points. Using inertial residuals, it demonstrated the ability to map a 3D environment using a randomly oscillating 2D lidar. Later, LOAM (Zhang and Singh, 2014) used the same constant velocity assumption with both actuated 2D and 3D lidars. This assumption is still used in recent frameworks like CT-LIO (Dellenbach et al., 2022) or MOLA (Blanco-Claraco, 2025). A key contribution of LOAM was the introduction of planar and edge features extracted from the raw lidar data for efficient registration. This concept has been reused in many works, including LeGo-LOAM (Shan and Englot, 2018) and the present work (using a different extraction method).

Throughout the years, other continuous representations have been used to model more complex lidar trajectories. A recurrent formulation is based on splines (Cao et al., 2025; Droeschel and Behnke, 2018). The benefit of splines with respect to discrete state variables has been demonstrated in the context of vision-based SLAM by Cioffi et al. (2022) at the cost of a higher computational cost. Another branch of continuous-time estimation is based on a particular class of sparse GPs (Barfoot, 2024). This approach elegantly models the trajectory based on a probabilistic motion model. For both lidars and radars, continuous GP states with a white-noise-on-acceleration motion prior have demonstrated high levels of accuracy and real-time computation (Burnett et al., 2025a). Note that different motion priors are possible, as demonstrated by Tang et al. (2019) with a white-noise-on-jerk prior. And inertial data can also be used as control inputs (Burnett et al., 2025b; Lilge and Barfoot, 2025).

The hybrid approach shown in Figure 2(c) relies on IMU measurements to locally represent the system’s trajectory. An issue with inertial-based motion prediction/characterization is the strong dependence on initial conditions and biases when integrating and double-integrating the gyroscope and accelerometer measurements. To prevent the reintegration of the IMU data every time the initial conditions are updated during the state optimization process, Lupton and Sukkarieh (2012) introduced the concept of preintegration. It corresponds to the creation of pseudo-measurements that combine the information from multiple IMU measurements without the need to know the initial conditions. Numerous works have improved on the original preintegration method (Eckenhoff et al., 2019; Forster et al., 2017; Yang et al., 2020). The inertial chapter of the SLAM Handbook provides an overview and comparison of several of these works (Huang et al., 2026). Note that, unlike the original preintegration use case, the hybrid state estimation approach does not attempt to combine IMU measurements into a small number of pseudo-measurements but rather requires an ‘upsampling’ of the inertial information for it to be available at any timestamp. This approach was originally introduced to address the issue of lidar-IMU extrinsic calibration by first upsampling the raw inertial readings before performing preintegration at a higher frequency (Le Gentil et al., 2018). Later approaches elegantly addressed preintegration under the scope of continuous state representation (Le Gentil et al., 2020b; Le Gentil and Vidal-Calleja, 2023), enabling full-batch lidar-inertial localization and mapping (Le Gentil et al., 2021). As this hybrid approach is well-suited to asynchronous inertial-aided estimation, other works with event cameras (Le Gentil et al., 2020a; Li et al., 2024) and radars (Hatleskog et al., 2025) have adopted preintegration as their state representation. 2Fast-2Lamaa also uses continuous preintegration to characterize the system’s motion during each lidar scan. The corresponding discrete state is the gravity vector orientation, the linear velocity at the beginning of the scan, and the IMU biases, resulting solely in 11-degree-of-freedom (DoF) to optimize.

2.2. Map representation

Traditionally, robotic maps consist of geometric landmarks jointly estimated with the system’s pose (Dissanayake et al., 2001). Over the past decades, we have seen a constant increase in sensor bandwidth and computational power. Naturally, the robotics community has harnessed these new hardware capabilities with algorithms that build and use denser and denser maps of the environment. Nowadays, the most common representation for lidar state estimation consists of voxel maps where each cell stores the centroid and normal vector of all the points that occurred in the cell. Fast-LIO2 (Xu et al., 2022) is an example of such methods, leveraging point-to-plane registration constraints. Other works, such as KISS-ICP (Vizzo et al., 2023) and KISS-SLAM (Guadagnino et al., 2025), chose to use point-to-point residuals, but suffer from significantly lesser performance on benchmarks such as the Newer College dataset (Ramezani et al., 2020). Other frameworks keep more information in each cell, for example, the local distribution of the points with normal distributions (Biber and Strasser, 2003; Magnusson, 2009). Another way to model the environment is to use sets of surface primitives. Surfels might be the simplest of these primitives and have been used in numerous lidar frameworks (Bosse et al., 2012; Droeschel and Behnke, 2018; Park et al., 2018). Some recent works deal with more complex primitives by directly building and using a mesh of the environment (Lin et al., 2023; Ruan et al., 2023).

The aforementioned representations (point clouds, surfels, meshes, etc.) store information about the environment’s surface. While memory-efficient, they contain limited information about the rest of the space explored by the system. Volumetric approaches, on the other hand, store information over the whole space and can track occupancy, truncated signed distance field (TSDF), etc. These are less explored for lidar-based estimation, but are crucial for autonomous navigation (e.g. to plan safe trajectories through the environment). Voxblox (Oleynikova et al., 2017) and FIESTA (Han et al., 2019) are prime examples of such mapping techniques. Leveraging advances in computer graphics, many mapping frameworks have been built atop the OpenVDB structure (Museth, 2013; Museth et al., 2013). Some fuse information from multiple scans at the TSDF level (Vizzo et al., 2022), where others use the occupancy (Zhu et al., 2021), or the Euclidean distance field (Wu et al., 2025). Focusing on state estimation, VoxGraph (Reijgwart et al., 2020) extends VoxBlox (Oleynikova et al., 2017) and achieves real-time performance. It leverages SDF submaps to compute submap-to-submap pose constraints, which are then integrated into a pose-graph optimization framework. More specific to lidar systems, D-LIO (Coto-Elena et al., 2026) uses a fast truncated distance field stored in a multi-level hashmap to perform scan-to-map registration. While the authors claim scalability to large-scale environments, their experiments on the VBR dataset (Brizi et al., 2024) show a large RAM requirement that cannot be fulfilled without removing information from memory. Another interesting work is shown by Boche et al. (2025) with OKVIS2-X, where vision-based estimation can be complemented with lidar-to-occupancy-map factors (Boche et al., 2024) using the Supereight2 data structure (Funk et al., 2021). Focusing on exploration, Schmid et al. (2021) also leverage volumetric mapping with a submap-based strategy to account for large state estimate drift.

With the democratization of GPU computing, many robotics works have explored the use of neural representations and Gaussian splatting for map representation. Point-SLAM (Sandström et al., 2023) and Gs-icp SLAM (Ha et al., 2024) are examples of both approaches for SLAM with RGB-D sensors. DeepSDF (Park et al., 2019) proposes an auto-decoder architecture to learn and infer the signed distance function at object-scale. Later, iSDF (Ortiz et al., 2022) demonstrates the ability to model and learn such a field online for room-scale environments. While less popular, neural representations can be used for lidar-based state estimation. LocNDF (Wiesmann et al., 2023) addresses the localization problem with a neural field close to a TSDF, but cannot be run at sensor framerate, even in small-scale environments, while Nerf-LOAM (Deng et al., 2023) performs odometry and mapping in large-scale environments. More recently, PIN-SLAM (Pan et al., 2024) showcases top performance on the VBR dataset (Brizi et al., 2024) via TSDF modelling with real-time operations. PINGS (Pan et al., 2025) builds upon PIN-SLAM by coupling Gaussian splats with neural distance fields for improved rendering capacities using both lidar and vision-based sensing.

2Fast-2Lamaa follows a line of work that mixes both surface-based and volumetric information. Similar to the former, it only stores information on the surface of elements in the environment, but allows for the query of a Euclidean distance field approximation over the whole space without large memory requirements. This trend originated from the goal of surface reconstruction with GP implicit surfaces (Williams and Fitzgibbon, 2006), which models a signed distance field close to the objects’ surface. Later, Wu et al. (2021) show the ability to approximate the distance to the closest surface anywhere in space by applying a non-linear operation over a GP-inferred field. It is demonstrated with offline experiments that such GP-based approaches enable lidar state estimation and planning (Wu et al., 2023). Le Gentil et al. (2024b) significantly improve the distance approximation while alleviating the original tradeoff between accuracy and surface interpolation. It represents the foundation of the fast distance field derived in the present work, which does not consider a single and inefficient GP, but breaks down the distance field modelling into many small local GPs. Consequently, our new field formulation enables registration in kilometre-long maps in real time. In contrast, the original work by Le Gentil et al. (2024b) can barely run object-level odometry at 2 Hz.

2.3. Dynamic object rejection

A common assumption of many robotic state estimation algorithms is to consider the system’s environment to be static. This assumption is very rarely verified in real-world applications. While robust techniques like RANSAC or m-estimators enable a robot to deal with a certain level of dynamicity in the scene (considering dynamic object points as outliers), there has been a growing interest in performing dynamic object detection to create maps that only contain static elements (Duberg et al., 2024; Falque et al., 2023; Jia et al., 2024a; Schmid et al., 2023; Wu et al., 2024; Yoon et al., 2019). However, most methods that focus on state estimation in dynamic environments first classify the points before performing standard scan registration (Pfreundschuh et al., 2021), and optionally tracking objects (Jia et al., 2024b). Other methods simply detect dynamic objects after lidar data registration (Le Gentil et al., 2024a; Lichtenfeld et al., 2024), relying on robust estimators to ‘ignore’ dynamic objects during the scan-alignment process. An exception to these two-step approaches is BTSA (Chen et al., 2025), which introduces a dynamic-aware ICP algorithm that couples the problems of dynamic point detection and state estimation in a single process. Another method, HiMo (Zhang et al., 2025), does not address the problem of ego-motion distortion, but tackles the moving object point cloud undistortion through scene flow estimation. We refer the reader to the relevant chapter of the SLAM Handbook (Schmid et al., 2026) for a deeper dive into state estimation in dynamic and deformable environments.

As the problem of dynamic object detection is not the core focus of 2Fast-2Lamaa, the proposed optimization-based motion-distortion correction step relies on robust loss functions to consider dynamic objects as outlier information. However, after scan-to-map registration, dynamic points can be removed from the map in an online or offline manner through some sort of ray tracing, similarly to the work from Pomerleau et al. (2014). In our ablation study, we evaluate the impact of the presence of dynamic points in the map on localization.

4.1. IMU preintegrated continuous state

To undistort the incoming lidar data, let us consider the lidar points and IMU readings that have been collected over a short period of time. For convenience and implementation efficiency, this temporal window is chosen to span over two lidar scans (each defined as a full revolution for spinning sensors), with the previous scan from τ_i-1 to τ _i and the current scan from τ _i to τ_i+1. As formulated for SE(3) by Forster et al. (2017), the rotation, velocity and translation preintegrated measurements from time τ_i-1, Δ R τ i t and Δ p τ i t , correspond to

Δ R τ i - 1 t = ∏ τ i - 1 t Exp ( ω ( t ) − b ω ( t ) ) d t , Δ v τ i - 1 t = ∫ τ i - 1 t Δ R τ i - 1 s ( f ( t ) − b f ( t ) ) d s Δ p τ i - 1 t = ∫ τ i - 1 t ∫ τ i - 1 t Δ R τ i - 1 s ( f ( t ) − b f ( t ) ) d s d t ,

(1)

T I τ i - 1 I t = R I τ i - 1 t p I τ i - 1 t 0 1 with R I τ i - 1 t = Δ R τ i - 1 t , v I τ i - 1 I t = v I τ i - 1 I τ i - 1 + ( t − τ i - 1 ) g τ i - 1 + Δ v τ i - 1 t . p I τ i - 1 I t = ( t − τ i - 1 ) v I τ i - 1 I τ i - 1 + ( t − τ i - 1 ) 2 2 g τ i - 1 + Δ p τ i - 1 t .

(2)

Note that the preintegrated measurements Δ R τ i - 1 t and Δ p τ i - 1 t are computed using a prior knowledge of the biases b ¯ f i - 1 and b ¯ ω i - 1 . Similar to the original on-manifold preintegration work (Forster et al., 2017), the preintegrated measurements are corrected through a first-order Taylor expansion to account for the unknown nature of the biases:

Δ R τ i - 1 t ≈ Δ R ¯ τ i - 1 t Exp ∂ Δ R τ i - 1 t ∂ b ω i - 1 Δ b ω i - 1 , Δ v τ i - 1 t ≈ Δ v ¯ τ i - 1 t + ∂ Δ v τ i - 1 t ∂ b ω i - 1 Δ b ω i - 1 + ∂ Δ v τ i - 1 t ∂ b f i - 1 Δ b f i - 1 , Δ p τ i - 1 t ≈ Δ p ¯ τ i - 1 t + ∂ Δ p τ i - 1 t ∂ b ω i - 1 Δ b ω i - 1 + ∂ Δ p τ i - 1 t ∂ b f i - 1 Δ b f i - 1 ,

(3)

x I τ i - 1 1 = T I τ i - 1 I t T I L x L 1 .

(4)

4.2. Lidar features and data association

Using the raw lidar point clouds, features are extracted independently in the two scans that constitute the temporal window [τ_i-1, τ_i+1]. Similarly to the work by Zhang and Singh (2014), features can be planar or edge points. However, the feature point selection does not follow any type of curvature/roughness score. The planar points P i - 1 and P i are obtained by voxel-subsampling the corresponding scans and retaining only one point per voxel (random selection among the points in each voxel). For the edge points, the proposed method considers each lidar channel/ring independently and detects jumps in range values between consecutively collected points. The latter form the sets of edges E i - 1 and E i . Note that the sets P i and E i are mutually exclusive. Figure 4 shows an example of features obtained over 3 s of data in a suburban environment.OPEN IN VIEWER

Then, the features from the two scans are associated together, between P i - 1 and P i or between E i - 1 and E i , with a k-closest-neighbour search that enforces some conditions that depend on the feature type. For planar points, each point in P i is associated with three features from P i - 1 , enforcing a minimum spread in terms of distance and angle, to avoid colinearity (a minimum of 0.05 m and 75° in our implementation). Similarly, each edge feature in E i is matched with two sufficiently spaced points from E i - 1 (typically 0.05 m). For each point, the association begins with a radius search that provides the set of points A . Starting with the closest point in A , the process iteratively tests the type-specific conditions for each point in A by order of distance, until the required number of valid points is reached (successful association) or when there are no more points to test in A (failed association).

4.3. Motion correction

The motion is estimated via a non-linear least-squares optimization of point-to-plane and point-to-line distances

S ˜ = a r g m i n S ∑ x L j ∈ P i r P j j 2 + ∑ x L j ∈ E i r E j j 2 ,

(5)

r P j = ( x I τ i - 1 j − x I τ i - 1 k ) ⊤ ( x I τ i - 1 k − x I τ i - 1 l ) × ( x I τ i - 1 k − x I τ i - 1 m ) ‖ ( x I τ i - 1 k − x I τ i - 1 l ) × ( x I τ i - 1 k − x I τ i - 1 m ) ‖ ,

(6)

r E j = ‖ ( x I τ i - 1 j − x I τ i - 1 k ) × ( x I τ i - 1 j − x I τ i - 1 l ) ‖ ‖ x I τ i - 1 k − x I τ i - 1 l ‖ .

(7)

The undistortion process presented in this section is performed for every incoming scan and estimates the continuous trajectory between τ_i-1 and τ_i+1. It is done without fixing the motion between τ_i-1 and τ _i in any way. Computing the motion for the duration of two scans at every scan creates an overlap in the consecutive temporal windows. That is why, at each step, the module solely outputs the transformation between τ _i and τ_i+1, ignoring the estimate between τ_i-1 and τ _i . It is important to note that the trajectory over a small temporal window (typically 200 ms) is often close to translation-only movement, or can be close to rotation-only in some scenarios. These motions are not informative enough to make the accelerometer observable, as there is an ambiguity with gravity, as detailed by Tereshkov (2015). To address this issue, a weak zero-mean prior residual r b f c b f i - 1 , with c a constant, is added to prevent unrealistically high accelerometer bias estimates. Thus, (5) becomes

S ˜ = a r g m i n S ∑ x L j ∈ P i r P j 2 + ∑ x L j ∈ E i r E j 2 + ‖ r b f ‖ 2 .

(8)

5.1. Scan-to-map registration

Given a motion-corrected point cloud U i , 2Fast-2Lamaa estimates the scan-to-map transformation T W I τ i using a map M (given or incrementally built as detailed in Section 6), which provides a distance field d(x) that can be queried for any point x. As the undistortion step provides decent pose increment estimates, we do not estimate T W I τ i directly, but perform a right-hand perturbation

T W I τ i = T ˜ W I τ i − 1 T ˜ I τ i − 1 I τ i Exp ( Δ r ) Δ p 0 1 ,

(9)

Δ r ˜ , Δ p ˜ = a r g m i n Δ r , Δ p ∑ x j ∈ U i d T W I τ i x j 1 2

(10)

5.2. Gravity-alignment residual

Inspired by Noh et al. (2025), the scan-to-map registration (10) can be complemented with a gravity-alignment residual r g i , when used for odometry or mapping, as opposed to pure localization. First the gravity vector g _W in the original sensor frame at τ₀ is estimated alongside the system’s linear velocities v W I τ i and IMU biases b _f and b _ω , using the first M estimated poses T W I τ i , obtained through scan-to-map registration. M is programmatically chosen to guarantee that enough translation and rotation occurred in the trajectory to make the accelerometer biases observable. Defining the optimized state S g = { g W , v W I τ 0 , ⋯ , v W I τ M - 1 , b f , b ω } , the corresponding optimization is

S ˜ g = arg min S g ∑ i = 0 M - 2 ‖ r I i ‖ 2 ,

(11)

r I i = Log R W I τ i ⊤ R W I τ i + 1 Δ R τ i τ i + 1 ⊤ v W I τ i + 1 − v W I τ i − Δ τ i g W − R W I τ i Δ v τ i τ i + 1 p W I τ i + 1 − p W I τ i − Δ τ i v W I τ i − Δ τ i 2 g W − R W I τ i Δ p τ i τ i + 1 .

(12)

Then, for any incoming undistorted scan and the associated linear body velocity v I τ i I τ i , a measurement of the gravity g τ i in the current frame is obtained as

g τ i = v I τ i I τ i - Δ R τ i - 1 τ i ⊤ ( v I τ i - 1 I τ i - 1 - Δ v τ i - 1 τ i ) Δ τ i - 1 .

(13)

Finally, the gravity residual r g i is defined as

r g i = arccos g W ⊤ R W I τ i g τ i ‖ g W ‖ ‖ g τ i ‖ ,

(14)

5.3. Topometric localization

Atop localization in a globally consistent map, 2Fast-2Lamaa enables topometric localization (and mapping) for repeated robot operations along the same path. This is inspired by the Teach and Repeat framework (Furgale and Barfoot, 2010), which represents the environment with a graph of overlapping geometric submaps topologically linked based on a demonstrated robot trajectory. The localization process has two ‘layers’. At the high level, it consists of moving from submap to submap (node to node) in the topological graph. At the low level, it performs scan-to-submap geometric registration. This approach alleviates the need for a globally consistent map while allowing high-precision localization for autonomous navigation. We have integrated this mapping and localization strategy into the proposed framework. In its current version, 2Fast-2Lamaa only considers the simplest type of graph: an undirected chain. Concretely, the localization in that chain consists of performing scan-to-submap registration using (10) and the submap associated with the current node in the chain. Then, if the state estimate reaches the edge of the submap, the next node (or previous one, depending on the direction of motion) becomes the current node. Figure 5 illustrates a topometric map in the Suburbs environment of our self-collected dataset. While the trajectory used to create the map drifted from the ground-truth, the topometric nature of the localization enables accurate localization for repeating trajectories (<3 cm lateral position root mean squared error (RMSE); see Section 9.3).OPEN IN VIEWER

6.1. GP-based distance field

The proposed mapping process is based on GP-based distance fields (Le Gentil et al., 2024b). Using a point cloud as input, it performs standard GP regression to obtain a latent field o(x) that can be seen as an occupancy field equal to 1 on object surfaces (where the lidar points lie), and that decreases to zero further away from it. Applying a specific non-linear function over o(x) provides an approximation of the Euclidean distance between the query location x and the closest surface. Formally, o is modelled with a zero-mean GP

o ( x ) ∼ G P 0 , k ( x , x ′ ) ,

(15)

o ^ ( x ) = K x , X K X , X + σ I − 1 1 , var ( o ^ ( x ) ) = k ( x , x ) − k x , X K X , X + σ I − 1 k x , X ⊤ ,

(16)

d ( x ) = r o ( x ) .

(17)

The definition of r depends on the kernel function k

r k ( x , x ′ ) ≜ ‖ x − x ′ ‖ .

(18)

With an unscaled, stationary, isotropic kernel (that can be written as a function of the distance k(x, x′) → k(‖x − x′‖)), the reverting function is the inverse of k. Note that due to the non-linear nature of r, the distance field d is not a GP. Accordingly, the inferred variance from (16) is not really informative about the distance estimate’s uncertainty. While not used in 2Fast-2Lamaa, we propose a novel uncertainty proxy in Appendix A.1.

6.2. Efficient large-scale GP distance field

An issue with naively applying standard GP regression is the cubic computational complexity O(N³) with respect to the number N of points in the map due to the matrix inversion in (16). In this work, we propose a novel and efficient strategy for GP-based distance field by computing local GPs instead of a single global one. The core principle behind this strategy is the spatially limited impact of each input point due to the lengthscale of the kernel. ¹ In other words, performing GP regression considering a large-enough local neighbourhood of points will (locally) yield a similar inferred mean when compared with using all the available observations.

The key to our approach is to leverage both a sparse voxelized representation V of the dense 3D point cloud of the environment, and a spatial index I that allows for efficient point insertion/removal and closest/radius neighbour searches. ² The voxelized representation consists of a hashmap that maps 3-integer tuples to cells, which store the centroid of all the lidar/map points that occurred in the voxel. The tuples are the cell’s grid index that can be computed directly from the 3D coordinates of a point and the fixed cell size.

Integrating new points in the map simply consists of checking if the corresponding cell index is already present in the hashmap. If yes, the cell’s centroid is updated incrementally with the new point location. Otherwise, a new cell is created, and its position is added to the spatial index. Thanks to the hashmap properties, accessing an existing cell or inserting a new one is constant-time O(1) on average. The insertion of a new cell in a spatial index is slower with an O(log(N)) average complexity at best. Fortunately, thanks to the voxelized nature of the stored information, only a small portion of the incoming lidar points lead to cell creation (e.g. a 7.9 km-long suburban trajectory in the Boreas-RT dataset contains over 1.8 trillion lidar points, but the final number of cells in the map is around 10 million).

To query d(x) at any location x, the closest cell v _i in V is queried using the spatial index I . Then, the local neighbourhood around v _i is obtained through a radius search in I . This set of points represents the surface around the closest element to x in the map. An estimate of d(x) can therefore be obtained using (16) and (17) with the aforementioned local neighbourhood as input observations. Considering the voxel nature of V and a fixed radius for the local neighbour search, there is a maximum of points M used for GP inference. Knowing that a query of I is O(log(N)) on average, the overall distance field inference is O(log(N) + M³). As M is fixed, the global complexity for large-scale mapping only grows logarithmically with the size of the observed environment.

There are two main caveats to the proposed field. The first is linked to the use of local GPs, creating a discrete behaviour when potentially switching from one local GP to the next while querying two locations that are side by side. In other words, the global field is not guaranteed to be continuous over the full space R 3 . However, the ‘jumps’ are generally small enough, in terms of value and gradient, that this does not prevent the use of the field in gradient-based optimization problems, as illustrated in this work. The second drawback is that the voxelized representation degrades the overall probabilistic properties of GP-based distance inference. Simply put, naively feeding the voxels’ centroids to the GP inference using σI as the measurement noise gives the same importance to each cell. However, not all cells provide the same amount of information: a cell in which only one point occurred should not have the same impact as a cell that averaged the position of 100 points. Figure 6(a) shows the corresponding negative impact on the field: non-smooth surface and thicker walls. To alleviate this issue, we propose to ‘weight’ the cell centroid observations based on the number of points that occurred in each voxel. Let us denote the per-voxel number of observations as the counter c _i of a cell. Accordingly, in (16), we replace the measurement uncertainty model σI with diag(w) where w _i (components of w) are computed as a function of the cell’s counter and the maximum cell counter in the neighbourhood. Note that a lower w _i means that the cell can be trusted more than a cell with a higher w _i . Thus, the function that transforms c _i to w _i has to be decreasing. We picked a decreasing sigmoid function in the shape of 1/1 + exp(−c _i /c_max). Figure 6(b) illustrates the improved efficient field inference with the proposed weighting mechanism. Appendix A.2 presents a brief quantitative analysis of the proposed efficient GP-based distance field with and without cell weighting.OPEN IN VIEWER

6.3. Free-space carving

In self-driving scenarios (among others), it is impossible to guarantee that the environment is static at the time of mapping. Thus, a mechanism for rejecting dynamic objects from the map can help build cleaner maps for later reuse in a localization-only phase. 2Fast-2Lamaa includes a free-space carving step to remove dynamic points previously inserted in the map and account for changes in the environment, such as parked cars that are no longer present. The principle of the proposed mechanism relies on the comparison between the spherical projection of the current scan and that of the map cells in the vicinity of the current sensor position. First, the scan is projected onto an image-like data structure by discretizing the azimuth and elevation of each point using a resolution of around 1°. Each pixel retains the range value of the closest point that is projected into it. Then, the map points within a radius around the sensor position are queried and projected in the aforementioned image-like data structure using spherical coordinates. If the range of a map point is smaller than the corresponding scan range, using a margin equal to the voxel resolution, it is removed from the map as it occurred between a surface currently observed and the sensor. Figure 7 illustrates this process with dense simulated data for the sake of clarity. Free-space carving can be performed online before inserting a freshly registered point cloud into the map, and offline by revisiting the whole trajectory and scans.OPEN IN VIEWER

8.1. Low-level data structures

The hashmaps in the work use the ankerl::unordered_dense::map implementation (Leitner-Ankerl, 2022a). It has been benchmarked against numerous C++ hashmap implementations and displayed the best overall performance (Leitner-Ankerl, 2022b). The cells are stored via pointers in the hashmap to ensure the compactness of the hashmap storage.

The constraints on the choice of the spatial index I are as follows:

• Fast and scalable insertion of new elements.

The ikd-Trees (Cai et al., 2021), PH-Trees (Zäschke et al., 2014), and i-Octrees (Zhu et al., 2024) match the aforementioned requirements. Appendix A.3 shows a toy-example evaluation. We observed a significant advantage for the i-Octree. For efficiency, the spatial indexing structure is not updated with the changing centroid of the voxels, but the hashmap is. The spatial index relies on the coordinates of the first point that occurred in each voxel. Accordingly, the closest neighbour or radius searches performed with the ‘out-of-date’ spatial index are only approximations. We found this not to be a problem empirically, as the GPs are computed with the up-to-date voxel centroids of the local neighbourhood that is more than twice as large as the voxel resolution. For some specific scenarios, such as queries in the middle of a corridor, the query for the closest voxel in the spatial index, whether it is out-of-date or up-to-date, can erroneously trigger GP computations using points from the furthest of the two sides. A simple workaround is to perform K closest neighbour searches with associated local GP-based distance field computation for each query. The final distance would be the smallest of the K resulting GP-inferred distances. Note that this corridor-like scenario is unlikely when performing scan-to-map registration thanks to the accurate initial guess from 2Fast-2Lamaa’s undistortion module. Thus, our implementation uses K = 1 for registration. For other queries, the default is K = 2.

8.2. Non-linear optimizations

All the optimizations in this work (lidar-inertial undistortion and scan-to-map registration) are based on Ceres, an open-source non-linear least-squares solver (Agarwal and Mierle, 2022). Both (5) and (10) leverage Cauchy loss functions to attenuate the impact of outliers. The analytical Jacobians of the residuals are provided to the solver. To lower the computational cost of the overall pipeline, we adopted a basic keyframing strategy for the scan-to-map registration, performing (10) only when the sensor has moved sufficiently or after a fixed time period.

8.3. GP kernel and distance field

While it was shown that the square-exponential provides the best accuracy for distance field estimation (Le Gentil et al., 2024b), 2Fast-2Lamaa’s implementation uses the rational-quadratic kernel ( 1 − ‖ x − x ′ ‖ 2 / 2 α l 2 ) - α with α = 2 for efficiency, as it avoids the use of operations such as exponential or logarithm, which are computationally costly. The lengthscale l is defined as twice the voxel resolution. To compute the local GP around a voxel, the neighbourhood search uses a radius of two and a half times the voxel resolution.

8.4. Point-cloud filtering

Lidar data is generally not affected by different levels of luminosity in the environment. However, it can be degraded by floating particles in the air, such as dust or precipitation. To increase robustness in more challenging scenarios, 2Fast-2Lamaa offers the possibility to filter incoming point clouds based on the sensor-provided per-point intensity. While not thoroughly analyzed in this paper, we have observed a significant improvement of 2Fast-2Lamaa’s odometry and localization performance on the Farm sequences of the Boreas-RT dataset (Lisus et al., 2026), which were collected on an extremely dusty road. Additionally, a ‘density check’ is always performed on each undistorted point cloud in case the intensity filtering does not sufficiently remove amorphous point clusters. Using the user-provided map resolution, the scan is discretized into voxels. Points that fall into voxels that have more than twelve neighbours are ignored for registration.

8.5. Submapping strategy

When used with submaps, as opposed to building a single global map, 2Fast-2Lamaa uses a very simple submapping strategy: a scan is integrated in a submap if the sensor has travelled less than a user-defined distance γ since the submap creation. Note that the submaps overlap by a ratio β (equal to 0.2 in our experiments). Concretely, it means that a new submap is created when the distance travelled goes above (1 − β)γ. During the overlap, each scan is added to both submaps. When performing topometric localization, the change from one submap to the next is done when the current pose estimate is closer to the poses in the second half of the overlap than to the first half.

9.1. Parameters

Aside from hardware-related variables such as extrinsic calibrations or maximum lidar range, 2Fast-2Lamaa can be tailored to specific applications or environments through a set of user-defined parameters. Table 1 provides an overview of the parameters that differ from dataset to dataset. Notably, the voxel size is smaller for the Newer College dataset, and a single global map is used as the scale of the environment is smaller than in other datasets, and the trajectory does not form large loops with loss of co-visibility. As the VBR dataset contains some indoor and handheld sequences, we use a shorter submap length when compared to the Boreas datasets to increase the potential for submap-to-submap loop closures. Not that the VBR dataset evaluation is the only set of experiments that leverages the proposed loop-closure mechanism. As the Boreas and Boreas-RT datasets’ IMU are of good quality, the optimization-based undistortion provides good odometry information. 2Fast-2Lamaa thus leverages a keyframing strategy to limit the number of scan-to-map registrations, allowing for the use of a higher number of points for registration. Finally, free-space carving is disabled in the Newer College dataset evaluation as very few dynamic objects are present in the scene. Please note that 2Fast-2Lamaa exposes many more parameters for the sake of generality, but only the ones mentioned here have been changed in our evaluation. Performance improvements could be attained by tuning parameters for each specific sequence type.

OPEN IN VIEWER

Table 1.

List and description of the different parameters that differ between the different datasets.

​	Voxel size	Global/submaps	Keyframing	Num pts registration	Planar only	Free-space carving
Description	Size of the voxels for lidar feature extraction and map	Selection between the use of a single global map or submaps of given length γ	Travelled distance, orientation, and time threshold for keyframing scan-to-map registration	Maximum number of points for scan-to-map registration	Using only planar features	Radius for free-space carving
Boreas/Boreas-RT	0.30 m	Submaps 300 m	10 m/15°/0.5 s	8000	No	50 m
VBR	0.30 m	Submaps 50 m + online pose-graph	No	2000	No	50 m
Newer College	0.15 m	Global map	No	2000	Yes	No

9.2. Boreas dataset

9.2.3. Odometry benchmark

Table 2 reports the odometry accuracy of 2Fast-2Lamaa and the different baselines. Except for OG, which displays a very small rotational error, 2Fast-2Lamaa is the most accurate method, significantly outperforming the other frameworks. In this experiment, 2Fast-2Lamaa uses submaps (topometric mapping) as it provides slightly better odometry results than the global map version on the Boreas-RT dataset. Section 9.6 provides more details about this difference. As shown in Table 3, 2Fast-2Lamaa’s accuracy is consistent throughout the different sequences, except for 2021-01-26-10-59, which has been collected in a snowstorm and ice accumulated on the front-facing part of the lidar, thus blocking a significant portion of the sensor’s field of view (cf. Figure 10). As all the methods provide decent odometry estimates, it is difficult to draw any strong conclusion on why our method performs better. Note that STEAM-LIO and LTR elegantly perform motion-distortion correction as part of their continuous-time state estimation without ‘fixing’ the undistortion in an open-loop fashion based on previous state estimates. Thus, the difference in the undistortion strategy is unlikely to explain the difference in the global performance. We believe that our continuous map representation makes a difference, as LTR and STEAM-LIO rely on discrete downsampled point clouds with normals to represent the environment. The impact of the continuous map is investigated in Section 9.6.

OPEN IN VIEWER

Table 2.

Average relative pose accuracy of the proposed method and several baselines on the Boreas dataset leaderboard (Burnett et al., 2023).

Method	Trans. (%)	Rot. (°/100 m)
OG (Le Gentil et al., 2025)	0.54	0.07
LTR (Burnett et al., 2022)	0.54	0.16
STEAM-LIO (Burnett et al., 2025a)	0.45	0.15
2Fast-2Lamaa (ours)	0.27	0.10

OPEN IN VIEWER

Table 3.

Per-sequence odometry error (KITTI metric) of 2Fast-2Lamaa on the Boreas dataset leaderboard.

9.2.4. Localization benchmark

For consistency with the odometry benchmark and to match the topometric nature of LTR’s maps, we used 2Fast-2Lamaa with submaps (as opposed to a single global map). 2Fast-2Lamaa’s submaps correspond to 300 m of trajectory while LTR uses a threshold of 30 m to create a new submap. Table 4 provides the leaderboard results. LTR provides slightly better performance over most of the axes. We believe that the state prior in LTR’s formulation helps stabilize the pose estimate between consecutive scans, thus providing more accurate localization abilities. This is supported by the higher roll and pitch rotational error of 2Fast-2Lamaa: our pure scan-to-map registration does not enforce any motion model nor leverage information from previous state estimates. Analyzing the per-sequence results for 2Fast-2Lamaa, there is no strong correlation between both the lateral and longitudinal error and the weather conditions. However, sequences with snow display a notably higher error along the vertical axis. This is expected as the mapping sequence boreas-2020-11-26-13-58 is free from snow. Accordingly, the snow cover on the ground creates a vertical offset in the estimated poses. Overall, despite slightly worse performance compared to LTR, both methods offer high levels of accuracy with under 5 cm lateral RMSE, which is less than the width of any road lane marking.

OPEN IN VIEWER

Table 4.

RMSE localization error for 2Fast-2Lamaa and LTR on the Boreas dataset leaderboard.

9.3. Boreas-RT

9.3.1. Dataset description

To evaluate the proposed framework, we use data from the Boreas-RT dataset (Lisus et al., 2026). It uses the same sensing platform as the Boreas dataset with the main hardware differences being a different radar firmware and an additional 6-DoF Silicon Sensing DMU41 IMU to prevent data contamination between ground-truth and raw sensor data. In this section, we leverage 16 sequences in four different environments, totalling 120 km of data for benchmarking against various baselines. The different routes are Suburbs, Regional, Tunnel, and Skyway, listed in increasing level of difficulty. We also provide 2Fast-2Lamaa’s results on all of the 60 sequences of Boreas-RT (≈650 km) in Appendix A.4.

Suburbs sequences follow the same route as in the Boreas dataset. The road traffic varies between sequences, but a lot of geometric features are present in the surroundings. The Regional data is collected at a higher speed with fewer geometric features to constrain the vehicle’s pose. The Tunnel route goes through a kilometre-long tunnel with very few geometric features to leverage. Finally, the Skyway sequences go over a bridge/skyway with only a few lampposts as static features. A lot of the non-ground lidar points correspond to moving vehicles. Figure 11 provides images from the onboard camera as well as the OpenStreetMap overlay of the trajectories. Both the Suburbs and Skyway routes form loops, whereas Regional and Tunnel sequences do not revisit previously driven areas. The Regional data present an extra challenge for localization: the route is driven twice in each direction, thus, when mapping with one sequence and localizing with the other three, two of the localization runs experience quite different viewpoints than the one of the mapping step (cf. Figure 12). Similarly, the tunnel sequences are collected while driving in both directions. However, the tunnel consists of two separate tubes (one for each traffic direction) without co-visibility. Thus, our Tunnel localization experiments are run for each driving direction independently (two times: one sequence for mapping, one sequence for localization), and the quantitative results are the average of both.OPEN IN VIEWER

Figure 11.

Images from the onboard camera of the Boreas-RT dataset and trajectory overlays (blue lines) on OpenStreetMap.

OPEN IN VIEWER

Figure 12.

The four Regional sequences have been collected in both directions (two each way). When mapping with one sequence, part of the environment is only visible from the opposite traffic lane, thus leaving gaps in the map (as illustrated here under a bridge, with the map in grey, and the mapping trajectory in green). This makes localization challenging: the second line represents the localization trajectory of the second sequence and is coloured with the localization error (purple low and red high).

9.3.3. Odometry benchmark

The odometry errors obtained with our method and the baselines are reported in Table 5. Both MOLA and Fast-LIO2 experienced failures in at least one sequence (large drift above 10 %, or complete state divergence), with MOLA failing in half of the total sequences. For fairness, we only display the average across the successful runs. When both methods succeed in estimating the vehicle’s trajectory, they reach a similar level of accuracy. LTR performs well when the environment is rich in geometric features (Suburbs and Regional), but its odometry error significantly increases for more challenging routes. 2Fast-2Lamaa is the only method that provides consistent results across all environments without failures. Its translation accuracy outperforms all the baselines. The Suburbs and Regional results of both LTR and 2Fast-2Lamaa highlight the accuracy gain of elegantly modelling the continuous motion of the sensors, thus addressing the problem of motion-distortion correction in a principled way. We believe that the increased difference in accuracy for more challenging sequences can be explained by the use of our continuous dense distance fields that better represent the environment.

OPEN IN VIEWER

Table 5.

Average relative pose accuracy of the proposed method and several baselines on a subset of the Boreas-RT dataset.

Sequence type (length, avg./max. vel.)	MOLA	Fast-LIO2	LTR w/gyro	2Fast-2Lamaa
	(Blanco-Claraco, 2025)	(Xu et al., 2022)	(Burnett et al., 2022)	Topometric (ours)
Suburbs (4 × 7.9 km, 8.1/18.6 m/s)	0.74/0.27	0.74/0.20	0.29/0.09	0.22/0.08
Regional (4 × 9.3 km, 11.1/27.0 m/s)	0.81/0.23 a	0.88/0.19 b	0.35/0.09	0.20/0.08
Tunnel (4 × 1.9 km, 8.9/28.2 m/s)	1.72/0.30 a	0.83/0.17	2.49/0.09	0.34/0.11
Skyway (4 × 11.1 km, 18.2/31.7 m/s)	X a	0.98/0.20	0.59/0.08	0.30/0.09

KITTI odometry metric reported as XX/YY with XX (%) and YY (°/100 m) the translation and orientation errors, respectively.

^aMola displayed high drift (error > 10 % ) on three sequences from Regional, one from Tunnel, and all from Skyway. Average computed by omitting these failed sequences.

^bFast-LIO2 failed on one of the Regional sequences (trajectory divergence). Average computed by omitting that sequence.

9.3.4. Localization benchmark

Table 6 shows the results of our evaluation. On the Suburbs sequences, 2Fast-2Lamaa and LTR perform similarly with a slight advantage for LTR, as in the Boreas dataset. However, for all the other routes, 2Fast-2Lamaa is the best performing method with the smallest translational errors. When considering the more challenging environments (Tunnel and Skyway), the difference between our method and the baselines is larger. 2Fast-2Lamaa’s error stays contained with RMSEs under 0.20 m, while LTR and Fast-LIO-localization display errors of a couple of metres. Note that overall Fast-LIO-localization performs poorly and fails on many sequences (RMSE > 10 m ). Figure 13 shows the localization (position) for all methods and sequences as a function of the distance travelled. We believe that the Fast-LIO-localization implementation does not fuse the odometry information and the regular localization steps appropriately, and potential dropouts exacerbate the issue due to high computational load. This is supported by the noisy nature of Fast-LIO-localization’s plots, which oscillate between centimetre- and metre-level errors. Eventually, most sequences end by drifting away from the original trajectory.

OPEN IN VIEWER

Table 6.

RMSE localization error for 2Fast-2Lamaa and different baselines on a subset of the Boreas-RT dataset.

Seq. type and method	Long. (m)	Lat. (m)	Vert. (m)	Roll (°)	Pitch (°)	Yaw (°)
Suburbs
Fast-LIO-localization (HViktorTsoi, 2020) a	X	X	X	X	X	X
LTR w/gyro (Burnett et al., 2022)	0.032	0.021	0.032	0.013	0.011	0.021
2Fast-2Lamaa topometric (ours)	0.024	0.021	0.034	0.036	0.025	0.026
Regional
Fast-LIO-localization (HViktorTsoi, 2020) a	2.031	0.510	0.603	0.310	0.660	1.183
LTR w/gyro (Burnett et al., 2022)	0.056	0.055	0.061	0.037	0.039	0.035
2Fast-2Lamaa topometric (ours)	0.042	0.035	0.029	0.038	0.036	0.033
Tunnel
Fast-LIO-localization (HViktorTsoi, 2020) a	2.212	0.100	0.048	0.205	0.192	0.715
LTR w/gyro (Burnett et al., 2022)	2.960	0.111	0.044	0.063	0.061	0.071
2Fast-2Lamaa topometric (ours)	0.178	0.024	0.026	0.087	0.020	0.026
Skyway
Fast-LIO-localization (HViktorTsoi, 2020) a	6.105	1.734	0.888	0.482	0.538	0.968
LTR w/gyro (Burnett et al., 2022) b	7.502	0.203	0.085	0.054	0.042	0.175
2Fast-2Lamaa topometric (ours)	0.059	0.026	0.035	0.050	0.030	0.028

^aFast-LIO-localization succeeded (RMSE < 10 m) only on 2 out of 3 Regional sequences, 1 out of 2 Tunnel, and 1 out of 3 Skyway.

^bOnly 1 out of 3 Skyway sequences ran to completion for LTR.

OPEN IN VIEWER

Figure 13.

Localization error (position [m] with logarithmic scale) against travelled distance of 2Fast-2Lamaa and the different baselines on the Boreas-RT dataset. The different sequences are shown with different shades of colour.

We also want to remind the reader that the Regional sequences are run in both directions for localization, while the mapping is done only in one, as previously illustrated in Figure 12. When considering the sequence where the car is travelling in the same direction as the mapping run, the RMSEs are: Long. = 0.030 m, Lat. = 0.025 m, Vert. = 0.029 m, Roll = 0.029°, Pitch = 0.022°, and Yaw = 0.028°. These values are similar to those obtained in the easiest environment (Suburbs), demonstrating robustness to high-velocity trajectories.

9.4. Newer College dataset

9.4.3. Odometry benchmark

Table 7 shows the ATE obtained for each of the sequences. Note that the baseline results are the ones reported by Burnett et al. (2025a) for STEAM-LIO and by Chen et al. (2023) for Fast-LIO2 and DLIO. Overall, all the methods provide a similar level of accuracy, with a slight advantage for 2Fast-2Lamaa for three out of the five sequences. This benchmark demonstrates that our method competes with state-of-the-art frameworks even when considering handheld sensor suites. It also shows that our method can be used with a low-cost IMU, such as the one embedded in the Ouster lidar.

OPEN IN VIEWER

Table 7.

ATE and RMSE localization error (m) for 2Fast-2Lamaa and various baselines on the Newer College dataset sequences.

Metric and method	01_short (1609 m, 1530 s)	02_long (3063 m, 2180 s)	05_quad (479 m, 398 s)	06_spinning (97 m, 120 s)	07_parkland (696 m, 500 s)
ATE
Fast-LIO2 (Xu et al., 2022)	0.378	0.332	0.088	0.078	0.148
DLIO (Chen et al., 2023)	0.360	0.327	0.084	0.061	0.120
STEAM-LIO (Burnett et al., 2025a)	0.304	0.337	0.109	0.082	0.144
2Fast-2Lamaa global map (ours)	0.262	0.266	0.078	0.085	0.115
Localization RMSE
2Fast-2Lamaa global map (ours)	0.128	0.154	0.089	0.091	0.155

9.5. VBR-SLAM dataset

9.5.3. SLAM benchmark

Table 8 shows the results obtained with 2Fast-2Lamaa and the different baselines on both the training and testing sequences. While the ranking of the methods varies from one sequence to another, on average, our method displays the best performance, as shown by the overall score. Note that the second-best method, PIN-SLAM, requires a high-grade GPU to perform close to real-time operations. As discussed later in Section 9.7, 2Fast-2Lamaa only uses a fraction of a consumer-grade laptop CPU to achieve better performance. Our future work includes the formulation of a full-batch optimization built on point-to-surface residuals, with the proposed distance field, for better global map accuracy.

OPEN IN VIEWER

Table 8.

ATE obtained using the VBR-SLAM dataset.

Method	Colosseo (m)	Campus 0 (m)	Campus 1 (m)	Pincio (m)	Spagna (m)	DIAG (m)	Ciamp. 0 (m)	Ciamp. 1 (m)	Overall score
Training sequences
PIN-SLAM (Pan et al., 2024)	0.506	0.555	0.392	0.647	0.480	0.362	1.366	1.253	74.44
SMLE (Bhandari et al., 2024)	1.411	1.196	0.994	0.793	0.696	0.457	1.495	1.308	71.65
KISS-ICP (Vizzo et al., 2023)	1.517	1.026	1.039	0.785	1.048	1.397	2.808	2.153	68.23
2Fast-2Lamaapose-graph (ours)	0.634	0.900	0.584	0.624	0.442	0.359	1.141	0.831	74.49
Testing sequences
PIN-SLAM (Pan et al., 2024)	0.422	0.570	0.864	1.063	0.373	0.394	1.790	1.521	73.00
SMLE (Bhandari et al., 2024)	0.454	0.897	1.022	1.071	0.247	0.456	1.350	1.396	73.11
KISS-ICP (Vizzo et al., 2023)	0.525	1.019	1.289	0.864	0.271	0.463	1.700	2.566	71.30
2Fast-2Lamaapose-graph (ours)	0.402	0.484	0.874	0.756	0.350	0.405	1.154	1.558	74.02

9.5.4. Odometry benchmark

For completeness, we also provide the RPE metrics as reported on the VBR-SLAM leaderboard in Table 9. SMLE displays the best performance on this benchmark. 2Fast-2Lamaa’s results are, respectively, second and last on the training and testing sequences. It is important to note that the ground-truth of the VBR-SLAM dataset is obtained as a full-batch optimization of the lidar data using the RTK-GNSS measurements as priors. This approach does not provide an independent ground-truth and highly depends on the registration algorithm used. While sufficient for global trajectory consistency analysis (via the ATE), we believe it creates biases in the RPE evaluation. This latter metric is highly sensitive to the rotational accuracy of the ground-truth due to the lever effect of orientation inaccuracies when using long trajectory chunks as done in this leaderboard. For example, a rotation error of 0.1° corresponds to a translation error of around 0.17 m after 100 m travelled, which represents a minimum relative error of 0.17 %. To the best of our knowledge, at the time of writing, the VBR-SLAM ground-truth is computed without accounting for motion distortion. Accordingly, the rotational accuracy of the ground-truth is probably suboptimal, especially on handheld sequences where the sensors’ orientation can change a lot during each lidar scan (≫0.1°). This correlates with the fact that the best performing method, SMLE, does not perform motion-distortion correction, and that 2Fast-2Lamaa’s performance is significantly lower on the handheld sequences as opposed to the automotive ones.

OPEN IN VIEWER

Table 9.

RPE obtained using the VBR-SLAM dataset.

Method	Colosseo (%)	Campus 0 (%)	Campus 1 (%)	Pincio (%)	Spagna (%)	DIAG (%)	Ciamp. 0 (%)	Ciamp. 1 (%)	Overall score
Training sequences									​
PIN-SLAM (Pan et al., 2024)	0.490	0.243	0.155	0.454	11.61	0.468	0.738	0.648	66.80
SMLE (Bhandari et al., 2024)	0.431	0.613	0.312	0.461	0.310	0.457	0.424	0.293	76.70
KISS-ICP (Vizzo et al., 2023)	0.453	0.515	0.300	0.486	0.399	1.791	0.650	0.395	75.01
2Fast-2Lamaapose-graph (ours)	0.905	0.599	0.390	0.821	0.581	0.718	0.525	0.372	75.09
Testing sequences
PIN-SLAM (Pan et al., 2024)	0.496	0.228	0.526	1.082	0.456	0.550	1.018	0.769	74.88
SMLE (Bhandari et al., 2024)	0.393	0.251	0.639	0.557	0.323	0.515	0.612	0.282	76.43
KISS-ICP (Vizzo et al., 2023)	0.453	0.250	0.655	0.488	0.410	0.776	0.761	0.475	75.73
2Fast-2Lamaapose-graph (ours)	0.969	0.356	0.703	0.932	0.701	1.025	0.653	0.629	74.03

9.6. Ablation study

2Fast-2Lamaa is a fairly modular framework. Thus, numerous combinations of components/features can be used (global map vs submaps, using edge features or not, cleaning the map or not, …) to tailor the performance to a specific environment or application. In this section, we perform an ablation study using our 16-sequence subset of Boreas-RT (presented in Section 9.3). We refer to the full framework used to generate 2Fast-2Lamaa’s results on Boreas and Boreas-RT datasets (Sections 9.2 and 9.3, respectively) as the ‘baseline’. Each variant corresponds to a single difference from the baseline. All the results are presented in Tables 10 and 11 for the odometry and localization modes, respectively, and are discussed by theme in the following subsections.

OPEN IN VIEWER

Table 10.

Ablation study of 2Fast-2Lamaa based on the Boreas-RT dataset and using the average translational KITTI odometry metric (%).

Variant	Suburbs	Regional	Tunnel	Skyway
Baseline	0.22	0.20	0.34	0.30
No edge	0.22	0.20	0.46	0.26
No field	0.29	0.24	0.35	0.31
Global map	0.27	0.20	0.32	0.32
No map	0.55	0.66	0.56	0.85
No carving	0.31	0.20	0.33	0.31 a
No acc.	0.23	0.21	0.33	0.30
No IMU	0.25	0.25	0.36	0.33
No keyframe	0.25	0.26	0.31	0.36

OPEN IN VIEWER

Table 11.

Ablation study of 2Fast-2Lamaa based on the Boreas-RT dataset and using the RMSE position error (m).

Seq. type and variantact	Long.	Lat.	Vert.
Suburbs
Baseline	0.023	0.021	0.034
Global map	0.024	0.022	0.034
Online-carved map	0.024	0.022	0.034
Uncarved map	0.024	0.022	0.034
Regional
Baseline	0.042	0.035	0.029
Global map	0.044	0.033	0.028
Online-carved map	0.042	0.034	0.030
Uncarved map	0.042	0.033	0.029
Tunnel
Baseline	0.178	0.024	0.026
Global map	0.216	0.025	0.025
Online-carved map	0.174	0.025	0.026
Uncarved map	0.147	0.025	0.025
Skyway
Baseline	0.059	0.026	0.035
Global map	0.061	0.034	0.060
Online-carved map	0.057	0.026	0.036
Uncarved map	0.052	0.026	0.036

9.7. Computational requirements

All the variants of 2Fast-2Lamaa presented in this paper run in real time on a laptop equipped with an Intel i7-1370p CPU and 32 GB of RAM. Table 12 shows the CPU load and final RAM usage for a selection of variants from our ablation study. All the variants are lightweight and do not use more than 16 % of the CPU. Whether it is for odometry or localization, the main difference between using a global map or submaps resides in the larger RAM usage when using a global map. The similar CPU load in both scenarios highlights the efficiency of the i-Octree used for spatial indexing. Not using GPs or free-space carving results in a non-negligible drop in computational load. An even lighter version could be run by using neither of the two features. The No keyframe variant leads to the highest computational load. However, this is still moderate and leaves a lot of free CPU resources for other components in a standard robot navigation stack. In terms of computation time for the Baseline, on a typical Suburbs sequence, the undistortion/odometry optimization takes 41 ms on average, the scan-to-maps registration (for each keyframe) requires 61 ms, and the subsequent map update lasts 54 ms with free-space carving and 19 ms without. Note that these numbers can be changed at will by changing the number of threads allocated to each one of the different modules. Note that the baselines (Fast-LIO2, MOLA, and LTR) all required a more powerful computer to run in real time (Intel i9-12900K).

OPEN IN VIEWER

Table 12.

Computational load of various configurations of 2Fast-2Lamaa. The ‘core usage’ corresponds to the average number of CPU cores used by the specific component.

Variant	Undist. core usage	Loc. core usage	Total CPU (%)	Undist. RAM (GB)	Loc. RAM (GB)
Odometry
Baseline	1.19	0.73	9.60	0.17	0.33
Global map	1.14	0.88	10.4	0.17	1.57
No field	1.09	0.36	7.24	0.17	0.31
No carving	1.04	0.59	8.12	0.19	0.33
No keyframe	1.42	1.66	15.4	0.18	0.50
Localization
Baseline	1.06	0.71	8.86	0.17	0.32
Global map	1.05	0.78	9.16	0.16	2.24