RSS: Robust Stereo SLAM With Novel Extraction and Full Exploitation of Plane Features

Planar structures, prevalent in man-made environments, can be observed by a camera for significant periods of time due to their large spatial presence. These structures provide strong planar regularities for Simultaneous Localization and Mapping (SLAM) systems, facilitating long-term navigation. Therefore, we propose a novel point-plane-based stereo SLAM system, fully regularized by plane features within a unified non-linear optimization framework. The core of our method is an accurate and efficient stereo plane extraction algorithm with strict 2D and 3D outlier rejection mechanisms, effectively extracting main planes from robust stereo correspondences and enabling real-time point-plane association. Furthermore, we introduce a novel optimization formulation, incorporating geometric feature (point and plane) and across-feature (point-on-plane) constraints that promote each other through the mutual constraints between associated point and plane features, which fully exploits plane constraints to improve the performance of SLAM system. The proposed plane extraction algorithm is evaluated on the EuRoC MAV dataset, achieving significant improvements in number, accuracy, reliability, and efficiency over the state-of-the-art (SOTA) stereo point-plane-based system (Zhang et al., 2021). The results of an ablation study on two public datasets show that the proposed SLAM system outperforms (Zhang et al., 2021) in both accuracy and robustness, and further demonstrate the mutual enhancement between the two types of constraints.