Breaking the Static Assumption: A Dynamic-Aware LIO Framework Via Spatio-Temporal Normal Analysis

TL;DR

This work tackles LiDAR-Inertial Odometry in dynamic environments where moving objects corrupt pose estimation and mapping under static-world assumptions. It introduces a dynamic-aware ICP that embeds spatio-temporal normals into registration, along with a dual-map registration scheme (temporal M_t and long-term M_v) and a spatial-consistency based static-map verifier to filter dynamic points. Key contributions include the dynamic-aware ICP, the dual-map architecture, a thresholded spatio-temporal normal for dynamic point detection, and a static-map building procedure that reduces false positives, with open-source code and a new dataset. Results show improved robustness in geometrically degraded and dynamic-dominated scenes and real-time performance on standard hardware, highlighting practical applicability for robust localization and mapping in the wild.

Abstract

This paper addresses the challenge of Lidar-Inertial Odometry (LIO) in dynamic environments, where conventional methods often fail due to their static-world assumptions. Traditional LIO algorithms perform poorly when dynamic objects dominate the scenes, particularly in geometrically sparse environments. Current approaches to dynamic LIO face a fundamental challenge: accurate localization requires a reliable identification of static features, yet distinguishing dynamic objects necessitates precise pose estimation. Our solution breaks this circular dependency by integrating dynamic awareness directly into the point cloud registration process. We introduce a novel dynamic-aware iterative closest point algorithm that leverages spatio-temporal normal analysis, complemented by an efficient spatial consistency verification method to enhance static map construction. Experimental evaluations demonstrate significant performance improvements over state-of-the-art LIO systems in challenging dynamic environments with limited geometric structure. The code and dataset are available at https://github.com/thisparticle/btsa.

Figures (7)

• Figure 1: Comparison of our method against FAST-LIO2 xu2022fast for mapping and trajectory estimation. Colored ellipses highlight detected dynamic objects. Our approach reduces localization drift through effective dynamic object handling, especially in challenging geometric environments.
• Figure 2: Space-Time Velocity Vectors in a 2D Example. The instantaneous velocity vectors (in pink) are tangent to the space-time surfaces generated by motion, while the space-time normals (in red) are perpendicular to these surfaces. Each instantaneous velocity vector can be decomposed into a temporal component, determined by the frame rate, and a spatial component representing the underlying motion of points.
• Figure 3: An Overview of the Proposed Framework for Dynamic-Aware Point Cloud Registration and Static Map Construction. The Pre-Processing step undistorts the point cloud, which is then input into the dynamic ICP loop with a Temporal Sliding Window Map. The optimized pose, eliminating dynamic elements, is derived using the proposed spatial-temporal estimation and selective update procedure. The static map is constructed by filtering out false positives from unstable points through a spatial consistency check.
• Figure 4: Spatial Consistency Check. Figure (a) illustrates the causes of false positives in dynamic detection by using spatiotemporal normal vectors over a specified time window. Figure (b) provides an example demonstrating our approach to eliminating false positives based on the spatial consistency principle. Figure (c) is a zoomed-in view of Figure (b) that highlights two distinct cases of false positives.
• Figure 5: Examples of 3D Point Cloud and Scene Image Visualizations from Self-Collected Dataset.