PaperlayerView exact revision on arXiv

Source-linked AI summary

Kimera: from SLAM to Spatial Perception with 3D Dynamic Scene Graphs

Antoni Rosinol, Andrew Violette, Marcus Abate, Nathan Hughes, Yun Chang, Jingnan Shi, Arjun Gupta, Luca Carlone

arXiv:2101.06894v3cs.ROcs.CV

TL;DR

Robots lack unified representations for metric-semantic understanding, hierarchical abstraction, and dynamic relations needed for high-level operation. The paper introduces 3D Dynamic Scene Graphs and Kimera, an automatic visual-inertial spatial perception engine, and evaluates them across real and simulated settings. Kimera supports real-time metric-semantic reconstruction and DSG-based hierarchical semantic path planning, while some scene-graph construction remains offline.

  • Problem

    Robots provide partial, fragmented scene representations and lack suitable support for multi-level decision making and high-level reasoning.

  • Method

    The paper introduces layered 3D Dynamic Scene Graphs and Kimera, which automatically builds DSGs from visual-inertial data using spatial-perception modules.

  • Results

    Kimera achieves competitive visual-inertial SLAM, real-time metric-semantic mesh reconstruction, and DSG-based semantic hierarchical path planning, with hierarchical planning outperforming volumetric planning by several orders of magnitude in timing.

  • Takeaways & Limitations

    DSGs provide an actionable representation for navigation and planning by organizing spatial concepts hierarchically and modeling their spatio-temporal relations.

  • Takeaways & Limitations

    Metric-semantic reconstruction is incremental and real-time, but the remaining scene-graph nodes are built at the end of the run and require a few minutes to parse the scene.

Abstract

from arXiv · show

Humans are able to form a complex mental model of the environment they move in. This mental model captures geometric and semantic aspects of the scene, describes the environment at multiple levels of abstractions (e.g., objects, rooms, buildings), includes static and dynamic entities and their relations (e.g., a person is in a room at a given time). In contrast, current robots' internal representations still provide a partial and fragmented understanding of the environment, either in the form of a sparse or dense set of geometric primitives (e.g., points, lines, planes, voxels) or as a collection of objects. This paper attempts to reduce the gap between robot and human perception by introducing a novel representation, a 3D Dynamic Scene Graph(DSG), that seamlessly captures metric and semantic aspects of a dynamic environment. A DSG is a layered graph where nodes represent spatial concepts at different levels of abstraction, and edges represent spatio-temporal relations among nodes. Our second contribution is Kimera, the first fully automatic method to build a DSG from visual-inertial data. Kimera includes state-of-the-art techniques for visual-inertial SLAM, metric-semantic 3D reconstruction, object localization, human pose and shape estimation, and scene parsing. Our third contribution is a comprehensive evaluation of Kimera in real-life datasets and photo-realistic simulations, including a newly released dataset, uHumans2, which simulates a collection of crowded indoor and outdoor scenes. Our evaluation shows that Kimera achieves state-of-the-art performance in visual-inertial SLAM, estimates an accurate 3D metric-semantic mesh model in real-time, and builds a DSG of a complex indoor environment with tens of objects and humans in minutes. Our final contribution shows how to use a DSG for real-time hierarchical semantic path-planning. The core modules in Kimera are open-source.

1 Introduction

Robots lack unified representations that connect metric-semantic understanding, multiple abstraction levels, and dynamic spatio-temporal relations. The paper introduces 3D Dynamic Scene Graphs and Kimera as an automatic spatial perception system, evaluates them broadly, and demonstrates hierarchical semantic path planning.

  • Motivation: High-level scene understanding supports safe autonomous operation, long-horizon planning, and effective human-robot interaction.Robots should execute instructions involving locations, objects, and extended time horizons while reasoning about inconsistencies, causal relations, and occluded objects.
  • Motivation: Current robots often receive geometric commands and lack representations or inference algorithms for multi-level decision making and high-level reasoning.Existing systems also lack a notion of causality or high-level reasoning.
  • Requirements: High-level 3D scene understanding requires metric-semantic geometry, hierarchical abstractions, and spatio-temporal relations among objects, structures, and humans.These ingredients connect spatial grounding, actionable planning, and reasoning about static and dynamic entities.
  • 3D Dynamic Scene Graphs: A 3D Dynamic Scene Graph is a layered directed graph whose nodes represent spatial concepts and whose edges encode pairwise spatio-temporal relations.Its hierarchy captures objects, rooms, agents, places, and connectivity, supporting navigation and planning while explicitly modeling humans with poses over time and dense meshes.
  • Kimera: Kimera automatically builds a DSG from visual-inertial data while combining real-time metric-semantic reconstruction with scene-graph construction.The system includes Kimera-Core and Kimera-DSG modules, and its spatial perception formulation generalizes SLAM to hierarchical spatial concepts and relations.
  • Evaluation and applications: Kimera is evaluated on real and simulated data, including uHumans2, and supports semantic hierarchical path planning with DSGs.The paper reports competitive visual-inertial SLAM, real-time metric-semantic mesh reconstruction on embedded hardware, and path planning substantially faster than volumetric approaches.

