Selective Kalman Filter: When and How to Fuse Multi-Sensor Information to Overcome Degeneracy in SLAM

TL;DR

The paper targets robustness and efficiency gaps in SLAM by addressing degeneracy through a Selective Kalman Filter (SKF) that fuses data only when LiDAR SLAM degenerates, and only in the directions where degeneracy occurs. It introduces a covariance-based degeneracy detection that accounts for coupling between rotation and translation, enabling precise identification of degenerative directions, and a directional visual data selection that preserves non-degenerate state accuracy. SKF-Fusion, integrated into the LIO-VIO pipeline on the R$^3$LIVE framework, demonstrates improved end-to-end accuracy and reduced visual processing time in degenerate environments, with competitive performance in non-degenerate scenarios. The work provides open-source code and shows that selective fusion can yield robustness and real-time benefits across multi-sensor SLAM setups.

Abstract

Research trends in SLAM systems are now focusing more on multi-sensor fusion to handle challenging and degenerative environments. However, most existing multi-sensor fusion SLAM methods mainly use all of the data from a range of sensors, a strategy we refer to as the all-in method. This method, while merging the benefits of different sensors, also brings in their weaknesses, lowering the robustness and accuracy and leading to high computational demands. To address this, we propose a new fusion approach -- Selective Kalman Filter -- to carefully choose and fuse information from multiple sensors (using LiDAR and visual observations as examples in this paper). For deciding when to fuse data, we implement degeneracy detection in LiDAR SLAM, incorporating visual measurements only when LiDAR SLAM exhibits degeneracy. Regarding degeneracy detection, we propose an elegant yet straightforward approach to determine the degeneracy of LiDAR SLAM and to identify the specific degenerative direction. This method fully considers the coupled relationship between rotational and translational constraints. In terms of how to fuse data, we use visual measurements only to update the specific degenerative states. As a result, our proposed method improves upon the all-in method by greatly enhancing real-time performance due to less processing visual data, and it introduces fewer errors from visual measurements. Experiments demonstrate that our method for degeneracy detection and fusion, in addressing degeneracy issues, exhibits higher precision and robustness compared to other state-of-the-art methods, and offers enhanced real-time performance relative to the all-in method. The code is openly available.

Figures (10)

• Figure 1: Performance on localization and mapping after applying four different degeneracy detection methods.
• Figure 2: Pipeline of the Selective Kalman Filter in the LIVO system: focusing on "when" and "how" to fuse information and LiDAR SLAM degeneracy detection. Teal blocks represent the processing of LiDAR information, while red blocks indicate the handling of visual information.
• Figure 3: Two-dimensional cross-section diagram illustrating LiDAR $y$-coordinate pose estimation with point clouds: (a) Estimation using only point A, resulting in inaccurate outcomes; (b) Estimation incorporating point A with additional points B, leading to more accurate esitmation.
• Figure 4: Pipeline of SKF-Fusion.
• Figure 5: Localization and mapping of SKF-Fusion in the challenging indoor degenerate_seq_02 dataset with evident degeneracy. White point clouds represent the current frame of LiDAR data, purple ellipsoids depict the translational uncertainty representation of our proposed method, and brown ellipsoids depict the translational uncertainty representation of Hessian-based methods (LION, X-ICP). Larger ellipsoids indicate greater uncertainty and more degenerative, and the principal direction of the ellipsoids represents the primary direction of degeneracy. (a) Overall top-down view; (b) and (c) show the uncertainty when the LiDAR faces a wall, and the current frame's point cloud features only a planar characteristic; (d) When facing a wall and the ground, our method identifies the horizontal direction as the most degenerative direction, whereas Hessian-based methods consider it to be the vertical direction.