Vertically Constrained LiDAR-Inertial SLAM in Dynamic Environments

Shuangfeng Wei, Junfeng Qiu, Anpeng Shen, Keming Qu, Tong YangApplied Sciences 2026 · DOI

TL;DR

This paper tackles the critical challenge of vertical drift in LiDAR-Inertial SLAM systems operating in dynamic environments. By introducing a novel concentric sector model for ground segmentation and a multi-frame residual analysis for dynamic artifact removal, the authors significantly improve the accuracy and robustness of SLAM, leading to more reliable 3D mapping.

What Problem Does This Paper Address?

Simultaneous Localization and Mapping (SLAM) using LiDAR and Inertial Measurement Units (IMUs) is crucial for autonomous systems. However, real-world environments are rarely static. Dynamic objects like pedestrians and vehicles, along with complex terrain, introduce significant challenges. These dynamic elements can create "artifacts" in the LiDAR point clouds, leading to errors in map construction and, critically, vertical drift in the system's estimated trajectory. This drift causes the map to appear to "float" or "sink" over time, degrading the overall accuracy and reliability of the SLAM system. Existing methods often struggle to jointly address ground segmentation, dynamic object removal, and vertical drift suppression, especially in real-time and without relying on heavy deep learning models.

Methods

The proposed solution is a LiDAR-Inertial SLAM pipeline that integrates three key components:

  • Concentric Sector Model for Ground Segmentation: Instead of traditional uniform meshing, this method divides the LiDAR point cloud into adaptive-density concentric regions based on radial distance. This approach accounts for the natural decay of point density with distance, improving accuracy in both near and far fields. It uses a "triple-check" criterion (perpendicularity, elevation, and flatness) to robustly identify ground points.
  • Dynamic Artifact Rejection (Multi-frame Residual Analysis with Curvature Filtering): This module identifies and removes dynamic objects. It combines:
  • Multi-frame Residual Analysis: Tracks changes in non-ground points across consecutive frames to detect motion inconsistencies.
  • Curvature-based Filtering: Uses curvature features to distinguish moving objects from static structures. This combined approach aims to effectively remove dynamic artifacts that cause mapping errors.

  • Vertical Drift Optimization (Factor Graph): The system constructs a factor graph that fuses LiDAR odometry, IMU gravity direction constraints, and the newly extracted ground-plane constraints. This unified framework allows for joint optimization, effectively suppressing vertical drift and refining both pose estimation and map accuracy.

Datasets

The paper evaluates its algorithms using the M2DGR public dataset, released by Shanghai Jiao Tong University. This dataset is described as featuring large-scale, multi-modal characteristics and covering diverse indoor and outdoor scenes. For specific experiments, they also utilized the KITTI odometry benchmark (Sequence 00).

Key Insights and Ideas

  • Adaptive Ground Segmentation is Key: The non-uniform nature of LiDAR point clouds (density decreasing with distance) necessitates adaptive segmentation strategies. The concentric sector model effectively addresses this, leading to more accurate ground plane estimation.
  • Dynamic Objects are a Major Source of Vertical Drift: The paper strongly emphasizes that dynamic objects are not just map clutter but a primary driver of vertical drift in SLAM. Effectively removing them is crucial for stable vertical alignment.
  • Multi-frame Consistency is Essential for Artifact Detection: Relying on single-frame information is insufficient for robustly identifying dynamic objects. Analyzing residuals across multiple frames provides the necessary temporal context to distinguish true motion from sensor noise or static features.
  • Joint Optimization is More Powerful: By integrating ground constraints, IMU gravity, and LiDAR odometry within a single factor graph optimization, the system achieves a more holistic and robust solution for vertical drift suppression than treating these as separate modules.

Key Results

  • Ground Segmentation: The proposed concentric sector model demonstrates improved accuracy and robustness in complex urban road scenarios, handling undulations, cracks, and varying terrain effectively. Quantitative analysis showed an average absolute error of 0.009 m and a maximum error of 0.023 m in urban road tests.
  • Dynamic Artifact Rejection: The multi-frame residual analysis combined with curvature filtering achieved a residual ratio of around 13% across various blocks, outperforming A-LOAM. This indicates a significant reduction in dynamic artifacts.
  • Vertical Drift Optimization: The system achieved a maximum trajectory error of 1.96 m on the M2DGR dataset, significantly lower than baseline methods like LeGO-LOAM (5.63 m), SC-LeGO-LOAM (12.51 m), and FAST-LIO2 (5.63 m). The proposed method showed superior stability and reduced fluctuation, particularly in dynamic zones.

Limitations and Future Work

  • Reliance on High-Quality IMU Data: The paper acknowledges that the system's performance is still dependent on the quality of IMU data.
  • Need for Multi-Sensor Fusion: For larger-scale, more complex, or cross-domain scenarios, the authors suggest that fusing additional sensors like cameras or radar will be crucial. This indicates a future research direction.
  • Scalability and Generalization: While the current system shows good generalization, further research is needed to address the demands of increasingly complex and diverse environments.

Summary updated

Abstract

With the advancement of Light Detection and Ranging (LiDAR) technology and computer science, LiDAR–Inertial Simultaneous Localization and Mapping (SLAM) has become essential in autonomous driving, robotic navigation, and 3D reconstruction. However, dynamic objects such as pedestrians and vehicles, with complex terrain conditions, pose serious challenges to existing SLAM systems. These factors introduce artifacts into the acquired point clouds and result in significant vertical drift in SLAM trajectories. To address these challenges, this study focuses on controlling vertical drift errors in LiDAR–Inertial SLAM systems operating in dynamic environments. The research focuses on three key aspects: ground point segmentation, dynamic artifact removal, and vertical drift optimization. In order to improve the robustness of ground point segmentation operations, this study proposes a method based on a concentric sector model. This method divides point clouds into concentric regions and fits flat surfaces within each region to accurately extract ground points. To mitigate the impact of dynamic objects on map quality, this study proposes a removal algorithm that combines multi-frame residual analysis with curvature-based filtering. Specifically, the algorithm tracks residual changes in non-ground points across consecutive frames to detect inconsistencies caused by motion, while curvature features are used to further distinguish moving objects from static structures. This combined approach enables effective identification and removal of dynamic artifacts, resulting in a reduction in vertical drift.
BibTeX
@inproceedings{wei2026vertically,
  author = {Shuangfeng Wei and Junfeng Qiu and Anpeng Shen and Keming Qu and Tong Yang},
  title = {{Vertically Constrained LiDAR-Inertial SLAM in Dynamic Environments}},
  booktitle = {Applied Sciences},
  year = {2026},
  doi = {10.3390/app16021046},
}
Figure