2 3D Dynamic Scene Graphs

A 3D Dynamic Scene Graph is a layered, spatially grounded representation that combines metric-semantic geometry, multiple abstraction levels, and spatio-temporal relations among static and dynamic entities. Its layers support planning by connecting meshes, objects, agents, places, structures, rooms, and buildings.

  • A DSG is a layered directed graph whose nodes represent spatial concepts and whose edges represent pairwise spatio-temporal relations.Nodes include objects, rooms, and agents, while relations can encode facts such as an agent being in a room at a given time.
  • Each DSG node is spatially grounded with coordinates and shape or bounding-box attributes, distinguishing it from a purely symbolic knowledge base.
  • A single-story indoor DSG contains five abstraction layers: metric-semantic mesh; objects and agents; places and structures; rooms; and building.
  • Layer 1: Metric-Semantic Mesh: The metric-semantic mesh models static environmental geometry through vertices carrying position, normal, color, and panoptic semantic labels.Mesh faces are represented by triplets of vertex nodes, which define environmental topology; dynamic-object meshes are stored separately.
  • Layer 2: Objects and Agents: Objects are static non-structural entities, whereas agents are time-varying entities represented with trajectories, shapes, and semantic classes.Objects have poses, bounding boxes, and semantic classes; human and robot agents additionally use time-varying pose graphs and mesh models.
  • Layer 3: Places and Structures: Places model free-space positions and traversability, while structures model separators such as walls, floors, ceilings, and pillars.Places form a topological map for path planning, and structures can connect to rooms and objects through enclosure or attachment relations.
  • Layers 4–5: Rooms and Building: Rooms connect adjacent spaces and contained places, while a building node connects all rooms; the node set is task-dependent and compositional.Additional layers, such as levels, neighborhoods, or cities, can be added to extend the representation.

3 Kimera: Spatial Perception Engine

Kimera transforms stereo or RGB-D and IMU streams into a 3D Dynamic Scene Graph through real-time metric-semantic reconstruction and higher-level scene parsing. Kimera-Core estimates motion and meshes, while Kimera-DSG adds objects, humans, places, structures, rooms, and building layers.

  • Kimera takes streaming stereo or RGB-D camera and IMU data and outputs a 3D DSG.The metric-semantic mesh and agent nodes are built incrementally in real time; objects, places, structures, and rooms are built automatically at run completion.
  • Kimera-Core: Kimera-Core combines visual-inertial estimation, pose-graph and mesh optimization, meshing, and semantic reconstruction.Its modules are Kimera-VIO, Kimera-PGMO or Kimera-RPGO, Kimera-Mesher, and Kimera-Semantics.
  • Kimera-Core: Kimera-Core returns IMU-rate state estimates, globally consistent trajectories, and fast local and global semantically annotated meshes.The architecture is parallelized across five threads to accommodate IMU, frame, and keyframe rates.
  • Kimera-DSG: Kimera-DSG builds higher DSG layers from Kimera-Core’s globally consistent metric-semantic mesh, which serves as Layer 1.Kimera-DSG includes Kimera-Objects, Kimera-Humans, and Kimera-BuildingParser.
  • Kimera-DSG: Kimera-Objects estimates bounding boxes for unknown-shape objects or fits CAD models for known-shape objects, while Kimera-Humans reconstructs human meshes and trajectories.Human meshes use SMPL, and known-object CAD fitting uses TEASER++.
  • Kimera-DSG: Kimera-BuildingParser identifies structures, extracts a topological place graph, segments rooms, and generates the higher building layers.
  • Kimera-Mesher: Kimera-Mesher produces per-frame and multi-frame 3D meshes, with the latter maintaining an up-to-date mesh over the VIO fixed-lag horizon.Planar-surface detections add regularity factors to the VIO back-end, coupling state estimation with mesh regularization.

