Situational Graphs for Robot Navigation in Structured Indoor Environments

3 min read

S-Graphs puts the scene graph inside the SLAM factor graph, so walls, rooms and corridors are optimised together with the robot poses. Hydra builds its graph on top of an odometry estimate and corrects it afterwards; S-Graphs makes the graph part of the estimate. It has three layers: keyframe poses with odometry factors, planes extracted from 3D LiDAR by sequential RANSAC (minimal azimuth, elevation, distance parametrisation), and a “topological” layer of rooms and corridors. A room is four walls (two opposed x-planes, two opposed y-planes, separated by more than a threshold) and a corridor is two parallel walls; each adds factors that tie its centre and width to its planes. Loop closure is two-stage: room and corridor re-detection acts as a soft loop constraint, backed by NDT scan matching for large drift. The code extends HDL-Graph-SLAM.

Findings

  • Simulated ATE (Gazebo, four environments): 0.04, 0.07, 0.03 and 0.05 m, against HDL-SLAM’s 0.09, 0.11, 0.04 and 0.15 m and much larger LOAM-family errors (MLOAM reaches 2.82 m on SE-2).
  • The ablation without the room/corridor layer degrades sharply in the larger simulated scenes (0.40 m on SE-1, 1.01 m on SE-2), which the authors attribute to drift causing wrong plane matches that the room factors correct.
  • Real data from a Boston Dynamics Spot with a Velodyne VLP-16 on two construction-site floors and an office: point-cloud RMSE against architectural plans of 0.94, 0.26 and 1.32 m, best or second best in each.
  • On the large loop sequences LE-1 and LE-2 (1321 s and 1585 s), start-to-end translation error is 0.08 and 0.11 m. LE-1 has a lot of glass, which causes missed corridor detections.
  • Compute is the cost: mean optimisation time per update is 70 to 331 ms against 3 to 23 ms for HDL-SLAM, with large standard deviations (up to 447 ms) as the graph grows.

Limits

  • Strong Manhattan-world assumption: walls are classed as x, y or horizontal by their dominant normal component, and rooms must be four-walled rectangles. Curved, angled or open-plan spaces fall outside the model (the authors plan non-Manhattan validation).
  • Geometry only. There are no objects or semantic labels beyond plane, room and corridor; the Foundations paper makes this point too.
  • Glass and missing walls break detection, as LE-1 shows.
  • Optimisation time grows with the graph; the authors propose optimising only the neighbourhood of the current room.
  • Single robot, static indoor environments, LiDAR required.

Relevance

S-Graphs is the tightly coupled alternative to Hydra’s loosely coupled design, and vS-Graphs carries it to cameras. Putting room factors in the estimator means a wrong room hypothesis now pulls on the robot pose, which is a question for robust estimation: the paper uses Mahalanobis gating for data association but describes no outlier model for the room factors themselves. For collaborative SLAM, rooms are attractive shared landmarks between robots because they are large and few, but only in buildings that look like the model.