S-Graphs+: Real-Time Localization and Mapping Leveraging Hierarchical Representations

In this letter, we present an evolved version of Situational Graphs, which jointly models in a single optimizable factor graph (1) a pose graph, as a set of robot keyframes comprising associated measurements and robot poses, and (2) a 3D scene graph, as a high-level representation of the environment that encodes its different geometric elements with semantic attributes and the relational information between them. Specifically, our S-Graphs+ is a novel four-layered factor graph that includes: (1) A keyframes layer with robot pose estimates, (2) a walls layer representing wall surfaces, (3) a rooms layer encompassing sets of wall planes, and (4) a floors layer gathering the rooms within a given floor level. The above graph is optimized in real-time to obtain a robust and accurate estimate of the robot's pose and its map, simultaneously constructing and leveraging high-level information of the environment. To extract this high-level information, we present novel room and floor segmentation algorithms utilizing the mapped wall planes and free-space clusters. We tested S-Graphs+ on multiple datasets, including simulated and real data of indoor environments from varying construction sites, and on a real public dataset of several indoor office areas. On average over our datasets, S-Graphs+ outperforms the accuracy of the second-best method by a margin of 10.67%, while extending the robot situational awareness by a richer scene model. Moreover, we make the software available as a docker file.

Automated summary

In this paper, an evolved version of Situational Graphs called S-Graphs+ is presented, which jointly models a pose graph and a 3D scene graph in a single optimizable factor graph. S-Graphs+ includes four layers: keyframes, walls, rooms, and floors, and is optimized in real-time to achieve accurate robot pose estimation and map construction. Novel room and floor segmentation algorithms utilizing mapped wall planes and free-space clusters are also introduced. Experimental results demonstrate that S-Graphs+ outperforms other methods in terms of accuracy and scene modeling capabilities. The software is made available as a docker file.