3.3 Kimera-Semantics: 3D Metric-Semantic Reconstruction

Kimera-Semantics reconstructs and semantically annotates a global 3D mesh, while Kimera-PGMO corrects trajectory and mesh drift through joint pose-graph and deformation optimization.

  • Global mesh: Kimera-Semantics builds a global mesh by integrating keyframe depth into a voxel-based TSDF and extracting the surface with marching cubes.Depth comes from dense stereo or RGB-D, followed by bundled raycasting at each keyframe.
  • Semantic annotation: 2D pixel-wise semantic labels are fused into the global mesh to produce semantic annotations.Kimera-Semantics uses segmentation outputs from each keyframe and 3D Bayesian updates.
  • Loop closures: PCM-based outlier rejection selects consistent loop closures and adds an odometry-consistency check for online single-robot operation.This addresses incorrect closures caused by perceptual aliasing before pose or mesh optimization.
  • Loop closures: Kimera-PGMO simultaneously optimizes robot poses and mesh deformations after loop closures, whereas Kimera-RPGO optimizes only the pose graph.The user selects the solver based on computational considerations and the need for a consistent map.
  • Deformation optimization: The unified deformation graph connects mesh vertices, pose vertices, mesh edges, pose-graph edges, and visibility-based pose-to-mesh edges.Its objective preserves odometry and loop closures while enforcing local rigidity between connected mesh vertices and between poses and visible mesh vertices.

3.5 Kimera-Humans: Human Shape Estimation and Robust Tracking

Kimera-Humans represents people as dense, time-varying meshes and maintains robust human trajectories using pose graphs, consistency checks, and error mitigation.

  • Human nodes: Kimera-Humans tracks dense time-varying human meshes rather than representing dynamic targets only as points or 3D poses.Human nodes are based on estimated human shape and pose over time.
  • Dynamic masking: Human detections are dynamically masked during ray casting so moving people are not reconstructed in the static 3D mesh.Only free-space information is used for pixels labeled as humans.
  • Shape estimation: GraphCMR regresses an SMPL mesh and camera parameters from a cropped image, then Kimera-VIO transforms the mesh into the global frame.The crop is obtained from pixel-wise human segmentation and a bounding box around each detection.
  • Human tracking: A pose graph for each human stores pelvis locations over time and connects consecutive poses with a permissive zero-velocity prior.GraphCMR locations enter as prior factors, enabling trajectory optimization.
  • Data association: Nearest-neighbor spatial association links detections to existing human graphs under the assumption that within-timestep human motion is smaller than inter-human separation.If consistency checks fail for every graph, Kimera initializes a new pose graph.
  • Data association: Centroid speed, joint displacement, and SMPL shape parameters are checked to reject implausible associations between successive detections.The method uses a conservative 3 m/s centroid-speed bound and a 3 m joint-displacement bound.
  • Error monitoring: Occlusion-sensitive or small detections are marked incorrect, and pose-graph optimization with stationary-motion priors mitigates remaining node errors.Detections near image boundaries or at most 30 pixels in size are flagged as incorrect.

3.6 Kimera-Objects: Object Pose Estimation

Kimera-Objects extracts object instances from the optimized metric-semantic mesh and estimates either bounding boxes or full poses depending on CAD-model availability.

  • Objects with Unknown Shape: For objects without known shapes, Kimera-Objects separates instances by Euclidean clustering within each semantic object class.It then estimates each instance’s centroid and axis-aligned bounding box.
  • Objects with Known Shape: For objects with known shapes, Kimera-Objects aligns CAD and reconstructed meshes using keypoint matching and robust TEASER++ registration.The resulting 3D object pose also yields an axis-aligned bounding box.

3.7 Kimera-BuildingParser: Extracting Places, Rooms, and Structures

