Abstract: Modern 3D laser-range scanners have a high data rate, making online simultaneous localization and mapping (SLAM) computationally challenging. Recursive state estimation techniques are efficient but commit to a state estimate immediately after a new scan is made, which may lead to misalignments of measurements. We present a 3D SLAM approach that allows for refining alignments during online mapping. Our method is based on efficient local mapping and a hierarchical optimization back-end. Measurements of a 3D laser scanner are aggregated in local multiresolution maps by means of surfel-based registration. The local maps are used in a multi-level graph for allocentric mapping and localization. In order to incorporate corrections when refining the alignment, the individual 3D scans in the local map are modeled as a sub-graph and graph optimization is performed to account for drift and misalignments in the local maps. Furthermore, in each sub-graph, a continuous-time representation of the sensor trajectory allows to correct measurements between scan poses. We evaluate our approach in multiple experiments by showing qualitative results. Furthermore, we quantify the map quality by an entropy-based measure.
Introduction
Simultaneous Localization and Mapping (SLAM) with 3D Lidar is a critical capability for autonomous systems, yet it remains a balancing act between real-time performance and mapping accuracy. This paper addresses two primary challenges in the field. First, the "commitment problem" inherent in many efficient SLAM systems, where pose estimates are finalized as soon as a scan arrives, baking errors into the map. Second, the motion distortion or "rolling shutter" effect caused by the sensor's movement during a single scan rotation (typically ~100ms).
System Architecture
The proposed system is structured into a hierarchical pipeline designed for both efficiency and the ability to revisit and refine past estimates. The architecture is divided into three main stages: preprocessing, local mapping, and allocentric mapping, with a continuous refinement loop.
In the preprocessing stage, raw scan lines are filtered and assembled into 3D scans using initial motion estimates from IMU or wheel odometry. Local mapping then aggregates these scans into robot-centric, multiresolution maps using surfel-based registration. This process transforms sparse point clouds into dense, usable surface models. Finally, allocentric mapping manages the global SLAM graph, where local maps are integrated into a larger environment model.
Hierarchical Optimization and Continuous-Time Trajectory
A core innovation of this work is the multi-level graph structure that enables online refinement. The environment is represented as a graph of map nodes ($\mathcal{M}$), where each map node contains a sub-graph of individual scan nodes ($\mathcal{S}$).
To address motion distortion, the system utilizes a continuous-time trajectory representation. Instead of discrete snapshots, the sensor's path is modeled using cubic B-splines in SE(3). This allows the system to interpolate the exact pose for every scan line ($\mathcal{L}$) within a rotation, effectively "un-distorting" the data. The hierarchical nature allows for "local window" optimizations; the system can refine the alignment of scans within a local map to correct drift without the computational burden of re-optimizing the entire global map at every step.
Experimental Evaluation
The approach was validated using two distinct datasets: a MAV Courtyard dataset collected by a drone and the Deutsches Museum dataset provided by Google Cartographer.
To quantify map quality, the authors employed Mean Map Entropy (MME), where lower values indicate sharper, more consistent maps. On the Deutsches Museum dataset, the proposed method achieved an MME of -2.42, significantly outperforming Google Cartographer's score of -2.04. Qualitatively, the system demonstrated a dramatic reduction in "ghosting" effects, consolidating double-images of walls into single, sharp planes.
In terms of performance, the refinement of a single map node takes approximately 54 ms. The system's ability to process multiple map nodes in parallel (e.g., 16 nodes in ~380 ms) ensures it remains suitable for online operation on modern hardware.
Conclusion
By combining hierarchical graph optimization with a continuous-time trajectory model, this framework provides a robust solution for high-fidelity 3D Lidar SLAM. The ability to refine past alignments online allows the system to recover from initial estimation errors, while the B-spline representation ensures that the resulting maps are free from motion-induced distortions. Future work will look toward more advanced dynamic object filtering and deeper integration of IMU pre-integration to handle high-acceleration maneuvers.
Summary updated