Ground-Fusion++: Robust Modular SLAM

• Ground-Fusion++ is a degradation-aware modular SLAM framework that intelligently switches between LiDAR-Inertial and Visual-Inertial subsystems under varying conditions.
• It integrates GNSS, RGB-D, LiDAR, IMU, and wheel odometry with robust frame alignment and smoothing to maintain continuous and accurate localization.
• The system demonstrates significant improvements on the M3DGR benchmark, reducing localization errors in scenarios with visual degradation, LiDAR degeneracy, and other sensor failures.

Searching arXiv for the specified Ground-Fusion++ paper and closely related precursor work to support the article. {"^^^^^^^^6query6 (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^^^^^", "^^^^^^^^6max_results6 ^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6query6^^^^} Continuing with a narrower arXiv search focused on the Ground-Fusion++ title and the original Ground-Fusion system. {"^^^^^^^^6query6 Robust Sensor-Fusion Ground SLAM^^^^^^^^6\6 OR ^^^^^^^^6\6 OR \6^^^^} arxiv_search(^^^^^^^^6query6 Ground-Fusion M^^^^6query6^^^^DGR", ^^^^^^^^6max_results6 (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6query6^^^^) Ground-Fusion++ is a degradation-aware, modular multi-sensor fusion framework for ground-robot simultaneous localization and mapping (SLAM). Introduced alongside the M^^^^6query6^^^^DGR benchmark, it is designed for conditions in which no single sensing modality remains reliable across all corner cases, notably visual degradation, LiDAR degeneracy, wheel slippage, and GNSS denial. Its defining configuration couples GNSS, RGB-D, LiDAR, IMU, and wheel odometry; uses a LiDAR-Inertial Odometry (LIO) subsystem as the primary pose source; falls back to an enhanced Visual-Inertial Odometry (VIO) subsystem when LiDAR quality degrades; and supports real-time dense colorized mapping through an IMMesh-style rendering pipeline (&&&^^^^6query6^^^^&&&).

^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^. Problem setting and lineage

Ground-Fusion++ was introduced to address a central practical issue in ground-robot SLAM: no single sensor modality is reliable in all corner-case conditions. The motivating failure modes are explicitly enumerated as visual degradation under low light, changing illumination, motion, and dynamic occlusion; LiDAR degeneracy in corridors, elevators, and sparse geometry; wheel slippage under wheel float, sharp turns, grass, and rough roads; and GNSS denial in tunnels, urban canyons, or covered regions. The framework is therefore not merely multi-sensor; it is explicitly organized around sensor unreliability and fallback behavior (&&&^^^^6query6^^^^&&&).

The system is positioned as an extension of Ground-Fusion, a low-cost ground SLAM system that tightly integrated RGB-D, IMU, wheel odometry, and GNSS and emphasized adaptive initialization, sensor anomaly detection, and robust localization in corner cases (&&&^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^&&&). Relative to that precursor, Ground-Fusion++ adds LiDAR to Ground-Fusion’s sensing stack, replaces a monolithic tightly coupled design with a modular hybrid architecture, and prioritizes LIO while retaining an enhanced Ground-Fusion-based VIO as a backup subsystem. The addition of LiDAR is motivated by improved long-term outdoor robustness and better stability in feature-poor or GNSS-denied settings, while the overall design is described as scalable and upgradable rather than a single fixed estimator (&&&^^^^6query6^^^^&&&).

A common misconception is to interpret Ground-Fusion++ as simply “Ground-Fusion plus LiDAR.” The paper’s own description is narrower and more architectural: the essential change is a resilient modular framework with degradation detection, subsystem switching, frame alignment, and transition smoothing, rather than a single estimator reformulation (&&&^^^^6query6^^^^&&&).

^^^^6max_results6^^^^. System architecture and sensing roles

Ground-Fusion++ comprises six main modules: a continuous-time Fast-LIO^^^^6max_results6^^^^-based LIO subsystem, an enhanced Ground-Fusion-based VIO subsystem, subsystem switching and fallback logic, a frame alignment module, a trajectory smoothing or transition module, and a rendering or mapping module. The LIO subsystem is the default localization source whenever LiDAR quality is sufficient. The VIO subsystem uses RGB-D, IMU, wheel odometry, and GNSS, and inherits Ground-Fusion’s degradation detection and adaptive sensor selection behavior. The switching logic monitors LiDAR quality and transfers control from LIO to VIO when degradation is detected. Frame alignment estimates the rigid transformation between LIO and VIO frames, while the transition module reduces discontinuities during subsystem changes. The rendering module consumes RGB, LiDAR point clouds, and odometry from the active subsystem and produces dense colorized maps (&&&^^^^6query6^^^^&&&).

The sensing stack is functionally differentiated. GNSS provides global positioning when available and is used for drift reduction over long trajectories. The RGB-D camera supports visual tracking and dense mapping and is intended to remain useful when LiDAR or GNSS are weak. LiDAR is the primary odometric source in stable geometric environments and is particularly strong in large-scale outdoor scenes. IMU contributes inertial propagation and motion constraints, especially during rapid motion or temporary sensing issues. Wheel odometry adds motion constraints tailored to ground robots and is particularly useful for reducing drift in visually degraded or GNSS-denied conditions (&&&^^^^6query6^^^^&&&).

This division of labor matters because the framework is designed so that modalities compensate for one another rather than being fused indiscriminately. The paper’s emphasis is on adaptive modality selection. The architecture therefore treats robustness as a system property emerging from redundant sensing, quality assessment, fallback logic, and robust state transfer, not as a by-product of simply increasing the sensor count (&&&^^^^6query6^^^^&&&).

^^^^6query6^^^^. Degradation detection, switching policy, and transition mathematics

The central adaptive mechanism is LiDAR degradation detection. The paper defines a LiDAR degradation indicator

PRESERVED_PLACEHOLDER_^^^^6query6^^^^

where PRESERVED_PLACEHOLDER_^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^ is the LiDAR feature count at time PRESERVED_PLACEHOLDER_^^^^6max_results6^^^^, PRESERVED_PLACEHOLDER_^^^^6query6^^^^ is the ICP alignment residual between consecutive scans, and PRESERVED_PLACEHOLDER_^^^^6\6^^^^ are empirical thresholds. The alignment residual is defined as

PRESERVED_PLACEHOLDER_^^^^6 OR \6^^^^

with $\mathbf{p}_i$ and $\mathbf{q}_i$ denoting corresponding point pairs and $N$ the number of matched points (&&&^^^^6query6^^^^&&&).

The switching policy is intentionally asymmetric. If LiDAR is healthy, Ground-Fusion++ uses LIO. If LiDAR is degraded, it switches to VIO, but only if VIO can be successfully initialized. This makes the system degradation-aware rather than merely fused. The VIO subsystem itself retains the adaptive sensor selection inherited from Ground-Fusion and also inherits wheel anomaly handling that filters unreliable wheel measurements (&&&^^^^6query6^^^^&&&).

Switching between subsystems requires explicit frame transfer. The framework estimates a rigid transformation between LIO and VIO trajectory frames by robust optimization on $SE(3)$ using a Cauchy robust kernel and Mahalanobis weighting through pose covariance PRESERVED_PLACEHOLDER_^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6query6^^^^. The purpose of this step is to make alignment robust to outlier pose estimates during switching. After alignment, the system applies transition smoothing through an exponential-map correction:

PRESERVED_PLACEHOLDER_^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^

where PRESERVED_PLACEHOLDER_^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6max_results6^^^^ and PRESERVED_PLACEHOLDER_^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6query6^^^^ are the current poses from the active and backup subsystems and PRESERVED_PLACEHOLDER_^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6\6^^^^ controls smoothing strength. This is the framework’s main mechanism for seamless pose continuity across subsystem switches (&&&^^^^6query6^^^^&&&).

The robustness strategy is therefore layered: redundant sensing, LiDAR-priority switching, degradation detection from feature count and ICP residual, adaptive fallback to VIO, robust frame alignment, trajectory smoothing, inherited wheel anomaly handling, and GNSS or wheel constraints for long-term drift control. The paper explicitly states that robustness is not achieved by a single estimator trick (&&&^^^^6query6^^^^&&&).

^^^^6\6^^^^. Operational pipeline, mapping, and implementation context

A practical view of the Ground-Fusion++ pipeline begins with sensor acquisition and synchronization. The paper describes ROS rosbag recording with unified timestamps and internal hardware synchronization for the D^^^^6\6query6 OR \6^^^^i RGB-D-IMU and Livox units. Quality assessment then evaluates LiDAR feature count and ICP residuals. LIO runs as the default pose source. If LiDAR degradation is detected, the framework triggers fallback to the enhanced VIO subsystem using RGB-D, IMU, wheel odometry, and GNSS. Pose frame alignment solves for the inter-subsystem transformation, the fusion stage smooths the active and backup trajectories, and the rendering module consumes RGB, point clouds, and the active odometry output to construct dense colorized maps (&&&^^^^6query6^^^^&&&).

The mapping component is an important distinguishing feature. Ground-Fusion++ uses a rendering module inspired by ImMesh and is explicitly described as supporting real-time dense colorized mapping. The paper emphasizes this as a major improvement over Ground-Fusion in a long-term outdoor test, where Ground-Fusion failed to produce a dense map. In this sense, Ground-Fusion++ is framed არა only as a localization system but also as a mapping-capable framework (&&&^^^^6query6^^^^&&&).

The benchmark context is also unusually rich. M^^^^6query6^^^^DGR includes an RGB-D IMU sensor, an omnidirectional camera, two non-repetitive LiDARs, a wheel odometer, and two GNSS receivers. Ground truth is obtained from Mocap indoors and RTK GNSS outdoors. The benchmark induces four degradation classes: visual challenge, LiDAR degeneracy, wheel slippage, and GNSS denial. Representative sequences include Dynamic^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Varying-illu^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Occlusion^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Dark^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Corridor^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Elevator^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Wheel-float^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Sha-turn^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Grass^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, and GNSS-denial^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^. The primary quantitative metric is ATE RMSE (&&&^^^^6query6^^^^&&&).

^^^^6 OR \6^^^^. Quantitative performance on M^^^^6query6^^^^DGR

The paper evaluates forty SLAM systems on M^^^^6query6^^^^DGR, spanning vision-based methods, LiDAR-based methods, and LiDAR-visual fusion methods, including Ground-Fusion, LIO-SAM, Fast-LIO^^^^6max_results6^^^^, R^^^^6max_results6^^^^LIVE, Fast-LIVO^^^^6max_results6^^^^, LVI-SAM, and Ground-Fusion++ itself. The reported general trends are that vision-only methods perform well only in favorable conditions and degrade sharply under darkness, occlusion, and dynamics; LiDAR-based methods are generally stronger outdoors and on large-scale trajectories but can fail badly in corridors and elevators; and fusion methods are the most robust overall (&&&^^^^6query6^^^^&&&).

Setting	Sequence(s)	Ground-Fusion++ ATE RMSE
Visual challenge	Dynamic^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Varying-illu^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^	around ^^^^6query6^^^^.^^^^6query6 m
Visual challenge	Occlusion^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^	^^^^6query6^^^^.^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6query6^^^^ m
LiDAR degeneracy	Corridor^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^	^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^.^^^^6query6query6^^^^ m
LiDAR degeneracy	Elevator^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^	^^^^6 OR \6^^^^.^^^^6max_results6^^^^8 m
Wheel slippage	Wheel-float^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Sha-turn^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^	around ^^^^6query6^^^^.^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6query6^^^^ m and ^^^^6query6^^^^.^^^^6query6 m
Wheel slippage	Grass^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^	^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^.^^^^6query6query6^^^^ m
GNSS denial	GNSS-denial^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^	^^^^6 OR \6^^^^.^^^^6max_results6query6^^^^ m
Long-term	Longtime^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^	7.^^^^6 OR \6^^^^ m

These results are presented as evidence that Ground-Fusion++ is among the best or near-best performing methods across most scenarios, especially under degraded conditions. The long-term comparison with Ground-Fusion is particularly emphasized: on Longtime^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^, Ground-Fusion++ reports 7.^^^^6 OR \6^^^^ m ATE RMSE, whereas Ground-Fusion reports ^^^^6max_results6max_results6^^^^.^^^^6 OR \6^^^^ m, and only Ground-Fusion++ successfully generates a high-quality dense color mesh in that scenario (&&&^^^^6query6^^^^&&&).

The paper also isolates the effect of degradation detection and switching in corridor sequences. For two corridor tests, the LIO subsystem alone reports 6.^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6\6^^^^ m and ^^^^6max_results66^^^^.66 m. Ground-Fusion++ combined with a prior degeneration detection method reports ^^^^6\6^^^^.7^^^^6max_results6^^^^ m and ^^^^6max_results6^^^^.^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6max_results6^^^^ m. The full Ground-Fusion++ system reports ^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^.67 m and ^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^.7^^^^6query6^^^^ m. The stated conclusion is that the proposed detection and switching strategy provides a substantial gain (&&&^^^^6query6^^^^&&&).

At the same time, the quantitative record is not presented as uniformly solved. Elevator^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^ at ^^^^6 OR \6^^^^.^^^^6max_results6^^^^8 m and GNSS-denial^^^^6query6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^ at ^^^^6 OR \6^^^^.^^^^6max_results6query6^^^^ m are explicitly described as still challenging, which prevents an overly broad reading of the robustness claim (&&&^^^^6query6^^^^&&&).

6. Interpretation, scope, and limitations

Ground-Fusion++ is best understood as a resilient baseline framework for sensor-fusion ground SLAM rather than as a single tightly coupled estimator. The paper’s main takeaways are that modularity matters, adaptive sensor selection is essential, LiDAR significantly improves long-term outdoor robustness when combined with visual, inertial, wheel, and GNSS information, robust switching is as important as sensor fusion, and mapping should be part of robustness evaluation (&&&^^^^6query6^^^^&&&).

Its practical contribution is therefore architectural. The framework combines continuous-time LIO, Ground-Fusion-derived VIO, adaptive LiDAR degradation detection, subsystem fallback, robust frame alignment on PRESERVED_PLACEHOLDER_^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6 OR \6^^^^, smoothing across mode transitions, and dense real-time colorized mapping. This design choice distinguishes it from approaches that either fuse only a limited set of sensors or assume relatively stable sensing conditions. A plausible implication is that the framework’s principal value lies in operational resilience under mixed failure modes rather than in optimizing for a single best-case regime.

The paper also records clear limitations. Extreme geometric degeneracy remains difficult, as shown by the elevator results. GNSS denial remains challenging over long trajectories. Some implementation internals are described only at a high level because Ground-Fusion++ is presented as a framework or baseline built from existing modules. The system is publicly released together with M^^^^6query6^^^^DGR, but the paper notes that the implementation discussion is less granular than the mathematical description of degradation detection, frame alignment, and transition smoothing (&&&^^^^6query6^^^^&&&).

Within the development line from Ground-Fusion to Ground-Fusion++, the shift is from a low-cost tightly coupled GNSS-RGBD-IMU-wheel system focused on initialization and anomaly handling to a LiDAR-augmented, degradation-aware hybrid framework centered on resilient subsystem selection and mapping continuity (&&&^^^^6arXiv (Zhang et al., 11 Jul 2025) Ground-Fusion++ (Yin et al., 2024) Ground-Fusion6^^^^&&&). In that narrower and technically specific sense, the “++” designation refers not to a generic incremental upgrade but to a distinct architectural response to corner-case SLAM.