Hierarchical Dynamic Object Removal Boosts Dual-Stage LiDAR-Inertial SLAM Performance

Every autonomous vehicle must answer two questions continuously and simultaneously: where am I, and what does the world around me look like? For robots navigating cities, the standard answer comes from SLAM—Simultaneous Localization and Mapping—systems that fuse laser scans with inertial measurements to build a map while tracking the vehicle’s own position within it. Yet the busiest urban streets are precisely where these systems struggle most, because the world they are trying to map refuses to hold still. Cars cut across the sensor’s field of view, pedestrians stream past curbs, buses glide through intersections. Each moving object leaves a smear of inconsistent returns in the point cloud, producing the ghosting artifacts that plague even state-of-the-art mapping pipelines. A research team at Yanshan University in Qinhuangdao, China, has now introduced a dual-stage LiDAR-inertial SLAM framework that confronts this problem head-on, combining a hierarchical dynamic object removal strategy with a tightly coupled probabilistic estimator. Their work, published in Autonomous Robots, demonstrates substantially lower trajectory error and visibly cleaner static maps in highly dynamic traffic scenes.

The scale of the problem is easy to underestimate. A spinning LiDAR sensor paints its environment with hundreds of thousands of range measurements per sweep, and the localization stage of a SLAM system relies on registering consecutive sweeps against one another and against a persistent map. When a scan contains returns from vehicles moving at urban speeds, the geometric correspondence between scans becomes internally inconsistent: part of the scene has shifted between frames for reasons that have nothing to do with the sensor platform’s own motion. Robust estimation techniques such as maximum-likelihood formulations can down-weight inconsistent returns, but as the Yanshan University team notes, when dynamic motion is not explicitly modeled, scan registration still degrades and the resulting global map accumulates phantom structures—ghost trails of cars that never occupied any single location, parked in the map forever.

The new framework’s first stage attacks the problem at the level of individual range measurements. Rather than comparing successive point clouds directly, the method builds multi-resolution range images—spherical projections of the LiDAR returns onto a pixel grid indexed by azimuth and elevation—and computes differences between consecutive images. A point that vanishes from a later view, or appears where free space was expected, is flagged as potentially dynamic. This approach echoes earlier multiresolution techniques such as Removert, but the Yanshan team adds a crucial refinement: visibility-aware incidence-angle correction. LiDAR returns striking a surface at grazing angles—typically near the ground plane, where the beam skims the asphalt—are notoriously unreliable and are frequently misclassified as dynamic simply because their measured depth is noisy. By explicitly modeling the incidence angle and the sensor’s visibility geometry, the correction step restores these grazing-angle ground points that would otherwise be wrongly discarded, preserving the very ground surface that most odometry pipelines depend on for stability.

The second stage operates at a higher level of abstraction: whole clusters of points rather than individual measurements. After the first pass, some dynamic contamination inevitably survives—points from a slow-turning vehicle whose motion between frames falls below the detection threshold, for example. The cluster-level reclassification stage groups residual candidate points into spatial clusters and evaluates each cluster’s consistency with the emerging static map, demoting clusters that show telltale signatures of motion while recovering static structures that earlier filtering may have unfairly removed. This two-tier hierarchy—point-level differencing followed by cluster-level adjudication—is what the authors describe as building a visibility-consistent static subset: a curated set of returns that the system can assert, with high confidence, belong to objects that were genuinely stationary during the observation window.

Once the refined static points are extracted, they feed into a tightly coupled factor-graph estimator, the mathematical heart of the system. Factor graphs have become the dominant architecture for modern SLAM, representing sensor constraints as probabilistic factors connecting robot states over time, with the full trajectory recovered by nonlinear optimization. The new system incorporates three kinds of factors: IMU pre-integration factors, which compress high-frequency inertial measurements between LiDAR frames into compact relative-motion constraints; scan-context loop closure factors, which recognize previously visited places by comparing compact global descriptors and thereby correct accumulated drift; and a novel static-prior factor that anchors the estimation to the vetted static point subset. Because only visibility-gated, confidence-weighted odometry features participate in the correspondences, the optimization is consistently fed geometric constraints that reflect real, unmoving structure—even amid dense traffic.