Kimera-BuildingParser extracts places, rooms, and structural elements from Kimera’s 3D mesh using topological graphs and horizontal ESDF sections.

  • Kimera-BuildingParser parses places, structures, and rooms from Kimera’s 3D metric-semantic mesh.
  • Places: The parser derives a topological graph from the ESDF, with nodes sampling free space and edges representing straight-line traversability.Objects and agent poses are associated with their nearest place to model proximity relations.
  • Structures: Structural elements are obtained from semantic wall, floor, and ceiling labels, then represented using centroids, canonical orientations, and axis-aligned bounding boxes.Walls are further segmented according to the rooms they belong to using oriented mesh normals.
  • Rooms: An ESDF section is a horizontal 2D slice cut 0.3m below the detected ceiling, where room interiors remain near 0.3m and wall distances approach 0m.Gravity from Kimera-VIO enables the horizontal cut, while the 3D ESDF makes the room partitioning approach practical.
  • Rooms: Truncating the ESDF section removes small openings, producing disconnected 2D ESDF rooms that define room regions for place labeling.Figure 11 contrasts the color-coded 3D ESDF slice with the truncated slice revealing room contours and overlaying estimated layout connectivity.
  • Rooms: Unlabeled places receive iterative neighborhood majority votes, after which place-room edges and room-room edges are added from the place topology.

3.8 Debugging Tools

Kimera provides open-source tools for debugging, visualization, benchmarking, continuous integration, and automated evaluation of its perception modules.

  • Kimera’s open-source evaluation suite supports debugging, visualization, and benchmarking for VIO, SLAM, and metric-semantic reconstruction.
  • A Jenkins Continuous Integration server checks compilation and unit tests and automatically evaluates Kimera-VIO, Kimera-RPGO, and Kimera-PGMO on EuRoC datasets using evo.
  • Jupyter Notebooks visualize intermediate VIO statistics and support inspection of the system’s internal outputs.

4 Experimental Evaluation

Kimera is evaluated across simulated and real datasets for localization, geometric and semantic reconstruction, dynamic-scene robustness, human tracking, and place parsing. Results show competitive or robust pose estimation, accurate meshes, effective dynamic masking, and strong room-detection performance, while complex or incomplete scenes remain challenging.

  • Localization: PCM makes Kimera-RPGO fairly insensitive to the loop-closure threshold α by rejecting outlier loop closures.Without PCM, smaller α values produce more loop-closure detections but also more outliers.
  • Localization: Kimera-VIO performs on-par with state of the art on static EuRoC scenes, while the proposed approach remains robust as the number of dynamic humans increases.The evaluation also reports that 2-point RANSAC boosts performance on EuRoC.
  • Geometric Reconstruction: 0.35–0.48m average error: Kimera-Semantics produces accurate global meshes, while Kimera-Mesher is up to 24% noisier but requires two orders of magnitude less computation time.The global-mesh accuracy is reported across datasets.
  • Semantic Reconstruction: Dynamic masking avoids human contrails and artifacts in the reconstructed mesh by using only free-space information for human-labeled pixels.The method is evaluated with and without dynamic masking using mesh RMSE.
  • Semantic Reconstruction: <8cm geometric error and >94% semantic accuracy: bundled raycasting causes only a small performance drop, while dense stereo is the largest source of degradation.Dense stereo struggles with depth in textureless regions such as walls, increasing geometric and semantic errors.
  • Mesh Optimization: Kimera-PGMO improves geometric mesh accuracy over the unoptimized mesh and provides the largest localization gains on large-scale scenes.In EuRoC, where localization is already very accurate, its localization gains may be marginal; on uHumans and uHumans2, mesh deformation gives the best geometric and semantic performance.
  • Human and Object Parsing: Human localization improves through filtering, mesh-feasibility checks, beta-parameter checks, and pose-graph data association, although occlusions remain a major source of error.Graph-CNN detections are particularly error-prone when humans are occluded.
  • Place and Room Parsing: 99% precision and 99% recall are achieved for Office place parsing, compared with 87% precision and 92% recall for Subway.Incorrect place classifications typically occur near doors, where room misclassification is described as inconsequential.

5 Motivating Examples

The paper uses DSGs to connect geometric scene understanding with actionable queries. Their hierarchy supports collision-aware planning, time-aware human–robot interaction, compact long-term representations, and short-term prediction of scene dynamics.

  • Obstacle Avoidance and Planning: Bounding boxes across DSG layers form a Bounding Volume Hierarchy that supports collision checking for obstacle avoidance and planning.Higher-layer boxes contain the boxes of lower-layer entities, such as objects within rooms.
  • Human-Robot Interaction: DSGs support time-aware queries about people and visualize human trajectories and dense poses for human–robot interaction.Example queries include asking where a person was at a given time.
  • Long-term Autonomy: Higher DSG layers provide compact abstractions that let a robot forget or retain environmental information for long-term autonomy.Known object instances can share one CAD model through cross-referenced graph nodes, enabling further compression.
  • Prediction: A dense metric-semantic mesh combined with agent descriptions supports short-term scene-dynamics prediction and simulation of possible high-level human actions.The mesh can be provided to a physics simulator to roll out potential actions.

