RaCo-SLAM: A Physics-Informed 4D Radar SLAM with Co-Visibility Consistency Factor

Abstract

Robust all-weather localization is a critical capability for autonomous systems. While 4D mmWave radar offers superior resilience to adverse environmental conditions compared to LiDAR and cameras, its application in high-precision Simultaneous Localization and Mapping (SLAM) is hindered by significant challenges, including severe point cloud sparsity, complex noise characteristics, and the prevalence of dynamic objects. To address these issues, we propose RaCo-SLAM, a robust and real-time 4D mmWave radar SLAM framework with co-visibility consistency. This framework features a novel physics-informed probabilistic model for adaptive feature extraction from sparse and noisy point clouds. For global consistency, we introduce a co-visibility consistency factor (CoVC factor) into the global optimization, moving beyond conventional loop-closure methods. This factor directly minimizes point-to-point registration errors to enforce global consistency and is designed for parallel real-time execution on a standard CPU. Comprehensive evaluation on diverse and challenging real-world datasets demonstrates state-of-the-art accuracy and robustness, achieving real-time performance exceeding 40 Hz on a standard CPU. To benefit the community, the code and collected dataset will be released at https://github.com/sudo-robot0/RaCo-SLAM.