The authors evaluate the framework on UrbanLoco, a benchmark dataset captured with a full sensor suite in dense urban environments, alongside a self-collected dynamic traffic dataset recorded on their own instrumented vehicle platform. Against state-of-the-art baselines—including prominent LiDAR-inertial odometry systems—the dual-stage approach achieves up to 11.9 percent lower trajectory error. Just as telling are the qualitative results: maps built by conventional pipelines in busy scenes carry visible smears and duplicated structures, while the new method’s static maps remain clean enough that one can trace curb lines, building facades, and parked-vehicle positions without ambiguity. The improvements hold across long sequences, suggesting the framework delivers on the demanding requirement of long-term map fidelity rather than merely momentary accuracy.

The research, led by Xiao Yang with Baicang Guo as corresponding author, emerges from the School of Vehicle and Energy Engineering at Yanshan University, whose Electric Intelligent Vehicle and Vehicle-Road Collaboration Team provided the experimental platform. Building a real-vehicle system meant the team had to solve engineering problems alongside algorithmic ones—constructing and debugging the sensor rig, synchronizing time stamps across the LiDAR and the inertial measurement unit, and calibrating the extrinsic transform between them, since even millisecond-level timing errors smear the point cloud when the platform turns. The work was supported by the National Natural Science Foundation of China and the National Key R&D Program of China, reflecting the strategic importance China places on autonomous driving and cooperative vehicle-infrastructure technology.

The significance of cleaner static maps extends well beyond academic benchmarks. For autonomous vehicles, the map is a prior: lane-level localization, path planning, and motion prediction all assume the static map faithfully represents permanent structure. Ghost artifacts from dynamic objects corrupt that prior, potentially causing a planner to avoid phantom obstacles or a localizer to snap to a ghost feature. In emerging vehicle-to-everything and high-definition mapping pipelines, maps are often built once and maintained over months or years, making long-term fidelity—the ability of a map built today to remain valid next month—arguably more important than raw per-frame accuracy. A filtering architecture that provably keeps dynamic contamination out of the persistent map addresses exactly this maintenance burden, which has historically required either manual map editing or aggressive, lossy map-update heuristics that sometimes erase genuine static structure along with the noise.

The method also contributes to a broader conceptual shift in the SLAM community. Early dynamic-SLAM approaches leaned on semantic segmentation networks to label objects likely to move—cars, pedestrians, cyclists—and simply deleted the associated points. But semantic priors fail on the unexpected: a fallen traffic cone, a door swinging open, a mattress migrating down the highway. Geometry-first approaches like the one from Yanshan University make no assumption about what should move; they infer motion purely from the observed inconsistency of range measurements across viewpoints, corrected for visibility. This makes the framework robust to the long tail of urban objects while remaining complementary to semantic methods, which could layer on top to further refine the cluster-level reclassification stage.

Challenges remain, as they do for any approach in this space. Visibility-based differencing is inherently sensitive to the vantage geometry of the moving platform: points in regions observed briefly or at extreme ranges have less evidence for or against motion, and the cluster-level stage must therefore make judgment calls with incomplete information. Extremely slow-moving or stationary-then-moving objects—traffic queuing at a light, then accelerating—straddle the boundary between the static and dynamic worlds in ways no single frame can resolve. And the computational cost of building multiresolution range images at every frame must be balanced against the real-time constraints of onboard deployment, a constraint the team addresses through the lightweight design of the differencing stage but which remains a live engineering tension.

Still, the reported gains—nearly twelve percent lower trajectory error on a benchmark designed to stress urban dynamics, plus demonstrably cleaner maps—mark a meaningful step for a problem the field has wrestled with for a decade. The paper lands in a lively literature: recent years have produced removal-first odometry pipelines, pseudo-occupancy-based map cleaners, and efficient dynamic-aware mapping frameworks, each carving away at the ghosting problem from a different angle. What distinguishes the new contribution is its insistence on hierarchy—treating dynamic removal not as a single binary filter but as a graduated pipeline of increasingly informed decisions, each stage feeding a more trustworthy static subset into the estimator below. For the autonomous vehicles that must soon share city streets with millions of moving things, that kind of disciplined skepticism about what the sensor actually saw may prove to be exactly what dependable mapping requires.