6 Applications

The paper applies DSGs to hierarchical semantic path planning, showing that planning across buildings, rooms, and places can outperform volumetric planning while supporting natural-language queries.

  • Hierarchical semantic path planning: DSGs support hierarchical path planning by successively refining A∗ searches from buildings to rooms, places, and finally smooth collision-free trajectories.This decomposes planning across multiple abstraction levels before extracting a feasible geometric path.
  • Path-planning performance: Volumetric ESDF path queries take minutes, whereas higher-level hierarchical queries finish in milliseconds despite small room over-segmentation.The reported runtime remains on the order of milliseconds when rooms are over-segmented into two or three rooms.
  • Path-planning performance: Several orders of magnitude faster: hierarchical planning runs at interactive speeds for large-scale scenes compared with planning directly on volumetric ESDFs.The comparison uses replicated office scenes and reports trajectories that remain nearly as short as ESDF trajectories despite being longer.
  • Semantic path queries: Planning queries on DSGs can use semantic descriptions such as objects, rooms, and buildings instead of metric coordinates.The module answers queries such as getting near any object x in room y of building z.

7 Related Work

Related work spans hierarchical and scene-graph representations, dynamic-environment SLAM, metric-semantic reconstruction, loop-closure correction, scene parsing, and human pose estimation. The paper positions its approach as combining capabilities that prior systems typically address separately.

  • World representations: Prior scene graphs often model 2D image content or objects, while hierarchical 3D representations address multiple abstraction levels in robotics.The paper contrasts object-only scene graphs with earlier hierarchical mapping approaches.
  • Dynamic-environment SLAM: Existing dynamic-environment research includes SLAMMOT, DATMO, joint visual-inertial odometry with target pose estimation, and camera-object tracking.These approaches focus on robustness to or joint modeling of dynamic elements.
  • Perception algorithms: The paper claims to be the first work combining visual-inertial SLAM, dense object modeling, known-object pose estimation, and dense human SMPL mesh tracking.This combination spans perception tasks that related work commonly treats separately.
  • Metric-semantic reconstruction: Metric-semantic reconstruction research includes real-time mapping, object-based approaches, and lidar-based approaches, while many representations remain non-hierarchical.The cited line of work ranges from early offline processing to recent real-time systems.
  • Loop closure with dense representations: Dense loop-closure methods correct point clouds or meshes through pose-graph updates and deformation procedures.Examples include Kintinuous, MIS-SLAM, and ElasticFusion, which deform dense representations using optimized pose information.
  • Metric-to-topological scene parsing: Metric-to-topological scene parsing partitions metric maps into semantically meaningful places such as rooms and hallways, with differing geometric assumptions and map models.The paper distinguishes its mesh-based approach from methods relying on Manhattan-world assumptions or point clouds.

8 Conclusion

The paper introduces DSGs and Kimera as a fully automatic spatial-perception system, evaluates them on real and simulated data, and demonstrates semantic and hierarchical path planning. It identifies incremental real-time construction and richer attributes as future directions.

  • Contributions: The paper introduces 3D Dynamic Scene Graphs as a unified representation for actionable spatial perception.
  • Contributions: Kimera builds a DSG from visual-inertial data fully automatically and supports semantic and hierarchical path planning applications.
  • Evaluation: The evaluation covers photo-realistic simulations and real data, including applications enabled by DSG representations.
  • Limitations and future work: Currently, metric-semantic reconstruction runs in real time, but the remaining scene graph is built after the run and requires a few minutes to parse the entire scene.The authors identify incremental real-time operation as a research avenue.
  • Limitations and future work: Future DSG extensions include heterogeneous multi-robot sensing and learned physical attributes such as material type and object affordances.

Disclaimer

The document states that its distribution is approved for public release and unlimited distribution. It also identifies the sponsoring contract and separates the authors’ views from those of the sponsoring office.

  • Distribution: Approved for public release: distribution is unlimited.
  • Funding: The work is supported by the Under Secretary of Defense for Research and Engineering under Air Force Contract No. FA8702-15-D-0001.
  • Disclaimer: The authors state that the expressed opinions, findings, conclusions, and recommendations do not necessarily reflect the sponsoring office’s views.
Loading 2101.06894v3…