itemizestditemize

# Foundations of Spatial Perception for Robotics:
Hierarchical Representations and Real-time Systems

Nathan Hughes, Yun Chang, Siyi Hu, Rajat Talak, Rumaisa Abdulhai, Jared
Strader, Luca Carlone The authors are with the Laboratory for Information &
Decision Systems (LIDS), Massachusetts Institute of Technology, Cambridge,
USA. Email: {na26933, yunchang, siyi, talak, rumaisa, jstrader,
<EMAIL_ADDRESS>work was partially funded by the AIA CRA
FA8750-19-2-1000, ARL DCIST CRA W911NF-17-2-0181, ONR RAIDER N00014-18-1-2828,
MIT Lincoln Laboratory’s Autonomy al Fresco program, Lockheed Martin
Corporation’s Neural Prediction in 3D Dynamic Scene Graphs program, and by
Carlone’s Amazon Research Award.

###### Abstract

3D spatial perception is the problem of building and maintaining an actionable
and persistent representation of the environment in real-time using sensor
data and prior knowledge. Despite the fast-paced progress in robot perception,
most existing methods either build purely geometric maps (as in traditional
SLAM) or “flat” metric-semantic maps that do not scale to large environments
or large dictionaries of semantic labels. The first part of this paper is
concerned with representations: we show that scalable representations for
spatial perception need to be _hierarchical_ in nature. Hierarchical
representations are efficient to store, and lead to layered graphs with small
_treewidth_ , which enable provably efficient inference. We then introduce an
example of hierarchical representation for indoor environments, namely a _3D
scene graph_ , and discuss its structure and properties. The second part of
the paper focuses on algorithms to incrementally construct a 3D scene graph as
the robot explores the environment. Our algorithms combine 3D geometry
(_e.g.,_ to cluster the free space into a graph of places), topology (to
cluster the places into rooms), and geometric deep learning (_e.g.,_ to
classify the type of rooms the robot is moving across). The third part of the
paper focuses on algorithms to maintain and correct 3D scene graphs during
long-term operation. We propose hierarchical descriptors for loop closure
detection and describe how to correct a scene graph in response to loop
closures, by solving a _3D scene graph optimization problem_. We conclude the
paper by combining the proposed perception algorithms into _Hydra_ , a real-
time spatial perception system that builds a 3D scene graph from visual-
inertial data in real-time. We showcase Hydra’s performance in photo-realistic
simulations and real data collected by a Clearpath Jackal robots and a Unitree
A1 robot. We release an open-source implementation of Hydra at
https://github.com/MIT-SPARK/Hydra.

##  I Introduction

The next generation of robots and autonomous systems will need to build
actionable, metric-semantic, multi-resolution, persistent representations of
large-scale unknown environments in real-time. _Actionable_ representations
are required for a robot to understand and execute complex humans instructions
(_e.g.,_ “bring me the cup of tea I left on the dining room table”). These
representations include both _geometric and semantic_ aspects of the
environment (_e.g.,_ to plan a path to the dining room, and understand where
the table is); moreover, they should allow reasoning over relations between
objects (_e.g.,_ to understand what it means for the cup of tea to be _on_ the
table). These representations need to be _multi-resolution_ , in that they
might need to capture information at multiple levels of abstractions (_e.g.,_
from objects to rooms, buildings, and cities) to interpret human commands and
enable fast planning (_e.g.,_ by allowing planning over compact abstractions
rather than dense low-level geometry). Such representations must be built in
_real-time_ to support just-in-time decision-making. Finally, these
representations must be _persistent_ to support long-term autonomy: (i) they
need to scale to large environments, (ii) they should allow fast inference and
corrections as new evidence is collected by the robot, and (iii) their size
should only grow with the size of the environment they model.

Figure 1: We introduce _Hydra_ , a highly parallelized system that builds 3D
scene graphs from sensor data in real-time, by combining geometric reasoning
(e.g., to build a 3D mesh and cluster the free space into a graph of places),
with topology (to cluster the places into rooms), and geometric deep learning
(e.g., to classify the type of rooms the robot is moving across). The figure
shows sample input data and the 3D scene graph created by Hydra in a large-
scale real environment.

3D spatial perception (or Spatial AI [1]) is the problem of building
actionable and persistent representations from sensor data and prior knowledge
in real-time. This problem is the natural evolution of Simultaneous
Localization and Mapping (SLAM), which also focuses on building persistent map
representations in real-time, but is typically limited to geometric
understanding. In other words, if the task assigned to the robot is purely
geometric (_e.g.,_ “go to position [X, Y, Z]”), then spatial perception
reduces to SLAM and 3D reconstruction, but as the task specifications become
more advanced (_e.g.,_ including semantics, relations, and affordances), we
obtain a much richer problem space and SLAM becomes only a component of a more
elaborate spatial perception system.

The pioneering work [1] has introduced the notion of spatial AI. Indeed the
requirements that spatial AI has to build actionable and persistent
representations can already be found in [1]. In this paper we take a step
further by arguing that such representations must be _hierarchical_ in nature,
since a suitable hierarchical organization reduces storage during long-term
operation and leads to provably efficient inference. Moreover, going beyond
the vision in [1], we discuss how to combine different tools (metric-semantic
SLAM, 3D geometry, topology, and geometric deep learning) to implement a real-
time spatial perception system for indoor environments.

Hierarchical Representations. 3D Scene Graphs [2, 3, 4, 5, 6, 7] have recently
emerged as expressive hierarchical representations of 3D environments. A 3D
scene graph (_e.g.,_ Fig. 1) is a layered graph where nodes represent spatial
concepts at multiple levels of abstraction (from low-level geometry to
objects, places, rooms, buildings, etc.) and edges represent relations between
concepts. Armeni et al. [3] pioneered the use of 3D scene graphs in computer
vision and proposed the first algorithms to parse a metric-semantic 3D mesh
into a 3D scene graph. Kim et al. [5] reconstruct a 3D scene graph of objects
and their relations. Rosinol et al. [2, 4] propose a novel 3D scene graph
model that (i) is built directly from sensor data, (ii) includes a sub-graph
of places (useful for robot navigation), (iii) models objects, rooms, and
buildings, and (iv) captures moving entities in the environment. Recent work
[6, 7, 8, 9] infers objects and relations from point clouds, RGB-D sequences,
or object detections. In this paper, we formalize the intuition behind these
works that suitable representations for robot perception have to be
hierarchical in nature, and discuss guiding principles behind the choice of
“symbols” we have to include in these representations.

Real-time Systems. While 3D scene graphs can serve as an advanced “mental
model” for robots, methods to build such a rich representation in real-time
remain largely unexplored. The works [5, 6, 7] allow real-time operation but
are restricted to “flat” 3D scene graphs that only include objects and their
relations while disregarding the top layers in Fig. 1. The works [2, 3, 4],
which focus on building truly hierarchical representations, run offline and
require several minutes to build a 3D scene graph ([3] even assumes the
availability of a complete metric-semantic mesh of the environment built
beforehand). Extending our prior works [2, 4] to operate in real-time is non-
trivial. These works utilize an Euclidean Signed Distance Function (ESDF) of
the entire environment to build the scene graph. Unfortunately, ESDFs memory
requirements scale poorly in the size of the environment; see [10] and Section
II. Moreover, the extraction of places and rooms in [2, 4] involves batch
algorithms that process the entire ESDF, whose computational cost grows over
time and is incompatible with real-time operation. Finally, the ESDF is
reconstructed from the robot trajectory estimate which keeps changing in
response to loop closures. The approaches [2, 4] would therefore need to
rebuild the scene graph from scratch after every loop closure, clashing with
real-time operation.

The present paper extends our prior work [11] and proposes the first real-time
system to build hierarchical 3D scene graphs of large-scale environments.
Following [11], recent works has explored constructing _situational graphs_
[12, 13], a hierarchical representation for scene geometry with layers
describing free-space traversability, walls, rooms, and floors. While related
to this research line, the works [12, 13] focus on LIDAR-based systems, which
mostly reason over geometric features (_e.g.,_ walls, rooms, floors), but lack
the rich semantics we consider in this paper (_e.g.,_ objects and room
labels). We postpone a more extensive literature review to Section VII.

Contribution 1: Foundations of Hierarchical Representations (Section II). We
start by observing that flat metric-semantic representations scale poorly in
the size of the environment and the size of the vocabulary of semantic labels
the robot has to incorporate in the map. For instance, a voxel-based metric-
semantic map picturing the floor of an office building with ${40}$ semantic
labels per voxel (as the one underlying the approaches of [2] and [14])
already requires roughly $450\text{\,}\mathrm{MiB}$ to be stored. Envisioning
future robots to operate on much large scales (_e.g.,_ an entire city) and
using a much larger vocabulary (_e.g.,_ the English dictionary includes
roughly 500,000 words), we argue that research should move beyond flat
representations. We show that hierarchical representations allow to largely
reduce the memory requirements, by enabling loss-less compression of semantic
information into a layered graph, as well as lossy compression of the
geometric information into meshes and graph-structured representations of the
free space. Additionally, we show that hierarchical representations are
amenable for efficient inference. In particular, we prove that the layered
graphs appearing in hierarchical map representations have small _treewidth_ ,
a property that enables efficient inference; for instance, we conclude that
the treewidth of the scene graph modeling an indoor environment does not scale
with the number of nodes in the graph (_i.e.,_ roughly speaking, the number of
nodes is related to the size of the environment), but rather with the maximum
number of objects in each room. While most of the results above are general
and apply to a broad class of hierarchical representations, we conclude this
part by introducing a specific hierarchical representation for indoor
environments, namely _3D scene graphs_.

Contribution 2: Real-time Incremental 3D Scene Graph Construction (Section
III). After establishing the importance of hierarchical representations, we
move to developing a suite of algorithms to estimate 3D scene graphs from
sensor data. In particular, we develop real-time algorithms that can
incrementally estimate a 3D scene graph of an unknown building from visual-
inertial sensor data. We use existing methods for geometric processing to
incrementally build a metric-semantic mesh of the environment and reconstruct
a sparse graph of “places”; intuitively, the mesh describes the occupied space
(including objects in the environment), while the graph of places provides a
succinct description of the free space. Then we propose novel algorithms to
efficiently cluster the places into rooms; here we use tools from topology,
and in particular the notion of _persistent homology_ [15]. Finally, we use
novel architectures for geometric deep learning, namely _neural trees_ [16],
to infer the semantic labels of each room (_e.g.,_ bedroom, kitchen, etc.)
from the labels of the object within. Towards this goal, we show that our 3D
scene graph representation allows to quickly and incrementally compute a tree-
decomposition of the 3D scene graph, which —together with our bound on the
scene graph treewidth— ensures that the neural tree runs in real-time on an
embedded GPU.

Contribution 3: Persistent Representations via Hierarchical Loop Closure
Detection and 3D Scene Graph Optimization (Section IV). Building a persistent
map representation requires the robot to recognize it is revisiting a location
it has seen before, and correcting the map accordingly. We propose a novel
hierarchical approach for loop closure detection: the approach involves (i) a
_top-down loop closure detection_ that uses hierarchical descriptors
—capturing statistics across layers in the scene graph— to find putative loop
closures, and (ii) a _bottom-up geometric verification_ that attempts
estimating the loop closure pose by registering putative matches. Then, we
propose the first algorithm to optimize a 3D scene graph in response to loop
closures; our approach relies on _embedded deformation graphs_ [17] to
simultaneously and consistently correct all the layers of the scene graph,
including the 3D mesh, objects, places, and the robot trajectory.

Contribution 4: Hydra, a Real-Time Spatial Perception System (Section V). We
conclude the paper by integrating the proposed algorithms into a highly
parallelized perception system, named _Hydra_ , that combines fast early and
mid-level perception processes (_e.g.,_ local mapping) with slower high-level
perception (_e.g.,_ global optimization of the scene graph). We demonstrate
Hydra in challenging simulated and real datasets, across a variety of
environments, including an apartment complex, an office building, and two
student residences. Our experiments (Section VI) show that (i) we can
reconstruct 3D scene graphs of large, real environments in real-time, (ii) our
online algorithms achieve an accuracy comparable to batch offline methods and
build a richer representation compared to competing approaches [7], and (iii)
our loop closure detection approach outperforms standard approaches based on
bag-of-words and visual-feature matching in terms of quality and quantity of
detected loop closures. The source code of Hydra is publicly available at
https://github.com/MIT-SPARK/Hydra.

Novelty with respect to [11, 16]. This paper builds on our previous conference
papers [11, 16] but includes several novel findings. First, rather than
postulating a 3D scene graph structure as done in [11], we formally show that
hierarchical representations are crucial to achieve scalable scene
understanding and fast inference. Second, we propose a novel room segmentation
method based on the notion of persistent homology, as a replacement for the
heuristic method in [11]. Third, we develop novel learning-based hierarchical
descriptors for place recognition, that further improve performance with
respect to the handcrafted hierarchical descriptors in [11]. Fourth, the real-
time system described in this paper is able to also assign room labels,
leveraging the neural tree architecture from [16]; while [16] uses the neural
tree over small graphs corresponding to a single room, in this paper we
provide an efficient way to obtain a tree-decomposition of the top layers of
the scene graph, hence extending [16] to simultaneously operate over _all_
rooms and account for their relations. Finally, this paper includes further
experiments and evaluations on real robots (Clearpath Jackal robots and
Unitree A1 robots) and comparisons with recent scene graph construction
baselines [7].

(a)  (b)  (c)

Figure 2: (a) Example of flat metric-semantic representation. We show labels
only for a subset of the cells in the grid map for the sake of readability.
(b) Example of hierarchical metric-semantic representation with a grid-map as
sub-symbolic layer. (c) A hierarchical metric-semantic representation with
compressed sub-symbolic layer. The cells representing each object are
compressed into bounding polygons, and the free-space cells are compressed
into a sparse graph where each node and edge are also assigned a radius that
defines a circle of free-space around it.

##  II Symbol Grounding and the Need for Hierarchical Representations

The goal of this section is twofold. First, we remark that in order to support
the execution of complex instructions, map representations must be metric-
semantic, and hence ground symbolic knowledge (_i.e.,_ semantic aspects in the
scene) into the map geometry (Section II-A). Second, we observe that “flat”
representations, which naively store semantic attributes for each geometric
entity in the map (_e.g.,_ attach semantic labels to each voxel in an ESDF)
scale poorly in the size of the environment; on the other hand, hierarchical
representations scale better to large environments (Section II-B) and enable
efficient inference (Section II-C). We conclude the section by introducing the
notion of _3D scene graph_ , an example of hierarchical representation for
indoor environments, and discussing its structure and properties (Section
II-D).

###  II-A Symbols and Symbol Grounding

High-level instructions issued by humans (_e.g.,_ “go and pick up the chair”)
involve symbols. A _symbol_ is the representation of a concept that has a
specific meaning for a human (_e.g.,_ the word “chair” is a symbol in the
sense that, as humans, we understand the nature of a chair). For a symbol to
be useful to guide robot action, it has to be _grounded_ into the physical
world. For instance, for the robot to correctly execute the instruction “go
and pick up the chair”, the robot needs to ground the symbol “chair” to the
physical location the chair is situated in.111In general, this is referred to
as the _symbol grounding_ problem [18] in artificial intelligence. In
principle, we could design the perception system of our robot to ground
symbols directly in the sensor data. For instance, a robot with a camera could
map image pixels to appropriate symbols (_e.g.,_ “chair”, “furniture”,
“kitchen”, “apartment”), as commonly done in 2D semantic segmentation [19].
However, grounding symbols directly into sensor data is not scalable: sensor
data (_e.g.,_ images) is collected at high rates and is relatively expensive
to store. This is neither convenient nor efficient when grounding symbols for
long-term operation. Instead, we need intermediate (or _sub-symbolic_)
representations that compress the raw sensor data into a more compact format,
and that can be used to ground symbols. Traditional geometric map models used
in robotics (_e.g.,_ 2D occupancy grids or 3D voxel-based maps) can be
understood as sub-symbolic representations: each cell in a voxel grid does not
represent a semantic concept, and instead is used to ground two symbols:
“obstacle” and “free-space”. Therefore, this paper is concerned with building
_metric-semantic_ representations, which ground symbolic knowledge into
geometric (_i.e.,_ sub-symbolic) representations.

Which Symbols Should a Map Contain? In this paper, we directly specify the set
of symbols the robot is interested in (_i.e.,_ certain classes of objects and
rooms, to support navigation in indoor environments). However, it is worth
discussing how to choose the symbols more generally. The choice of symbols
clearly depends on the task the robot has to execute. A mobile robot may
implement a path planning query by just using the “free-space” symbol and the
corresponding grounding, while a more complex domestic robot may instead need
additional symbols (_e.g.,_ “cup of coffee”, “table”, and “kitchen”) and their
groundings to execute instructions such as “bring me the cup of coffee on the
table in the kitchen”. A potential way to choose the symbols to include in the
map therefore consists of inspecting the set of instructions the robot has to
execute, and then extracting the list of symbols (_e.g.,_ objects and
relations of interest) these instructions involve. For instance, in this
paper, we mostly focus on indoor environments and assume the robot is tasked
with navigating to certain objects and rooms in a building. Therefore, the
symbols we include in our maps include free-space, objects, rooms, and
buildings. After establishing the list of relevant symbols, the goal is for
the robot to build a compact sub-symbolic representation (_i.e.,_ a geometric
map), and then ground these symbols into the sub-symbolic representation. A
pictorial representation of this idea is given in Fig. 2a: the robot builds an
occupancy map (sub-symbolic representation) and attaches a number of semantic
labels (symbols) to each cell. We refer to this representation as “flat” since
each cell is assigned a list of symbols of interest. As we will see shortly,
there are more clever ways to store the same information.

###  II-B Hierarchical Representations Enable Large-Scale Mapping

While the flat representation of Fig. 2a may contain all the information the
robot needs to complete its task, it is unnecessarily expensive to store. Here
we show that the symbolic knowledge underlying such representation naturally
lends itself to be efficiently stored using a hierarchical representation.

Consider the case where we use a flat representation (as shown in Fig. 2a) to
represent a 3D scene and assume our dictionary contains $L$ symbols of
interest. Let $\delta$ be the cell (or voxel) size and call $V$ the volume of
the scene the representation describes. Then, the corresponding metric-
semantic representation would require a memory of:

$m=\mathcal{O}\left(L\cdot V/\delta^{3}\right)$ (1)

to store $L$ labels for each voxel. Note that $m$ grows with the number of
symbols $L$, multiplied by the size of the sub-symbolic representation
$V/\delta^{3}$ (_i.e.,_ the number of voxels in the volume). When mapping
large environments the term $V/\delta^{3}$ quickly becomes unsustainably
large; for instance, covering a $10$km $\times$ $10$km area with a $10$cm grid
resolution, even disregarding the vertical dimension, would require $10^{10}$
voxels. In addition, it would be desirable for the next generation of robots
to execute a large set of tasks, hence requiring them to ground a large
dictionary of symbols $L$.222For reference, the English dictionary includes
around $500,000$ words. The size of the sub-symbolic representation, the
number of symbols, and their multiplicative relation in (1) make a flat
metric-semantic representation unsuitable for large-scale spatial perception.

A key observation here is that multiple voxels encode the same grounded
symbols (_e.g.,_ a chair). In addition, many symbols naturally admit a
hierarchical organization where higher-level concepts (_i.e.,_ buildings or
rooms for indoor environments) contain lower-level symbols (_e.g.,_ objects).
This suggests that we can more efficiently store information _hierarchically_
, where all voxels associated with a certain object (_e.g.,_ all voxels
belonging to a chair) are mapped to the same symbolic node (_e.g.,_ an object
node with the semantic label “chair”), object nodes are associated with the
room they belong to, room nodes are associated to the apartment unit they
belong to, and so on. This transforms the flat representation of Fig. 2a into
the hierarchical model of Fig. 2b, where only the lowest level symbols are
directly grounded into voxels, while the top layers are arranged
hierarchically. This hierarchical representation is more parsimonious and
reduces the required memory to

$m=\mathcal{O}\left(V/\delta^{3}+N_{\text{objects}}+N_{\text{rooms}}+\cdots+N_{\text{buildings}}\right),$
(2)

where $N_{\text{layer}}$ (with
$\text{layer}\in\\{\text{objects},\text{rooms},\ldots\\}$) is the number of
symbols at the specific layer of the hierarchy. This representation is more
memory efficient than (1) because it decouples the number of symbols from the
size $V/\delta^{3}$ of the sub-symbolic representation. For instance, the
scene in Fig. 2 has 336 voxels, and we assume $L=5$.333The symbols are stored
as building type, apartment type, room type, object type, and free-
space/obstacle. Then, the flat representation of Fig. 2a involves storing
$1680$ symbols, while Fig. 2b only requires storing $355$ symbols and $354$
edges describing their hierarchical relations. Crucially, the compression we
performed when moving from Fig. 2a to Fig. 2b is _lossless_ : the two
representations contain exactly the same amount of information.

While we “compressed” the symbolic representation using a hierarchical data
structure, the term $V/\delta^{3}$ in (2) that corresponds to the sub-symbolic
layer is still impractically large for many applications. Therefore, our
robots will also typically need some compression mechanism for the sub-
symbolic layer that provides a more succinct description of the occupied and
free space as compared to voxel-based maps. Fortunately, the mapping
literature already offers alternatives to standard 3D voxel-based maps such as
OctTree [20] or neural implicit representations [21]. In general, this
compression reduces the memory requirement to

$m=\mathcal{O}\left(N_{\text{sub-
sym}}+N_{\text{objects}}+N_{\text{rooms}}+\cdots+N_{\text{buildings}}\right),$
(3)

where $N_{\text{sub-sym}}$ is the size of the compressed sub-symbolic
representation. The behavior of $N_{\text{sub-sym}}$ is driven by the
compression approach used, but in general $N_{\text{sub-sym}}$ ends up being
much smaller than $V/\delta^{3}$; in the ideal case $N_{\text{sub-sym}}$ would
grow gracefully with respect to the complexity of the scene and the resolution
required by the task. We show a notional example of a compressed sub-symbolic
layer in Fig. 2c. This can represent the 336 original cells of the sub-
symbolic layer of Fig. 2b with roughly 23 nodes, 22 edges, and a 2D polygonal
mesh with roughly 132 vertices. Note that moving from Fig. 2b to Fig. 2c may
entail a _lossy_ compression of the sub-symbolic layer, _i.e.,_ the compressed
representation may only be an approximate representation of the geometry of
the environment. We consider this to be a feature rather than a bug: the
general idea is that we can always compress the sub-symbolic representation to
fit into the available memory of our robot, although such a compression might
induce some performance degradation in the execution of the task (_e.g.,_
coarser maps might lead to longer paths in motion planning).

###  II-C Hierarchical Representations Enable Efficient Inference

While above we showed that hierarchical representations are more scalable in
terms of memory requirements, this section shows that the graphs underlying
hierarchical representations also enable fast inference. Specifically, we show
that these hierarchical graphs have small _treewidth_ : their treewidth does
not grow with the size of the graph (_i.e.,_ proportionally to the size of the
explored scene), but rather with the treewidth of each layer in the hierarchy.
The treewidth is a well-known measure of complexity for many problems on
graphs [22, 23, 24, 25]. Chandrasekaran et al. [26] show that the graph
treewidth is the only structural parameter that influences tractability of
probabilistic inference on graphical models: while inference is NP-hard in
general for inference on graphical models [27], proving that a graph has small
treewidth opens the door to efficient, polynomial-time inference algorithms.
Additionally, in our previous work we have shown that the treewidth is also
the main factor impacting the expressive power and tractability of novel graph
neural tree architectures, namely _neural trees_ [16]. The results in this
section therefore open the door to the efficient use of powerful tools for
learning over graphs; see Section III-D2.

In the rest of this section, we formalize the notion of hierarchical graph and
show that the treewidth of a hierarchical graph is always bounded by the
maximum treewidth of each of its layers. We do this by proving that the tree
decomposition of the hierarchical graph can be obtained by a suitable
concatenation of the tree decompositions of its layers. The results in this
section are general and apply to arbitrary hierarchical representations (as
defined below) beyond the representations of indoor environments we consider
later in the paper.

######  Definition 1 (Hierarchical Graph).

A graph ${\cal G}=({\cal V},{\cal E})$ is said to be a _hierarchical graph_ if
the set of nodes ${\cal V}$ can be partitioned into $\ell$ layers, _i.e.,_
${\cal V}=\cup_{i=1}^{\ell}{\cal V}_{i}$, such that

  1. 1.

_single parent:_ each node $v\in{\cal V}_{i}$ at layer $i$ shares an edge with
at most one node in the layer ${\cal V}_{i+1}$ above,

  2. 2.

_locality:_ each $v\in{\cal V}_{i}$ can only share edges with nodes in the
same or adjacent layers, _i.e.,_ ${\cal V}_{i-1}$, ${\cal V}_{i}$, or ${\cal
V}_{i+1}$,

  3. 3.

_disjoint children:_ for any $u,v\in{\cal V}_{i}$ the children of $u$ and $v$,
namely $C(v)$ and $C(u)$ are disjoint (_i.e.,_ they share no nodes or edges),
where

$C(u)\triangleq\left\\{w\in{\cal V}_{i-1}~{}|~{}(w,u)\in{\cal E}\right\\}$ (4)

denotes the children of $u$, _i.e.,_ the set of nodes in layer ${\cal
V}_{i-1}$ sharing an edge with $u$.

We refer to ${\cal G}$ as an $\ell$-layered hierarchical graph and denote with
${\cal V}_{i}$ the set of nodes in layer $i$. Moreover, we conventionally
order the layers from the bottom to the top, _i.e.,_ the lowest layer is
${\cal V}_{1}$, while the top layer is ${\cal V}_{\ell}$.

To gain some insight into Definition 1, consider a hierarchical graph where
the bottom layer ${\cal V}_{1}$ describes the objects in the environment,
while the higher layers ${\cal V}_{2}$ and ${\cal V}_{3}$ describe room and
buildings, respectively. Then, the first condition in Definition 1 requires
that each object belongs to a single room, and each room belongs to a single
building. The second condition restricts the inter-layer edges to connect
objects to rooms, and rooms to buildings. Finally, the third condition says
that objects in a room are not connected to objects in another room, and that
rooms in a building do not share edges with rooms in other buildings. These
conditions are relatively mild; we note that they are easily met when the
edges in the graph represent inclusion or adjacency (_i.e.,_ for the graphs
considered in the rest of this paper).

We now show that a tree decomposition of a hierarchical graph can be
constructed by concatenating the tree decomposition of each of its layers.
This result will allow us to obtain the treewidth bound in Proposition 3. The
resulting tree decomposition algorithm (Algorithm 1) will also be used in the
neural tree approach used for room classification in Section III-D2. The
interested reader can find a refresher about tree decomposition and treewidth
in Section A-A444We leave the proof of Theorem 2 in the main text since it
contains a description of Algorithm 1, while we postpone other proofs to the
appendix. and an example of execution of Algorithm 1 in Fig. 3.

######  Theorem 2 (Tree Decomposition of Hierarchical Graph).

Let ${\cal G}=({\cal V}\\!=\\!\cup_{i=1}^{\ell}{\cal V}_{i},{\cal E})$ be an
$\ell$-layered hierarchical graph. Let $T$ be the tree decomposition of the
sub-graph spanned by ${\cal V}_{\ell}$ (top layer) and $T_{v}$ be the tree
decomposition of the sub-graph spanned by the children $C_{i}(v)$ of $v$, for
$v\in{\cal V}_{i}$ and $i=\ell,\ldots,2$. Then a tree decomposition for graph
${\cal G}$ can be constructed from $\\{T_{v}~{}|~{}v\in{\cal V}_{i}$ and
$i=\ell,\ldots,2\\}$ and $T$, by the simple concatenation procedure described
in Algorithm 1.

###### Proof.

(i) Notation: We denote the tree decomposition of a graph ${\cal G}$ by
$T=(B,E)=\text{TD}\left[{\cal G}\right]$, where $B$ denotes the collection of
bags and $E$ denotes the set of edges in the tree decomposition graph $T$. For
a tree decomposition graph $T=(B,E)$ and any node $w$ (not already included in
any of the bags $b\in B$), we denote $T+\\{w\\}$ to be the tree decomposition
$T$, after adding the element $w$ to every bag in $T$. Finally, given two
disjoint trees $T,T^{\prime}$ and an edge $(b,b^{\prime})$ with $b\in T$ and
$b^{\prime}\in T^{\prime}$, we use $T\leftarrow T\oplus
T^{\prime}\oplus{(b,b^{\prime})}$ to indicate updating graph $T$ by adding
graph $T^{\prime}$, along with the edge $(b,b^{\prime})$ to it.

Data: $\ell$-layered hierarchical graph ${\cal G}=({\cal
V}\\!=\\!\cup_{i=1}^{\ell}{\cal V}_{i},{\cal E})$

Result: Tree decomposition $T=(B,E)$

/* extract sub-graph with nodes ${\cal V}_{\ell}$ */

1 ${\cal G}_{\ell}\leftarrow{\cal G}[{\cal V}_{\ell}]$;

/* tree decomposition of ${\cal G}_{\ell}$ */

2 $T=(B,E)\leftarrow\text{TD}\left[G_{\ell}\right]$;

3 for  _each layer $i=\ell,\ldots,2$_ do

4 for  _each node $v$ in ${\cal V}_{i}$_ do

5 $T_{v}=(B_{v},L_{v})\leftarrow\text{TD}\left[{\cal G}[C(v)]\right]$;

6 ${T^{{}^{\prime}}_{v}\leftarrow T_{v}+\\{v\\}}$;

7 Pick any bag $b$ in $T$ such that $v\in b$;

8 Pick any bag $b^{\prime}$ in $T^{{}^{\prime}}_{v}$;

9 $T\leftarrow T\oplus T^{\prime}\oplus{(b,b^{\prime})}$;

10

11

Algorithm 1 Tree-Decomposition of Hierarchical Graphs

(ii) Intuitive Explanation of Algorithm 1: We form the tree decomposition $T$
of the hierarchical graph sequentially. We initialize $T$ with the tree
decomposition of the top layer graph ${\cal G}_{\ell}={\cal G}[{\cal
V}_{\ell}]$. We then augment $T$ with the tree decomposition graphs $T_{v}$ of
$C(v)$, for each $v\in{\cal V}_{\ell}$. We augment bags of $T_{v}$ with
element $\\{v\\}$ to mark the fact that $v$ connects each node in $C(v)$ (in
graph ${\cal G}$). This procedure is carried out for the remaining layers
$i=\ell-1,\ldots,2$ and all nodes $v\in{\cal V}_{i}$.

Figure 3 shows an example of execution of Algorithm 1 for the graph in Fig.
3a. Figure 3b shows the (disconnected) tree decompositions produced by line 1
(which produces the single green bag B1) and the first execution of line 1
(_i.e.,_ for $i=\ell$, which produces the two red bags). Figure 3c shows the
result produced by the first execution of line 1 and line 1, which adds B1 to
all the red bags, and then connects the two tree decompositions with an edge,
respectively. Figures 3d and 3e show the result produced by the next iteration
of the algorithm.

(iii) Proof: Recall that a tree decomposition $T=(B,E)$ of a graph ${\cal
G}=({\cal V},{\cal E})$ must satisfy the following conditions:

C1

$T$ must be a tree;

C2

Every bag $b\in B$ must be a subset of nodes of the given graph ${\cal G}$,
_i.e.,_ $b\subseteq{\cal V}$;

C3

For all edges $(u,v)\in{\cal E}$ in the given graph ${\cal G}$, there must
exist a bag $b\in B$ such that $u,v\in b$;

C4

For every node $v\in{\cal V}$ in the given graph ${\cal G}$, the set of bags
$\\{b\in B~{}|~{}v\in b\\}$ must form a connected component in $T$;

C5

For every node $v\in{\cal V}$ in the given graph ${\cal G}$, there must exist
a bag $b\in B$ that contains it, _i.e.,_ $v\in b$. To state this in another
way: $\cup_{b\in B}\\{v\in b\\}={\cal V}$.

Algorithm 1 constructs a tree decomposition $T$ of ${\cal G}$ sequentially by
exploring the graph, by layers (going from layer-$\ell$ to layer $1$) and by
nodes $v$ in each layer. Let ${\cal G}^{e}_{t}$ denote the sub-graph of ${\cal
G}$ that is explored by Algorithm 1, until iteration $t$. Here, we count an
iteration to be the execution of Algorithm 1, and count the initialization at
Algorithm 1 as the first iteration (_i.e.,_ the first iteration initializes
the tree decomposition, while the subsequent iterations update the tree
decomposition).

(a)

(b)  (c)

(d)  (e)

Figure 3: (a) Hierarchical object-room-building graph with objects O1, O2,
$\ldots$ O8, rooms R1, R2, R3, R4, and a single building B1. (b) Tree
decomposition $T$ of the building B1 and associated $T_{v}$ of the rooms
(_i.e.,_ children of B1) as computed in line 1 and 1 of Algorithm 1. (c) $T$
after adding B1 to every bag of the tree decomposition of the rooms and
joining $T_{v}$ to $T$ (line 1 and line 1 in Algorithm 1). (d) $T$ and
associated $T_{v}$ for the object nodes. (e) Final tree-decomposition of the
object-room-building graph formed by the concatenation procedure described in
Algorithm 1.

We prove that, at any iteration $t$, the tree decomposition graph $T$
constructed in Algorithm 1 is a valid tree decomposition of the graph ${\cal
G}^{e}_{t}$. We prove this by induction. At $t=1$, the explored graph ${\cal
G}^{e}_{1}$ is nothing but ${\cal G}_{\ell}$. Note that $T$, after iteration 1
(_i.e.,_ after the initialization at Algorithm 1) by definition equals the
tree decomposition ${\cal G}^{e}_{1}$. Now we only need to prove that
subsequent iterations produce a tree decomposition of ${\cal G}^{e}_{t}$. Each
new iteration (_i.e.,_ execution of Algorithm 1) updates $T$ to include
$T^{{}^{\prime}}_{v}$ and the edge $(b,b^{\prime})$. The new explored graph is
${\cal G}^{e}_{t+1}={\cal G}^{e}_{t}\oplus{\cal G}[{C(v)+\\{v\\}}]$. It
suffices to argue that $T\oplus T^{{}^{\prime}}_{v}\oplus(b,b^{\prime})$
(Algorithms 1 to 1) is a valid tree decomposition of ${\cal G}^{e}_{t+1}={\cal
G}^{e}_{t}\oplus{\cal G}[C(v)+\\{v\\}]$. We argue this by proving that the
requirements C1-C5 are satisfied.

For C1, we note that $T\oplus T^{{}^{\prime}}_{v}\oplus(b,b^{\prime})$ is a
tree as $T$ and $T^{{}^{\prime}}_{v}$ are disjoint trees and $(b,b^{\prime})$
is an edge connecting the two trees, $T$ and $T^{{}^{\prime}}_{v}$. C2 is
satisfied because $T$ and $T^{{}^{\prime}}_{v}$ are tree decompositions of
sub-graphs of ${\cal G}$. C3 is satisfied for all edges in ${\cal G}^{e}_{t}$
and in ${\cal G}[C(v)]$. The only edges that remain are those that connect
node $v$ to nodes in $C(v)$, namely $\\{(v,u)~{}|~{}u\in C(v)\\}$. We note
that by adding $\\{v\\}$ to every bag of $T_{v}$ to obtain
$T^{{}^{\prime}}_{v}$ we get that all such edges are also included in the bags
of $T^{{}^{\prime}}_{v}$. C4 is satisfied again because $T$ and $T_{v}$ are
valid tree decompositions and $T^{{}^{\prime}}_{v}$ is built from $T_{v}$ by
adding $v$ to all bags of $T_{v}$. As $T_{v}$ is a tree graph disjoint from
$T$, we retain property C4. ${\cal G}^{e}_{t+1}={\cal G}^{e}_{t}\oplus{\cal
G}[C(v)+\\{v\\}]$ (_i.e.,_ nodes in ${\cal G}^{e}_{t}$ and $C(v)$), have a bag
in $T\oplus T^{{}^{\prime}}_{v}\oplus(b,b^{\prime})$. ∎

Theorem 2 above showed that we can easily build a tree decomposition of a
hierarchical graph by cleverly “gluing together” tree decompositions of sub-
graphs in the hierarchy. Now we want to use that result to bound the treewidth
of a hierarchical graph ${\cal G}$. Each node in the tree decomposition is
typically referred to as a _bag_ , and intuitively represents a subset of
nodes in the original graph. The treewidth of a tree decomposition $T$ is
defined as the size of the largest bag minus one (see Section A-A). The
treewidth of a graph ${\cal G}$ is then defined as the minimum treewidth that
can be achieved among all tree decompositions of $\mathcal{G}$. Since
Algorithm 1 describes one such tree decompositions, Theorem 2 implies the
following upper bound on the treewidth of a hierarchical graph.

######  Proposition 3 (Treewidth of Hierarchical Graph).

The treewidth of an $\ell$-layered hierarchical graph ${\cal G}$ is bounded by
the treewidths of the sub-graphs ${\cal G}[C(v)]$ spanned by the children
$C(v)$ of every node $v$ in ${\cal G}$ and the treewidth of the graph ${\cal
G}[{\cal V}_{\ell}]$ spanned by all layer-$\ell$ nodes:

$\text{tw}\left[{\cal G}\right]\leq\max\left\\{\max_{v\in{\cal
V}}\\{\text{tw}\left[{\cal G}[C(v)]\right]+1\\},\text{tw}\left[{\cal G}[{\cal
V}_{\ell}]\right]\right\\}.$ (5)

Intuitively, the proposition states that the treewidth of a hierarchical graph
does not grow with the number of nodes in the graph, but rather with the
treewidth (roughly speaking, the complexity) of each layer in the graph.

###  II-D 3D Scene Graphs and Indoor Environments

While the considerations in the previous sections apply to generic
hierarchical representations, in this section we tailor the discussion to a
specific hierarchical representation, namely a _3D scene graph_ , and discuss
its structure and properties when such representation is used to model indoor
environments.

Choice of Symbols for Indoor Navigation. We focus on indoor environments and
assume the robot is tasked with navigating to certain objects and rooms in a
building. Therefore, we include the following groups of symbols that form the
_layers_ in our hierarchical representation: free-space, obstacles, objects
and agents, rooms, and building. The details of the label spaces that we use
for the object and room symbols can be found in Section A-D. The only “agent”
in our setup is the robot mapping the environment. In terms of relations
between symbols, we mostly consider two types of relations: inclusion (_i.e.,_
the chair _is in_ the kitchen) and adjacency or traversability (_i.e.,_ the
chair _is near_ the table, the kitchen is adjacent to/reachable from the
dining room).

As we mentioned in Section II-B, this choice of symbols and relations is
dictated by the tasks we envision the robot to perform and therefore it is not
universal. For instance, other tasks might require breaking down the free-
space symbol into multiple symbols (_e.g.,_ to distinguish the front from the
back of a room), or might require considering other agents (_e.g.,_ humans
moving in the environment as in [2, 4]). This dependence on the task also
justifies the different definitions of 3D scene graph found in the recent
literature: for instance, the original proposal in [3] focuses on
visualization and human-machine interaction tasks, rather than robot
navigation, hence the corresponding scene graphs do not include the free-space
as a symbol; the proposals [7, 5] consider smaller-scale tasks and disregard
the room and building symbols. Similarly, the choice of relations is task
dependent and can include a much broader set of relations beyond inclusion and
adjacency. For instance, relations can describe attributes of an object
(_e.g.,_ “has color”), material properties (_e.g.,_ “is made of”), can be used
to compare entities (_e.g.,_ “is taller than”), or may encode actions (_e.g.,_
a person “carries” an object, a car “drives on” the road). While these other
relations are beyond the scope of our paper, we refer the reader to [28] for
further discussion.

Choice of Sub-symbolic Representations. We ground each symbol in compact sub-
symbolic representations; intuitively, this reduces to attaching geometric
attributes to each symbol observed by the robot. We ground the “obstacle”
symbol into a 3D mesh describing the observed surfaces in the environment. We
ground the “free-space” symbol using a places sub-graph, which can be
understood as a topological map of the environment. Specifically, each place
node in the graph of places is associated with an obstacle-free 3D location
(more precisely, a sphere of free space described by a centroid and radius),
while edges represent traversability between places.555While we postpone the
technical details to Section III-C, the graph of places can be understood as a
sparse approximation of a Generalized Voronoi Diagram (GVD) of the
environment, a data structure commonly used in computer graphics and
computational geometry to compress shape information [29]. We ground the
“agent” symbol using the pose-graph describing the robot trajectory [30]. We
ground each “object”, “room”, and “building” symbol using a centroid and a
bounding box. Somewhat redundantly, we also store the mesh vertices
corresponding to each object, and the set of places included in each room,
which can be also understood as additional groundings for these symbols. As
discussed in the next section, this is mostly a byproduct of our algorithms,
rather than a deliberate choice of sub-symbolic representation.

3D Scene Graphs for Indoor Environments The choices of symbolic and sub-
symbolic representations outlined above lead to the 3D scene graph structure
visualized in Fig. 1. In particular, _Layer 1_ is a metric-semantic 3D mesh.
_Layer 2_ is a sub-graph of objects and agents; each object has a semantic
label (which identifies the symbol being grounded), a centroid, and a bounding
box (providing the grounding); each agent is modeled by a pose graph
describing its trajectory (in our case the robot itself is the only agent).
_Layer 3_ is a sub-graph of _places_ (_i.e.,_ a topological map) where each
place is an obstacle-free location and an edge between places denotes
straight-line traversability. _Layer 4_ is a sub-graph of rooms where each
room has a centroid, and edges connect adjacent rooms. _Layer 5_ is a building
node connected to all rooms (we assume the robot maps a single building).
Edges connect nodes within each layer (_e.g.,_ to model traversability between
places or rooms, or connecting nearby objects) or across layers (_e.g.,_ to
model that mesh vertices belong to an object, that an object is in a certain
room, or that a room belongs to a building). Note that the 3D scene graph
structure forms a hierarchical graph meeting the requirements of Definition 1.

Treewidth of Indoor 3D Scene Graphs. In Section II-C we concluded that the
treewidth of a hierarchical graph is bounded by the treewidth of its layers.
In this section, we particularize the treewidth bound to 3D scene graphs
modeling indoor environments. We first prove bounds on the treewidth of key
layers in the 3D scene graph (Lemmas 4 and 5). Then we translate these bounds
into a bound on the treewidth of the object-room-building sub-graph of a 3D
scene graph (Proposition 6); the bound is important in practice since we will
need to perform inference over such a sub-graph to infer the room (and
potentially building) labels, see Section III-D2.

We start by bounding the treewidth of the room layer.

######  Lemma 4 (Treewidth of Room Layer).

Consider the sub-graph of rooms, including the nodes in the room layer of a 3D
scene graph and the corresponding edges. If each room connects to at most two
other rooms that have doors (or more generally passage ways) leading to other
rooms, then the room graph has treewidth bounded by two.

The insight behind Lemma 4 is that the room sub-graph is not very far from a
tree (_i.e.,_ it has low treewidth) as long as there are not many rooms with
multiple doors (or passage ways). In particular, the room sub-graph has
treewidth equal to 2 if each room has at most two passage ways to other rooms
with multiple entries. Note that the theorem statement allows a room to be
connected to an arbitrary number of rooms with a single entry (_i.e.,_ a
corridor leading to multiple rooms that are only accessible via that
corridor). Figure 4a reports empirical upper-bounds of the treewidth of the
room sub-graphs in the 90 scenes of the Matterport3D dataset [31] (see Section
VI for more details about the experimental setup). The observed treewidth is
at most 2, confirming that the conclusions of Lemma 4 hold in several
apartment-like environments.

(a)  (b)

Figure 4: Histogram of the treewidth upper-bounds for (a) the room sub-graph
and (b) the object sub-graph of the 3D scene graphs obtained using the 90
scenes of the Matterport3D dataset [31]. We use the minimum-degree and the
minimum-fill-in heuristic to obtain treewidth upper-bounds [32]. We then
compute an upper-bound to be the lowest of the two.

######  Lemma 5 (Treewidth of Object Layer).

Consider the sub-graph of objects, which includes the nodes in the object
layer of a 3D scene graph and the corresponding edges. The treewidth of the
object sub-graph is bounded by the maximum number of objects in a room.

The result is a simple consequence of the fact that in our 3D scene graph,
there is no edge connecting objects in different rooms, therefore, the graph
of objects includes disconnected components corresponding to each room, whose
treewidth is bounded by the size of that connected component, _i.e.,_ the
number of objects in that room. Figure 4b reports the treewidth upper-bounds
for the object sub-graphs in the Matterport3D dataset. We observe that the
treewidth of the object sub-graphs tends to be larger compared to the room
sub-graphs, but still remains relatively small (below 20) in all tests.

We can now conclude with a bound on the treewidth of the object-room-building
graph in a 3D scene graph.

######  Proposition 6 (Treewidth of the Object-Room-Building Graph).

Consider the object-room-building graph of a building, including the nodes in
the object, room, and building layers of a 3D scene graph and the
corresponding edges. Assume the treewidth of the room graph is less than the
treewidth of the object graph. Then, the treewidth $\text{tw}[{\cal G}]$ of
the object-room-building graph ${\cal G}$ is bounded by

$\text{tw}[{\cal G}]\leq 1+N_{o},$ (6)

where $N_{o}$ denotes the largest number of objects in a room.

Proposition 6 indicates that the treewidth of the object-room-building graph
does not grow with the size of the scene graph, but rather depends on how
cluttered each room is. This is in stark contrast with the treewidth of social
network graphs [33, 16], and further motivates the proposed hierarchical
organization. The treewidth bounds in this section open the door to tractable
inference techniques; in particular, in Section III-D2, we show they allow
applying novel graph-learning techniques, namely, the neural tree [16].

##  III Real-time Incremental
3D Scene Graph Layers Construction

This section describes how to estimate an _odometric_ 3D scene graph directly
from visual-inertial data as the robot explores an unknown environment.
Section IV then shows how to correct the scene graph in response to loop
closures.

We start by reconstructing a metric-semantic 3D mesh to populate _Layer 1_ of
the 3D scene graph (Section III-A), and use the mesh to extract the objects in
_Layer 2_ (Section III-B). We extract the places in  _Layer 3_ as a byproduct
of the 3D mesh computation by first computing a Generalized Voronoi Diagram
(GVD) of the environment, and then approximating the GVD as a sparse graph of
places (Section III-C). We then populate the rooms in  _Layer 4_ by segmenting
their geometry using persistent homology (Section III-D1) and assigning them a
semantic label using the neural tree (Section III-D2). In this paper, we
assume that the scene we reconstruct consists of a single building, and for
_Layer 5_ we instantiate a single building node that we connect to all
estimated room nodes.

###  III-A Layers 1: Mesh

We build a metric-semantic 3D mesh (_Layer 1_ of the 3D scene graph) using
Kimera [34] and Voxblox [10]. In particular, we maintain a metric-semantic
voxel-based map within an active window around the robot. Each voxel of the
map contains free-space information and a distribution over possible semantic
labels. We gradually convert this voxel-based map into a 3D mesh using
marching cubes, attaching a semantic label to each mesh vertex. This is now
standard practice (_e.g.,_ the same idea was used in [35] without inferring
the semantic labels) and the expert reader can safely skip the rest of this
subsection.

In more detail, for each keyframe666Keyframes are selected camera frames
chosen by the visual-inertial odometry pipeline, see Section III-B and [2] for
further details. we use a 2D semantic segmentation network to obtain a pixel-
wise semantic segmentation of the RGB image, and reconstruct a depth-map using
stereo matching (when using a stereo camera) or from the depth channel of the
sensor (when using an RGB-D camera). We then convert the semantic segmentation
and depth into a semantically-labeled 3D point cloud and transform it
according to the odometric estimate of the robot pose (Section III-B). We use
Voxblox [10] to integrate the semantically-labeled point cloud into a
Truncated Signed Distance Field (TSDF) using ray-casting, and Kimera [2] to
perform Bayesian updates over the semantic label of each voxel during ray-
casting. Both Voxblox [10] and Kimera [2] operate over an _active window_ ,
_i.e.,_ only reconstruct a voxel-based map within a user-specified radius
$r_{a}$ around the robot ($r_{a}=$8\text{\,}\mathrm{m}$$ in our tests)777The
radius of the active window has to be larger than the maximum ray-casting
distance to construct the TSDF ($\approx 4\text{\,}\mathrm{m}$) and the block
resolution of the spatial hashing ($\approx 1.5\text{\,}\mathrm{m}$). to bound
the memory requirements. Within the active window, we extract the 3D metric-
semantic mesh using Voxblox’ marching cubes implementation, where each mesh
vertex is assigned the most likely semantic label from the corresponding
voxel. We then use spatial hashing [36] to integrate ${\cal M}_{a}$, the mesh
inside the active window, into the full mesh ${\cal M}_{f}$, optionally also
compressing the mesh to a given resolution. The use of the active window
circumvents the need to maintain a monolithic and memory-hungry voxel-based
representation of the entire environment (as done in our original proposal in
[34]), and instead allows to gradually transform the voxel-based map in the
active window (moving with the robot) into a lighter-weight 3D mesh. The full
mesh, ${\cal M}_{f}$, is uncorrected for odometric drift, and we address loop
closure detection and optimization in Section IV.

###  III-B Layers 2: Objects and Agents

Agents. The agent layer consists of the pose graph describing the robot
trajectory (we refer the reader to [2] for extensions to multiple agents,
including humans). During exploration, the odometric pose graph is obtained
using stereo or RGB-D visual-inertial odometry, which is also available in
Kimera [34, 2]. The poses in the pose graph correspond to the keyframes
selected by Kimera, and for each pose we also store visual features and
descriptors, which are used for loop closure detection in Section IV. As
usual, edges in the pose graph correspond to odometry measurements between
consecutive poses, while we will also add loop closures as described in
Section IV.

Objects. The object layer consists of a graph where each node corresponds to
an object (with a semantic label, a centroid, and a bounding box) and edges
connect nearby objects. After extracting the 3D mesh within the active window,
we segment objects by performing Euclidean clustering [37] on ${\cal V}_{a}$,
the vertices of $\mathcal{M}_{a}$. In particular, we independently cluster the
sets of vertices ${\cal V}_{a}^{i}$ with the same semantic label $i$ (where
${\cal V}_{a}^{i}=\\{v\in{\cal V}_{a}\;{:}\;\text{label}(v)=i\\}$) for all
semantic labels of interest. As in Kimera [2], each resulting cluster is then
used to estimate a centroid and bounding box for each putative object. After
each update, if a newly-detected object overlaps with an existing object node
of the same semantic class in the scene graph, we merge them together by
adding new mesh vertices to the previous object node; otherwise we add the new
object as a new node. In practice, we consider two objects overlapping if the
centroid of one object is contained in the other object’s bounding box, a
proxy for spatial overlap measures such as Intersection over Union (IoU).888In
previous work [2], we also detected objects with known shape using 3D
registration [38]; in practice, we see that such approach only works well for
large objects since small objects are not well-described by the 3D mesh, whose
resolution is implicitly limited by the resolution of the voxel-based map in
the active window ($10\text{\,}\mathrm{cm}$ in our tests). A more promising
approach is to directly ground small 3D objects (_e.g.,_ a pen) from sensor
data as in [39].

(a) GVD and 3D Mesh

(b) Free-space Representation

Figure 5: (a) GVD (wireframe) of an environment with GVD voxels having three
or more basis points highlighted. (b) Sparse places graph (in black) and
associated spheres of free-space corresponding shown in red. Note that the
union of the spheres roughly approximates the geometry of the free-space.

###  III-C Layers 3: Places

We build the places layer by computing a Generalized Voronoi Diagram (GVD) of
the environment, and then sparsifying it into a graph of places. While the
idea of sparsifying the GVD into a graph has appeared in related work [2, 40],
these works extract such representations from a monolithic ESDF of the
environment, a process that is computationally expensive and confined to off-
line use ([2] reports computation times in the order of tens of minutes).
Instead, we combine and adapt the approaches of Voxblox [10], which presents a
incremental version of the brushfire algorithm that converts a TSDF to an
ESDF, and of Lau et al. [41], who present an incremental version of the
brushfire algorithm that is capable of constructing a GVD during the
construction of the ESDF, but takes as input a 2D occupancy map. As a result,
we show how to obtain a local GVD and a graph of places as a byproduct of the
3D mesh construction.

From Voxels to the GVD. The GVD is a data structure commonly used in computer
graphics and computational geometry to compress shape information [29]. The
GVD is the set of voxels that are equidistant to at least two closest
obstacles (“basis points” or “parents”), and intuitively forms a skeleton of
the environment [29] (see Fig. 5a). Following the approach in [41], we use the
fact that voxels belonging to the GVD can be easily detected from the
wavefronts of the brushfire algorithm used to create and update the ESDF in
the active window. Intuitively, GVD voxels have the property that multiple
wavefronts “meet” at a given voxel (_i.e.,_ fail to update the signed distance
of the voxel), as these voxels are equidistant from multiple obstacles. The
brushfire algorithm (and its incremental variant) are traditionally seeded at
voxels containing obstacles (_e.g.,_ [41]), but as mentioned previously, we
follow the approach of [10] and use the TSDF to seed the brushfire wavefronts
instead. In particular, we track all TSDF voxels that correspond to zero-
crossings (_i.e.,_ voxels containing a surface) when creating
$\mathcal{M}_{a}$ using marching cubes and then use these voxels as a
surrogate occupancy grid (_i.e.,_ wavefronts of the brushfire algorithm are
treated as if they started from one of these TSDF voxels). Like [10], we also
use the TSDF voxels within the truncation distance to seed the wavefronts of
the brushfire algorithm, _i.e.,_ instead of starting the wavefronts directly
from occupied voxels, we start them from the outermost voxels still within the
truncation distance. In our implementation, we compute a variant of the GVD
called the _$\theta$ -simplified medial axis_ ($\theta$-SMA) from [42] that
filters out less stable and noisier portions of the GVD using a threshold on
the minimum angle between basis points. Each voxel in the GVD is assigned a
distance to its basis points, defining a sphere of free-space surrounding the
voxel; collectively the GVD provides a compact description of the free-space
and its connectivity (Fig. 5b). Indeed, up to discretization, the GVD provides
an _exact_ description of the free-space [29], meaning that one can
reconstruct the full shape of the free-space from its GVD.

Data: GVD graph $\mathcal{G}_{a}$, previous map of spatial hashes to voxel
clusters $C$, spatial resolution $\delta_{p}$

Result: New or updated nodes $\mathcal{N}$ and edges $\mathcal{E}$

1 $\mathcal{N}\leftarrow\emptyset$

2 $\mathcal{E}\leftarrow\emptyset$

3 $\mathcal{V}_{u}\leftarrow\\{\text{\emph{voxel}}\
v\in\mathcal{G}_{a}\;{:}\;\text{updated}(v)=\text{true}\\}$

4 for  _voxel_ $v\in\mathcal{V}_{u}$ do

5 $h_{v}\leftarrow\text{hash}(v,\delta_{p})$

6 found $\leftarrow$ false

7 for  _cluster_ $c\in C[h_{v}]$ do

8 if $v$ _neighbors any voxel in cluster_ $c$ then

9 $c\leftarrow c\cup\\{v\\}$

10 found $\leftarrow$ true

11 break

12

13

14

15 if $\lnot$found then

16 $C[h_{v}]\leftarrow C[h_{v}]\cup\\{v\\}$

17

18

19

/* add node corresponding to cluster of $v$ to set of returned nodes */

20 $\mathcal{N}\leftarrow\mathcal{N}\cup\\{\text{node}(v)\\}$

21

22

23 for  _voxel_ $n\in\text{neighbors}(\mathcal{G}_{a},v)$ do

24 $h_{n}=\text{hash}(n,\delta_{p})$

25 if $\text{node}(v)\neq\text{node}(n)\land h_{v}=h_{n}$ then

26 $\mathcal{N}\leftarrow\mathcal{N}\setminus\\{\text{node(n)}\\}$

27
$\mathcal{E}\leftarrow\mathcal{E}\setminus\\{e\in\mathcal{E}\;{:}\;\text{node(n)}\in
e\\}$

28 Merge clusters for nodes $n$ and $v$

29

30 else if $\text{node}(v)\neq\text{node}(n)$ then

31 $\mathcal{E}\leftarrow\mathcal{E}\cup\\{(\text{node}(v),\text{node}(n))\\}$

32

33

34

Algorithm 2 Places Sparsification

From the GVD to the Places Graph. While the GVD already provides a compressed
representation of the free-space, it still typically contains a large number
of voxels (_e.g.,_ more than $10,000$ in the Office dataset considered in
Section VI). We could instantiate a graph of places with one node for each GVD
voxel and edges between nearby voxels, but such representation would be too
large to manipulate efficiently (_e.g.,_ our room segmentation approach in
Section III-D1 would not scale to operating on the entire GVD). Previous
attempts at sparsifying the GVD (notably [40] and our earlier paper [11]) used
a subset of topological features (edges and vertices in the GVD, which
correspond to GVD voxels having more than three and more than four basis
points respectively) to form a sparse graph of places. However, the resulting
graph does not capture the same connectivity of free-space as the full GVD,
and may lead to graph of places with multiple connected components. It would
also be desirable for the user to balance the level of compression with the
quality of the free-space approximation instead of always picking certain GVD
voxels.

To resolve these issues, we first form a graph $\mathcal{G}_{a}$ of the GVD
where each GVD voxel corresponds to a node in $\mathcal{G}_{a}$ and edges
connect any two neighboring GVD voxels. We then use voxel-hashing [36] to
spatially cluster —at a user-specified resolution— all updated GVD voxels with
at least $n_{b}$ basis points ($n_{b}=2$ in our tests) inside the active
window, ensuring that each cluster forms a connected component in
$\mathcal{G}_{a}$. This clustering is shown in Algorithm 2. This algorithm
takes as input the graph of GVD voxels, $\mathcal{G}_{a}$, and for every voxel
that was updated, computes a spatial hash value [36], denoted
$\text{hash}(v,\delta_{p})$ in the algorithm for a given voxel $v$ and spatial
resolution $\delta_{p}$. This hash value is the same for voxels within a cube
of 3D space with side-lengths of $\delta_{p}$ and naturally clusters the
voxels of the GVD into clusters of the provided spatial resolution. Algorithms
2 to 2 incrementally grow clusters of voxels with the same hash value.
However, this results in multiple clusters on the same connected component of
$\mathcal{G}_{a}$. Algorithms 2 to 2 then search over the neighbors of a given
voxel $v$ in $\mathcal{G}_{a}$ and either combines clusters that share an edge
in $\mathcal{G}_{a}$ and the same spatial hash value, or propose an edge
between the clusters if the hash value differs. Once Algorithm 2 terminates,
we obtain a set of proposed nodes (corresponding to unique clusters of voxels
in $\mathcal{G}_{a}$ that form connected components) and proposed edges based
on the connectivity of $\mathcal{G}_{a}$.

These proposed nodes and edges are assigned information from the voxels that
generated them: each new or updated node is assigned a position and distance
to the nearest obstacle from the GVD voxel with the most basis points in the
cluster; each new or updated edge is assigned a distance that is the minimum
distance to the nearest obstacle of all the GVD voxels that the edge passes
through. We then associate place nodes with the corresponding mesh-vertices of
each basis point using the zero-crossing identified in the TSDF by marching
cubes. We run Algorithm 2 after every update to the GVD and merge the proposed
nodes $\mathcal{N}$ and edges $\mathcal{E}$ into the sparse graph of
places.999Separate book-keeping is done to remove nodes and edges that no
longer correspond to any GVD voxels during the brushfire update of the ESDF
and GVD, as GVD voxels are removed during this update when a wavefront is able
to modify the signed distance of the voxel

Inter-layer Edges. After building the places graph, we add inter-layer edges
from each object or agent node to the nearest place node in the active window
using nanoflann [43].

###  III-D Layer 4: Rooms

We segment the rooms by first geometrically clustering the graph of places
into separate rooms (using persistent homology, Section III-D1), and then
assigning a semantic label to each room (using the neural tree, Section
III-D2). We remark that the construction of the room layer is fundamentally
different from the construction of the object layer. While we can directly
observe the objects (_e.g.,_ via a network trained to detect certain objects),
we do not have a network trained to detect certain rooms. Instead, we have to
rely on prior knowledge to infer both their geometry and semantics. For
instance, we exploit the fact that rooms are typically separated by small
passageways (_e.g.,_ doors), and that their semantics can be inferred from the
objects they contain.

Figure 6: Connected components (in orange and red) in the sub-graph of places
induced by a dilation distance $\delta$ (walls are in gray, dilated walls are
dashed, places that disappear after dilation are in black).

####  III-D1 Layer 4: Room Clustering via Persistent Homology

This section discusses how to geometrically cluster the environment into
different rooms. Many approaches for room segmentation or detection (including
[2]) require volumetric or voxel-based representations of the full
environments and make assumptions on the environment geometry (_i.e.,_ a
single floor or ceiling height) that do not easily extend to arbitrary
buildings. To resolve these issues, we present a novel approach for
constructing _Layer 4_ of the 3D scene graph by clustering the nodes in the
graph of places ${\cal G}_{p}$ (_Layer 3_); this approach does not assume
planarity of the environment and circumvents the need to process large voxel-
based maps. Our room segmentation approach consists of two stages: the first
stage identifies the number of rooms in the environment (as well as a subset
of places associated with each room), and the second stage assigns the
remaining places in ${\cal G}_{p}$ to rooms via flood-fill.

Identifying Rooms via Persistent Homology. We use the key insight that
dilating the voxel-based map helps expose rooms in the environment: if we
inflate obstacles, apertures in the environment (_e.g.,_ doors) will gradually
close, naturally partitioning the voxel-based map into disconnected components
(_i.e.,_ rooms); however, we apply the same insight to the graph of places
${\cal G}_{p}$ (rather than the voxel-based map) to enable faster computation
and better scalability. As each node and edge in our place graph ${\cal
G}_{p}$ stores a distance to its closest obstacle $d^{p}{}$ (Section III-C),
dilation operations in the voxel-based map can be directly mapped into
topological changes in ${\cal G}_{p}$. More precisely, if we dilate the map by
a distance $\delta$, every place node or edge with $d^{p}{}$ smaller than
$\delta$ will disappear from the graph since it will no longer be in the free
space. We call the dilated graph ${\cal G}_{p}^{\delta}$; see Fig. 6 for an
illustration.

Directly specifying a single best dilation distance $\delta^{*}$ to extract
putative rooms is unwieldy: door and other aperture widths vary from scene to
scene (think about the small doors in an apartment versus large passage ways
in a mansion). In order to avoid hard-coding a single dilation distance, we
use a tool from topology, known as _persistent homology_ [44, 15], to
automatically compute the best dilation distance for a given
graph.101010Persistent homology is the study of the birth and death of
homologies across a _filtration_ of a simplicial complex; see [44, 15] for an
introduction on the topic. In this work, we restrict ourselves to graphs (a
subset of simplicial complexes) and focus only on 0-homologies (connected
components). The basic insight is that the most natural choice of rooms is one
that is more stable (or _persistent_) across a wide range of dilation
distances. To formalize this intuition, the persistent homology literature
relies on the notion of _filtration_. A _filtration_ of a graph ${\cal G}_{p}$
is a set of graphs ${\cal G}_{p}^{\delta_{i}}$ ordered by real-valued
parameters $\delta_{i}$ such that

$\emptyset\subseteq\ldots\subseteq{\cal G}_{p}^{\delta_{i+1}}\subseteq{\cal
G}_{p}^{\delta_{i}}\subseteq\ldots\subseteq{\cal G}_{p}\,.$ (7)

In our case, each graph ${\cal G}_{p}^{\delta_{i}}$ is the sub-graph of ${\cal
G}_{p}$ where nodes and edges with radius smaller than $\delta_{i}$ are
removed;111111Such a filtration is known as the Vietoris-Rips filtration [45].
hence the filtration is a set of graphs corresponding to increasing distances
$\delta_{i}$; see Figs. 7b and 7d for examples of graphs in the filtration.
For each graph in the filtration, we compute the 0-homology (the number of
connected components), which in our setup corresponds to the number of rooms
the graph gets partitioned into; computationally, the 0-homology can be
computed in one shot for all graphs in the filtration by iterating across the
edges in the original graph ${\cal G}_{p}$ using a union set [45], leading to
an extremely efficient algorithm.

The resulting mapping between the dilation distance and the number of
connected components is an example of _Betti curve_ , or $\beta_{0}(\delta)$;
see Fig. 7a for an example. Intuitively, for increasing distances $\delta$,
the graph first splits into more and more connected components (this is the
initial increasing portion of Fig. 7a), and then for large $\delta$ entire
components tend to disappear (leading to a decreasing trend in the second part
of Fig. 7a), eventually leading to all the nodes disappearing (_i.e.,_ zero
connected components as shown in the right-most part of Fig. 7a). In practice,
we restrict the set of distances to a range $\left[d^{-},d^{+}\right]$,
containing reasonable sizes for openings between rooms ([0.5,
1.2]$\text{\,}\mathrm{m}$ in our tests). We also do not count connected
components containing less than a minimum number of vertices ($15$ in our
tests), as we are not interested in segmenting overly small regions as rooms.

(a) Betti Curve (b) $\delta=0.7$ (c) $\delta=1.0$ (d) $\delta=1.3$

Figure 7: (a) Example of Betti curve; we only consider components with at
least 15 nodes. (b-d) Example of filtration of the graph for various
thresholds $\delta$. Nodes with distances $d^{p}{}$ smaller than the threshold
$\delta$ are shown in gray, while nodes with distances above $\delta$ are
colored by component membership.

Choices of rooms that are more persistent correspond to large “flat”
horizontal regions of the Betti curve, where the number of connected
components stays the same across a large sub-set of $\delta$ values. More
formally, let us denote with $H$ the set of unique values assumed by the Betti
curve, _i.e.,_ :

$H=\\{i\in{{\mathbb{N}}}\;{:}\;i=\beta_{0}(\delta),\forall\delta\in\left[d^{-},d^{+}\right]\\}$
(8)

and —for some small positive constant $\epsilon$— denote with $I$ the set of
intervals $I_{j}$ (for each $j\in H$) defined as:

$\displaystyle
I_{j}=\big{\\{}(d_{\min},d_{\max}):\forall\delta\in[d_{\min},d_{\max}],\beta_{0}(\delta)=j,$
$\displaystyle\text{ and }\beta_{0}(d_{\min}-\epsilon)\neq
j,\beta_{0}(d_{\max}+\epsilon)\neq j\big{\\}}$ (9)

where, for each number of connected components $H$, the set $I$ contains the
extremes $(d_{\min},d_{\max})$ of the corresponding flat interval in the Betti
curve. Finally, we denote with $L$ the lengths of the intervals in $I$. Then,
the most persistent choice for the number of rooms is the one corresponding to
the longest interval in $L$. In our approach, we choose the number of rooms
(and associate an initial set of places to each room) by looking at
“sufficiently persistent” intervals (_i.e.,_ flat regions with length larger
than a threshold). In more detail, we select a subset of $L$, denoted as
$\bar{L}$, that only contains the intervals of size greater than
$\alpha\cdot\max_{l\in L}l$, where $\alpha$ is a user-specified parameter in
$\left[0,1\right]$ that balances between over-segmentation ($\alpha$ closer to
0) and under-segmentation ($\alpha$ closer to 1). From these candidates, we
assign $\delta^{*}$ to be $\delta^{*}=d_{\min}^{*}$, where
$(d_{\min}^{*},d_{\max}^{*})$ are the extremes of the interval in $\bar{L}$
attaining the largest number of connected components. In other words, we
choose the number of rooms to be the largest among all flat intervals that are
sufficiently long (_i.e.,_ longer than $\alpha\cdot\max_{l\in L}l$). As
before, for this choice of $\delta^{*}$, we use the connected components
corresponding to that dilation distance as an initial guess for the room
clustering.121212Note that for $\alpha=1$, the proposed approach simply picks
the longest (most persistent) interval. While intuitive, that choice typically
ends up picking the very first (left-most interval in Fig. 7, with a single
connected component) that persists for small $\delta^{*}$, which is
undesirable in practice.

Assigning Remaining Nodes via Flood-Fill. The connected components produced by
the persistent homology do not account for all places in ${\cal G}_{p}$, as
nodes and edges in the graph disappear depending on the dilation distance. We
assign the remaining place nodes to the putative room via the flood-fill
algorithm, where the expansion queue is ordered by the distance of each edge
to the nearest obstacle, resulting in node connections with the greatest
distance to an obstacle being expanded first. This ensures that every node in
the places graph ${\cal G}_{p}$ is assigned to a room, provided that every
original connected component of ${\cal G}_{p}$ contains at least one room.

######  Remark 7 (Novelty, Advantages, and Limitations).

Many classical 2D room segmentation techniques, such as watershed or
morphology approaches, are also connected to persistent homology [46]. Our
room clustering method provides four key advantages. First, our approach is
able to reason over arbitrary 3D environments, instead of a 2D occupancy grid
(such as [47]). Second, the approach reasons over a sparse set of places and
is extremely computationally and memory efficient in practice; this is in
stark contrast with approaches processing monolithic voxel-based maps (_e.g.,_
[2] reported runtimes of tens of minutes). Third, the approach automatically
computes the dilation distance used to cluster the graph; this allows it to
work across a variety of environments (in Section VI, we report experiments in
both small apartments and student residences). Fourth, it provides a more
solid theoretical framework, compared to the heuristics proposed in related
work [11, 13]. As a downside, our room clustering approach, similarly to
related work relying on geometric reasoning for room segmentation, may fail to
segment rooms without a clear geometric footprint, _e.g.,_ open floor-plans.

Intra-Layer and Inter-Layer Edges. We connect pairs of rooms, say $(i,j)$,
whenever a place in room $i$ shares an edge with a place in room $j$. For each
room, we add edges to all places in the room. We connect all rooms to a single
building node, whose position is the mean of all room centroids.

####  III-D2 Layer 4: Room Classification via Neural Tree

While in the previous section, we (geometrically) clustered places nodes into
different rooms, the approach described in this section assigns a semantic
label (_e.g.,_ kitchen, bedroom) to each room. The key insight that we are
going to use is that objects in a room are typically correlated with the room
type (_e.g.,_ a room that contains refrigerator, oven, and toaster is likely
to be a kitchen, a room with a bathtub and a toilet is likely to be a
bathroom). Edges connecting rooms may also carry information about the room
types (_e.g.,_ a master bedroom is more likely to be connected to a bathroom).
We construct a sub-graph of the 3D scene graph that includes the object and
the room layers and the corresponding intra- and inter-layer edges. Given this
graph, and the object semantic labels, centroids, and bounding boxes (see
Section III-B), we infer the semantic labels of each room.

While room inference could be attacked via standard techniques for inference
in probabilistic graphical models, those techniques typically require
handcrafting or estimating expressions for the factors connecting the nodes,
inducing a probability distribution over the labels in the graph. We instead
rely on more modern techniques that use _graph neural networks_ (GNNs) to
learn a suitable neural message passing function between the nodes to infer
the missing labels. In particular, room classification can be thought of as a
semi-supervised node classification problem [48, 49], which has been
extensively studied in machine learning. We also observe that our problem has
two key features that make it unique. First, the object-room graph is a
_heterogeneous_ graph and contains two kinds of nodes, namely objects and
rooms, as opposed to large, homogeneous social network graphs (one of the key
benchmarks applications in the semi-supervised node classification
literature). Second, the object-room graph is a hierarchical graph (Definition
1), which gives more structure to the problem (_e.g.,_ Proposition
6).131313Note that the result in Proposition 6 is general enough to also
include the building node and perform building classification (_i.e.,_
classify an indoor environment into an office building, hospital, apartment,
etc.). Here we tailor the discussion to the object-room graph for a practical
reason: we lack a large enough dataset for training and testing a building
classification network. The dataset in our experiments includes 90 buildings,
which are mostly residential.  We review a recently proposed GNN architecture,
the _neural tree_ [16], that takes advantage of the hierarchical structure of
the graph and leads to (provably and practically) efficient and accurate
inference.

Neural Tree Overview. While traditional GNNs perform neural message passing on
the edges of the given graph $\mathcal{G}$ (the object-room graph in our
case), the key idea behind the neural trees architecture is to construct a
tree-structured graph from the input graph and perform message passing on the
resulting tree instead of the input graph [16]. This tree-structured graph,
the _H-tree_ , is similar to a tree decomposition, and is such that every node
in it represents either a node or a subset of nodes in the input graph. Trees
are known to be more amenable for message passing (_e.g.,_ the junction tree
algorithm enables exact inference for graphs with small treewidth) [50, 51].
Analogously, the neural tree has been shown to enable strong approximation
results [16] and lead to better classification accuracy in practice (see [16]
and Section VI). We briefly review the construction of the H-tree, the choice
of message passing, and the resulting performance guarantees, and we refer the
reader to [16] for an in-depth discussion.

Constructing the H-Tree. The neural tree performs message passing on the
H-tree, a tree-structured graph constructed from the input graph. Each node in
the H-tree corresponds to a sub-graph of the input graph. These sub-graphs are
arranged hierarchically in the H-tree such that the parent of a node in the
H-tree always corresponds to a larger sub-graph in the input graph. The leaf
nodes in the H-tree correspond to singleton subsets (_i.e.,_ individual nodes)
of the input graph.

The first step to construct an H-tree is to compute a tree decomposition $T$
of the object-room graph. Since the object-room graph is a hierarchical graph,
we use Algorithm 1 to efficiently compute a tree decomposition. The bags in
such a tree decomposition contain either (C1) only room nodes, (C2) only
object nodes, or (C3) object nodes with one room node. To form the H-tree, we
need to further decompose the leaves of the tree decomposition into singleton
nodes. For bags falling in the cases (C1)-(C2), we further decompose the bags
using a tree decomposition of the sub-graphs formed by nodes in the bag, as
described in [16]. For case (C3), we note that the sub-graph is again a
hierarchical graph with one room node, hence we again use Algorithm 1 to
compute a tree decomposition. We form the H-tree by concatenating these tree-
decompositions hierarchically as described in [16].

Message Passing and Node Classification. Message passing on the H-tree
generates embeddings for all the nodes and important sub-graphs of the input
graph. Any of the existing message passing protocols (_e.g.,_ the ones used in
Graph Convolutional Networks (GCN) [48, 52, 53, 48, 54], GraphSAGE [49], or
Graph Attention Networks (GAT) [55, 56, 57]) can be re-purposed to operate on
the neural tree. We provide an ablation of different choices of message
passing protocols and node features in Section VI. After message passing is
complete, the final label for each node is extracted by pooling embeddings
from all leaf nodes in the H-tree corresponding to the same node in the input
graph, as in [16].

One important difference between the H-tree in [16], and the H-tree
constructed for the object-room graph is the heterogeneity of the latter. The
H-tree of a heterogeneous graph will also be heterogeneous, _i.e.,_ the H-tree
will now contain nodes that correspond to various kinds of sub-graphs in the
input object-room graph. Specifically, the H-tree has nodes that correspond to
sub-graphs: (i) containing only room nodes, (ii) containing one room node and
multiple object nodes, (iii) containing only object nodes, and (iv) leaf nodes
which correspond to either an object or a room node. Accordingly, we treat the
neural tree as a heterogeneous graph when performing message passing. Message
passing over heterogeneous graphs can be implemented using off-the-shelf
functionalities in the PyTorch geometric library [58].

Expressiveness of the Neural Tree and Graph Treewidth. The following result,
borrowed from our previous work [16], establishes a connection between the
expressive power of the neural tree and the treewidth of the corresponding
graph.

######  Theorem 8 (Neural Tree Expressiveness, Theorem 7 and Corollary 8 in
[16]).

Call ${\cal F}(\mathcal{G},N)$ the space of functions that can be produced by
applying the neural tree architecture with $N$ parameters to the graph
$\mathcal{G}$. Let $f:[0,1]^{n}\rightarrow[0,1]$ be a function compatible with
a graph $\mathcal{G}$ with $n$ nodes, _i.e.,_ a function that can be written
as
$f({\bm{X}})=\textstyle\sum_{C\in\mathcal{C}\left(\mathcal{G}\right)}\theta_{C}({\bm{x}}_{C})$,
where $\mathcal{C}\left(\mathcal{G}\right)$ denotes the collection of all
maximal cliques in $\mathcal{G}$ and $\theta_{C}$ is some function that maps
features associated to nodes in a clique $C$ to a real number. Let each clique
function $\theta_{c}$ in $f$ be $1$-Lipschitz and be bounded to $[0,1]$. Then,
for any $\epsilon>0$, there exists a function $g\\!\in\\!{\cal
F}(\mathcal{G},N)$ such that $||f\\!-\\!g||_{\infty}\\!<\\!\epsilon$, while
the number of parameters $N$ is bounded by

$N=\textstyle{\cal
O}\left(n\times(\text{tw}\left[\mathcal{J}_{\mathcal{G}}\right]+1)^{2\text{tw}\left[\mathcal{J}_{\mathcal{G}}\right]+3}\times\epsilon^{-(\text{tw}\left[\mathcal{J}_{\mathcal{G}}\right]+1)}\right),$
(10)

where $\text{tw}\left[\mathcal{J}_{\mathcal{G}}\right]$ denotes the treewidth
of the tree-decomposition of $\mathcal{G}$, computed according to Algorithm 1.

While we refer the reader to [16] for a more extensive discussion, the
intuition is that graph-compatible functions can model (the logarithm of) any
probability distribution over the given graph. Hence, Theorem 8 essentially
states that the neural tree can learn any (sufficiently well-behaved)
graphical model over ${\cal G}$, with a number of parameters that scales
exponentially in the graph treewidth, and only linearly in the number of nodes
in the graph. Therefore, for graphs with small treewidth (as the ones of
Proposition 6), we can approximate arbitrary relations between the nodes
without requiring too many parameters. Furthermore, Proposition 6 and
Proposition 3 ensure that we can compute the tree decomposition (and hence the
H-tree) efficiently in practice. Beyond these theoretical results, in Section
VI we show that the use of the neural tree leads to improved accuracy in
practice.

##  IV Persistent Representations: Detecting and Enforcing Loop Closures in 3D
Scene Graphs

Figure 8: Loop closure detection (left) and geometric verification (right). To
find a match, we “descend” the 3D scene graph layers, comparing descriptors.
We then “ascend” the 3D scene graph layers, attempting registration.

The previous section discussed how to estimate the layers of an “odometric” 3D
scene graph as the robot explores an unknown environment. In this section, we
discuss how to use the 3D scene graph to _detect_ loop closures (Section
IV-A), and how to _correct_ the entire 3D scene graph in response to putative
loop closures (Section IV-B).

###  IV-A Loop Closure Detection and Geometric Verification

We augment visual loop closure detection and geometric verification by using
information across multiple layers in the 3D scene graph. Standard approaches
for visual place recognition rely on visual features (_e.g.,_ SIFT, SURF, ORB)
and fast retrieval methods (_e.g.,_ bag of words [59]) to detect loop
closures. Advantageously, the 3D scene graph not only contains visual features
(included in each node of the agent layer), but also additional information
about the semantics of the environment (described by the object layer) and the
geometry and topology of the environment (described by the places layer). In
the following we discuss how to use this additional 3D information to develop
better descriptors for loop closure detection and geometric verification.

####  IV-A1 Top-Down Loop Closure Detection

As mentioned in Section III-B, the agent layer stores visual features for each
keyframe pose along the robot trajectory. We refer to each such poses as
_agent nodes_. Loop closure detection then aims at finding a past agent node
that matches (_i.e.,_ observes the same portion of the scene seen by) the
latest agent node, which corresponds to the current robot pose.

Top-Down Loop Closure Detection Overview. For each agent node, we construct a
hierarchy of descriptors describing statistics of the node’s surroundings,
from low-level appearance to semantics and geometry. At the lowest level, our
hierarchical descriptors include standard DBoW2 appearance descriptors [59].
We augment the appearance descriptor with an object-based descriptor and a
place-based descriptor computed from the objects and places in a sub-graph
surrounding the agent node. We provide details about two choices of
descriptors (hand-crafted and learning-based) below. To detect loop closures,
we compare the hierarchical descriptor of the current (query) node with all
the past agent node hierarchical descriptors, searching for a match. When
comparing descriptors, we walk down the hierarchy of descriptors (from places,
to objects, to appearance descriptors). In particular, we first compare the
places descriptor and —if the descriptor distance is below a threshold— we
move on to comparing object descriptors and then appearance descriptors. If
any of the appearance or object descriptor comparisons return a strong enough
match (_i.e.,_ if two distance between two descriptors is below a threshold),
we perform geometric verification; see Fig. 8 for a summary.

Hand-crafted Scene Graph Descriptors. Our top-down loop closure detection
relies on having descriptors (_i.e.,_ vector embeddings) of the sub-graphs of
objects and places around each agent node. In the conference version [11] of
this paper, we proposed hand-crafted descriptors. In particular, for the
objects, we use the histogram of the semantic labels of the object nodes in
the sub-graph as an object-level descriptor. For the places, we use the
histogram of the distances associated to each place node in the sub-graph as a
place-level descriptor. As shown in [11] and confirmed in Section VI, the
resulting hierarchical descriptors already lead to improved loop closure
detection performance over traditional appearance-based loop closures.
However, these descriptors fail to capture relevant information about objects
and places, _e.g.,_ their spatial layout and connectivity. In the following,
we describe learning-based descriptors that use graph neural networks to
automatically find a suitable embedding for the object and place sub-graphs;
these are observed to further improve loop closure detection performance in
some cases; see Section VI.

Learning-based Scene Graph Descriptors. Given a sub-graph of objects and
places around the agent node, we learn fixed-size embeddings using a Graph
Neural Network (GNN). At a high level, we learn such embeddings from scene
graph datasets, such that the Euclidean distance between descriptors is
smaller if the corresponding agent nodes are spatially close.

In more detail, we learn separate embeddings for the sub-graph of objects and
the sub-graph of places. For every object layer sub-graph, we encode the
bounding-box size and semantic label of each object as node features in a GNN.
For every places layer sub-graph, we encode the distance of the place node to
the nearest obstacle and the number of basis points of the node as node
features. Rather than including absolute node positions in the respective node
features, we assign a weight to each edge $(i,j)$ between nodes $i$ and $j$ as
$w_{ij}=e^{-\left\|x_{i}-x_{j}\right\|}$, where $x_{i}$ and $x_{j}$ are the
positions of nodes $i$ and $j$. This results in a weight in the range $[0,1]$,
where the closer two nodes are, the higher the edge weight. Associating intra-
node distances to edges (rather than using absolute positions as node
features) makes the resulting embedding pose-invariant; this is due to the
fact that the node positions only enter the network in terms of their
distance, which is invariant to rigid transformations. Our GNN model
architecture follows the graph embedding architecture presented in [60], which
consists of multi-layer perceptrons as encoders for node and edge features,
message passing layers, and a graph-level multi-level perception to aggregate
node embeddings into the final graph embedding. We use triplet loss [60] to
train the models and defer the details of constructing triplets and other
model and training parameters to the experiments in Section VI.

####  IV-A2 Bottom-up Geometric Verification

After we have a putative loop closure between our query and match agent nodes
(say $i$ and $j$), we attempt to compute a relative pose between the two by
performing bottom-up geometric verification. Whenever we have a match at a
given layer (_e.g.,_ between appearance descriptors at the agent layer, or
between object descriptors at the object layer), we attempt to register frames
$i$ and $j$. For registering visual features we use standard RANSAC-based
geometric verification as in [34]. If that fails, we attempt registering
objects using TEASER++ [38], discarding loop closures that also fail object
registration. This bottom-up approach has the advantage that putative matches
that fail appearance-based geometric verification (_e.g.,_ due to viewpoint or
illumination changes) can successfully lead to valid loop closures during the
object-based geometric verification. Section VI shows the proposes
hierarchical descriptors improve the quality and quantity of detected loop
closures.

###  IV-B 3D Scene Graph Optimization

This section describes a framework to correct the entire 3D scene graph in
response to putative loop closures. Assume we use the algorithms in Section
III to build an “odometric” 3D scene graph, which drifts over time as it is
built from the odometric trajectory of the robot — we refer to this as the
_frontend (or odometric) 3D scene graph_. Then, our goals here are (i) to
optimize all layers in the 3D scene graph in a consistent manner while
enforcing the detected loop closures (Section IV-A), and (ii) to post-
processes the results to remove redundant sub-graphs corresponding to the
robot visiting the same location multiple times. The resulting 3D scene graph
is what we call a _backend (or optimized) 3D scene graph_ , and we refer to
the module producing such a graph as the _scene graph backend_. Below, we
describe the two main processes implemented by the scene graph backend: a 3D
scene graph optimization (which simultaneously corrects all layers of the
scene graph by optimizing a sparse subset of variables), and an interpolation
and reconciliation step (which recovers the dense geometry and removes
redundant variables); see Fig. 9.

(a) Loop closure detection (b) Optimization

(c) Interpolation (d) Reconciliation

Figure 9: Loop closure detection and optimization: (a) after a loop closure is
detected, (b) we extract and optimize a sub-graph of the 3D scene graph —the
_deformation graph_ — that includes the agent poses, the places, and a subset
of the mesh vertices. (c) We then reconstruct the rest of the graph via
interpolation as in [17], and (d) reconcile overlapping nodes.

3D Scene Graph Optimization. We propose an approach to simultaneously deform
the 3D scene graph layers using an _embedded deformation graph_ [17]. This
approach generalizes the _pose graph and mesh optimization approach_ in [2] as
it also includes the graph of places in the optimization. At a high-level, the
backend optimizes a sparse graph (the embedded deformation graph) built by
downsampling the nodes in the 3D scene graph, and then reconstructs the other
nodes in the scene graph via interpolation as in [17].

Specifically, we form the deformation graph as the sub-graph of the 3D scene
graph that includes (i) the agent layer, consisting of a pose graph that
includes both odometry and loop closures edges, (ii) the _3D mesh control
points_ , _i.e.,_ uniformly subsampled vertices of the 3D mesh (obtained using
the same spatial hashing process described in Section III-A), with edges
connecting control points closer together than a distance
($2.5\text{\,}\mathrm{m}$ in our implementation); (iii) a minimum spanning
tree of the places layer.141414The choice of using the minimum spanning tree
of places is motivated by computational reasons: the use of the spanning tree
increases sparsity of the resulting deformation graph, enabling faster
optimization. By construction, these three layers form a connected sub-graph
(recall the presence of the inter-layer edges discussed in Section III).

Figure 10: Hydra’s functional blocks. We conceptualize three different
functional block groupings: low-level perception, mid-level perception, and
high-level perception in order of increasing latency. Each functional block is
labeled with a number that identifies the “logical” thread that the module
belongs to.

The embedded deformation graph approach associates a local frame (_i.e.,_ a
pose) to each node in the deformation graph and then solves an optimization
problem to adjust the local frames in a way that minimizes deformations
associated to each edge (including loop closures). Let us call ${\cal T}_{a}$,
${\cal T}_{m}$, ${\cal T}_{p}$ the set of poses associated with the agent
layer, the mesh control points, and the places. The poses in ${\cal T}_{a}$
are initially set to be the odometric poses of the robot; the poses in ${\cal
T}_{m}$ are initially set to have identity rotation and translation equal to
the mesh control points’ positions; similarly, the poses in ${\cal T}_{p}$ are
initially set to have identity rotation, and translation equal to the position
of the corresponding places. Each edge represents a relative pose or a
relative position measurement between pair of poses. In particular, the set of
edges is ${\cal E}={\cal E}_{aa}\cup{\cal E}_{mm}\cup{\cal E}_{pp}\cup{\cal
E}_{am}\cup{\cal E}_{ap}\cup{\cal E}_{mp}$, where each subset denotes intra-
layer edges (_e.g.,_ ${\cal E}_{aa}$ contains the edges within the agent
layer), or inter-layer edges (_e.g.,_ ${\cal E}_{am}$ contains the edges
between a robot pose and a mesh control point).151515The set ${\cal E}_{aa}$
contains all the odometric and loop closure measurements relating poses in the
robot trajectory. ${\cal E}_{mm}$ contains all the relative positions between
pairs of nearby mesh control points (expressed in the local frame attached to
one of the control points). Similarly, ${\cal E}_{pp}$, ${\cal E}_{am}$,
${\cal E}_{ap}$ and ${\cal E}_{mp}$ contain the relative positions between
pairs of places, between robot poses and mesh control points visible from that
pose, between robot poses and places near that pose, and between a place and
the corresponding basis points in the mesh, respectively.  Intuitively, the
proposed 3D scene graph optimization finds the set of poses that minimizes the
mismatch with respect to these relative measurements: a small mismatch
corresponds to small deformations of the local geometry of the scene graph and
encourages small errors for the loop closure edges.

We can find an optimal configuration for the poses ${\cal T}={\cal
T}_{a}\cup{\cal T}_{m}\cup{\cal T}_{p}$ in the deformation graph by solving
the following optimization problem:

${\cal
T}^{\star}=\operatorname*{arg\,min}_{{\bm{T}}_{1},{\bm{T}}_{2},\ldots\in{\cal
T}}\sum_{(i,j)\in{\cal
E}}\left\|{\bm{T}}_{i}^{-1}{\bm{T}}_{j}-{\bm{E}}_{ij}\right\|^{2}_{{\bm{\Omega}}_{ij}}$
(11)

where ${\bm{T}}_{i}\in\mathrm{SE}(3)$ and ${\bm{T}}_{j}\in\mathrm{SE}(3)$ are
pairs of 3D poses in ${\cal T}$, ${\bm{E}}_{ij}$ is the relative measurement
associated to each edge $(i,j)\in{\cal E}$ (written as a 3D pose), and for a
matrix ${\bm{M}}$ we use the notation
$\left\|{\bm{M}}\right\|^{2}_{{\bm{\Omega}}}\triangleq\mathrm{tr}\left({\bm{M}}\Omega{\bm{M}}^{\mathsf{T}}\right)$.161616When
the relative measurement involves only a translation, the rotation component
of ${\bm{E}}_{ij}$ is conventionally set to the identity — a suitable choice
of the information matrix ${\bm{\Omega}}_{ij}$ for those measurements ensures
that such rotation component is disregarded by the optimization. The $4\times
4$ positive semidefinite matrix ${\bm{\Omega}}_{ij}$ is chosen as the inverse
of the odometry (or loop closure) covariances for the edges in ${\cal
E}_{aa}$, while it is set to $\mathrm{diag}\left([0\;0\;0\;\omega_{t}]\right)$
for the other relative position measurements, where the zeros cancel out the
rotation component of the residual error
${\bm{T}}_{i}^{-1}{\bm{T}}_{j}-{\bm{E}}_{ij}$, and $\omega_{t}$ is a user-
specified parameter that controls how much deformation we want to allow for
each edge during the optimization.171717The interested reader might find a
step-by-step derivation of (11) (but restricted to the agent layer and the
mesh control points) in [2].

In hindsight, 3D scene graph optimization transforms a subset of the 3D scene
graph into a _factor graph_ [30], where edge potentials need to be minimized.
The expert reader might also realize that (11) is mathematically equivalent to
standard pose graph optimization in SLAM [30], which enables the use of
established off-the-shelf solvers. In particular, we solve (11) using the
Graduated Non-Convexity (GNC) solver in GTSAM [61], which is also able to
reject incorrect loop closures as outliers.

Interpolation and Reconciliation. Once the optimization terminates, the agent
and place nodes are updated with their new (optimized) positions and the full
mesh is interpolated back from its control points according to the deformation
graph approach in [17, 2]. After the 3D scene graph optimization and the
interpolation step, certain portions of the scene graph —corresponding to
areas revisited multiple times by the robot— contain redundant information. To
avoid this redundancy, we merge overlapping nodes. For places nodes, we merge
nodes within a distance threshold ($0.4\text{\,}\mathrm{m}$ in our
implementation). For object nodes we merge nodes if the corresponding objects
have the same semantic label and if one of nodes is contained inside the
bounding box of the other node. After this process is complete, we recompute
the object centroids and bounding boxes from the position of the corresponding
vertices in the optimized mesh. Finally, we recompute the rooms from the graph
of places using the approach in Section III-D.

##  V Thinking Fast and Slow:
the Hydra Architecture

We integrate the algorithms described in this paper into a highly parallelized
_spatial perception system_ , named _Hydra_. Hydra involves a combination of
processes that run at sensor rate (_e.g.,_ feature tracking for visual-
inertial odometry), at sub-second rate (_e.g.,_ mesh and place reconstruction,
object bounding box computation), and at slower rates (_e.g.,_ the scene graph
optimization, whose complexity depends on the map size). Therefore these
processes have to be organized such that slow-but-infrequent computation
(_e.g.,_ scene graph optimization) does not get in the way of faster
processes.

We visualize Hydra in Fig. 10. Each block in the figure denotes an algorithmic
module, matching the discussion in the previous sections. Hydra starts with
fast _early_ perception processes (Fig. 10, left), which perform low-level
perception tasks such as feature detection and tracking (required for visual-
inertial odometry, and executed at frame-rate), 2D semantic segmentation, and
stereo-depth reconstruction (at keyframe rate). The result of early perception
processes are passed to mid-level perception processes (Fig. 10, center).
These include algorithms that incrementally construct (an odometric version
of) the agent layer (_e.g.,_ the visual-inertial odometry backend), the mesh
and places layers, and the object layer. Mid-level perception also includes
the _scene graph frontend_ , which is a module that collects the result of the
other modules into an “unoptimized” scene graph. Finally, the high-level
perception processes perform loop closure detection, execute scene graph
backend optimization, and extract rooms (including both room clustering and
classification).181818While room detection is fast enough to be executed at
keyframe rate, it still operates on the entire graph, hence it is more
suitable as a slow high-level perception process.  This results in a globally
consistent, persistent 3D scene graph.

Hydra runs in real-time on a multi-core CPU. The only module that relies on
GPU computing is the 2D semantic segmentation, which uses a standard off-the-
shelf deep network. The neural tree (Section III-D2) and the GNN-based loop
closure detection (Section IV-A) can be optionally executed on a GPU, but the
forward pass is relatively fast even on a CPU (see Section VI-C4). The fact
that most modules run on CPU has the advantage of (i) leaving the GPU to
learning-oriented components, and (ii) being compatible with the power
limitations imposed by current mobile robots. In the next section, we will
report real-time results with Hydra running on a mobile robot, a Unitree A1
quadruped, with onboard sensing and computation (an NVIDIA Xavier embedded
computer).

##  VI Experiments

The experiments in this section (i) qualitatively and quantitatively compare
the 3D scene graph produced by Hydra to another state-of-the-art 3D scene
graph construction method, SceneGraphFusion [7], (ii) examine the performance
of Hydra in comparison to batch offline methods, _i.e.,_ Kimera [2], (iii)
validate design choices for learned components in our method via ablation
studies, and (iv) present a runtime analysis of Hydra. We also document our
experimental setup, including training details for both the GNN-based loop
closure descriptors and the neural-tree room classification, and datasets
used. Our implementation of Hydra is available at https://github.com/MIT-
SPARK/Hydra.

###  VI-A Datasets

We use four primary datasets for training and evaluation: two simulated
datasets (Matterport3d [31] and uHumans2 [2]) and two real-world datasets
(SidPac and Simmons). In addition, we use the Stanford3D dataset [3] to
motivate neural tree design choices with respect to our initial proposal in
[16].

Matterport3d. We utilize the Matterport3D (MP3D) dataset [31], an RGB-D
dataset consisting of 90 reconstructions of indoor building-scale scenes. We
use the Habitat Simulator [62] to traverse the scenes from the MP3D dataset
and render color imagery, depth, and ground-truth 2D semantic segmentation. We
generate two different 3D scene graphs datasets for the 90 MP3D scenes by
running Hydra on pre-generated trajectories; one for training descriptors and
one for room classification. For training the GNN-based descriptors for loop
closure detection (GNN-LCD), we generate a single trajectory for each scene
through the navigable scene area such that we get coverage of the entire
scene, resulting in 90 scene graphs. For training the room classification
approaches, we generate 5 trajectories for each scene by randomly sampling
navigable positions until a total path length of at least
$100\text{\,}\mathrm{m}$ is reached, resulting in 450 trajectories. When
running Hydra on these 450 trajectories, we save intermediate scene graphs
every 100 timesteps (resulting in roughly 15 scene graphs per trajectory),
giving us 6810 total scene graphs.

uHumans2. The uH2 dataset is a Unity-based simulated dataset [2] that includes
four scenes: a small apartment, an office, a subway station, and an outdoor
neighborhood. For the purposes of this paper, we only use the apartment and
office scenes. The dataset provides visual-inertial data, ground-truth depth,
and 2D semantic segmentation. The dataset also provides ground truth
trajectories that we use for benchmarking.

SidPac. The SidPac dataset is a real dataset collected in a graduate student
residence using a visual-inertial hand-held device. We used a Kinect Azure
camera as the primary collection device, providing color and depth imagery,
with an Intel RealSense T265 rigidly attached to the Kinect to provide an
external odometry source. The dataset consists of two separate recordings,
both of which are used in our previous paper [11]. We only use the first
recording for the purposes of this paper. This first recording covers two
floors of the building (Floors 1 & 3), where we walked through a common room,
a music room, and a recreation room on the first floor of the graduate
residence, went up a stairwell, through a long corridor as well as a student
apartment on the third floor, then finally down another stairwell to revisit
the music room and the common room, ending where we started. These scenes are
particularly challenging given the scale of the scenes (average traversal of
around $400\text{\,}\mathrm{m}$), the prevalence of glass and strong sunlight
in regions of the scenes (causing partial depth estimates from the Kinect),
and feature-poor regions in hallways. We obtain a proxy for the ground-truth
trajectory for the Floor 1 & 3 scene via a hand-tuned pose graph optimization
with additional height priors, to reduce drift and qualitatively match the
building floor plans.

(a) Clearpath Jackal

(b) Unitree A1

Figure 11: (a) The Clearpath Jackal platform used to record one of the Simmons
sequences. The Jackal is equipped with both a RealSense D455 camera and a
Velodyne LIDAR. (b) The Unitree A1 platform used to record the second Simmons
sequence. The A1 is equipped with a RealSense D455 camera and a Microstrain
IMU. Both platforms have an Nvidia Xavier NX embedded computer onboard.

Simmons. The Simmons dataset is a real dataset collected on a single floor of
an undergraduate student residence with a Clearpath Jackal rover and a Unitree
A1 quadruped robot (Fig. 11). The Clearpath Jackal rover uses the RealSense
D455 camera as the primary collection device to provide color and depth
imagery, but the rover is also equipped with a Velodyne to perform LIDAR-based
odometry [63] as an external odometry source. The A1 also uses the RealSense
D455 camera as the primary collection device, but is not equipped with a
Velodyne; instead, it is equipped with an industrial grade Microstrain IMU to
improve the performance of the visual-inertial odometry [34]. The dataset
consists of two recordings, one recorded on the Jackal, and one recorded using
the A1. The Jackal recording covers the rooms scattered throughout half of a
single floor of the building, where the Jackal traverses a distance of around
$500\text{\,}\mathrm{m}$ through mostly student bedrooms, but also a lounge
area, a kitchen, and a laundry room; the rooms are all joined by a long
hallway that spans the full floor. The A1 sequence takes place on one end of
the floor and maps 4 bedrooms, a small lounge, and a section of the hallway
that connects all the rooms. The dataset is challenging due to the visual and
structural similarity across student rooms that have similar layouts and
furnishing. We obtain a proxy for the ground-truth trajectory for the Jackal
using LIDAR-based SLAM, by running LOCUS [63] with flat ground assumption and
LAMP [64] for loop closure corrections. The ground-truth trajectory of the A1
is obtained by registering individual visual keyframes with the visual
keyframes in the Jackal sequence, and then corrected using the proxy ground-
truth of the Jackal sequence.

Stanford3D. We use the 35 human-verified scene graphs from the Stanford 3D
Scene Graph (Stanford3d) dataset [3] to compare the neural tree against
standard graph neural networks for node classification and to assess new
design choices against our initial proposal in [16]. These scene graphs
represent individual residential units, and each consists of building, room,
and object nodes with inter-layer connectivity. We use the same pre-processed
graphs as in [16] where the single-type building nodes (residential) are
removed and additional 4920 intra-layer object edges are added to connect
nearby objects in the same room. This results in 482 room-object graphs, each
containing one room and at least one object per room. The full dataset has 482
room nodes with 15 semantic labels, and 2338 objects with 35 labels.

###  VI-B Experimental Setup

In this section, we first discuss the implementation of Hydra, including our
choice of networks for the 2D semantic segmentation. Then we provide details
regarding the training of our learning-based approaches: the GNN-LCD
descriptors and the neural tree for room classification.

(a) Original Image

(b) OneFormer

(c) HRNet

(d) Mobilenet

Figure 12: (a) A sample RGB image from SidPac Floor 1-3. (b-d) 2D semantic
segmentation from OneFormer [65], HRNet [66], and MobileNetV2 [67]. The
framerate of each approach is overlaid on the images for both the GPU in the
workstation used for evaluation (an Nvidia GTX 3080), as well as for a less
powerful embedded computer (the Nvidia Xavier NX). OneFormer was not tested on
the Xavier NX.

Hydra. To provide 2D semantic segmentation for the real-world datasets, we
compare three different models, all of which use the ADE20k [68] label space.
We use HRNet [66] as a “nominal” semantic segmentation source and MobileNetV2
[67] as a light-weight source of semantics for use on a robot. For both HRNet
and MobileNetV2, we use the pre-trained model from the MIT Scene Parsing
challenge [68] to export a deployable model for our inference toolchain (ONNX
and TensorRT). Additionally, we use a state-of-the-art model, OneFormer [65],
to provide more-accurate-but-slower 2D semantic segmentation. A comparison of
the accuracy and frame-rate of these models is shown in Fig. 12.

For both simulated and real datasets we use Kimera-VIO [34] for visual-
inertial odometry. For SidPac, we fuse the Kimera-VIO estimates with the
output of the RealSense T265 to improve the quality of the odometric
trajectory. For Simmons, we fuse the Kimera-VIO estimates with the odometry
output of [63] to improve the quality of the odometric trajectory for the
sequence recorded by the Jackal platform.

All the remaining blocks in Fig. 10 are implemented in C++, following the
approach described in this paper. In the experiments we primarily use a
workstation with an AMD Ryzen9 3960X with 24 cores and two Nvidia GTX3080s. We
also report timing results on an embedded computer (Nvidia Xavier NX) at the
end of this section, and demonstrate the full Hydra system running on the same
embedded computer onboard the A1 robot; video of the experiment is
available.191919https://youtu.be/AEaBq2-FeY0

GNN-LCD Training. We use the MP3D scene graphs to train loop closure
descriptors for the object and place sub-graphs. We generate a sub-graph
around every agent pose which consists of all nodes and edges within a
provided radius of the parent place node that the agent pose is nearest to. We
adaptively determine the radius for each sub-graph; each sub-graph has a
minimum radius of $3\text{\,}\mathrm{m}$ and is grown until either a minimum
number of nodes (10) or a maximum radius of $5\text{\,}\mathrm{m}$ is reached.
Each object and place sub-graph contains the node and edges features as
described in Section IV-A. The semantic label of each object node is encoded
either using word2vec [69] or a one-hot encoding. We explore the impact of
this label encoding in Section VI-C.

We use triplet loss to train our model. To construct the triplets, we have an
anchor (a candidate sub-graph), and need to find positive and negative
examples to compute the loss. A good proxy for how similar two sub-graphs are
is looking at the spatial overlap of the two sets of nodes of the sub-graphs.
We compute this overlap between the sub-graphs by using IoU over the bounding
boxes that encompass the positions of the nodes of each sub-graph. If the
overlap between two sub-graphs is at least 40 percent, we consider them to be
similar (and candidates for positive examples), otherwise they are considered
negative examples for that particular anchor. Sub-graphs from different scenes
are always considered negative examples. To train our GNN-LCD descriptors, we
use online triplet mining with batch-all triplet loss [70], where we construct
valid triplets for a batch of sub-graph input embeddings and average loss on
the triplets that produce a positive loss.

The message-passing architecture that we selected is GCNConv [48]. While more
expressive or performant architectures exist, few are compatible with the
computational framework used for inference in Hydra (_i.e.,_ ONNX). We split
the dataset by scene; we use 70% of the original 90 scene graphs to train on,
20% of the scene graphs for validation, and the last 10% for testing. Our
learning rate is $5\text{\times}{10}^{-4}$, and we train the object models for
50 epochs and place models for 80 epochs, saving the model when the average
validation error is at a minimum. Each model produces a descriptor of
dimension 64. Other key model parameters are reported in Section A-G.

Neural Tree Training. We train the neural tree and related baselines on two
datasets: Stanford3d and MP3D.

For the 482 object-room scene graphs in the Stanford3D dataset, we train the
neural tree and GNN baselines for the same semi-supervised node classification
task examined in [16], where the architecture has to label a subset of room
and object nodes. The goal of this comparison is to understand the impact of
some design choices (_e.g.,_ heterogeneous graphs, edge features) with respect
to our original proposal in [16] and related work. For this comparison, we
implement the neural tree and baseline approaches with four different message
passing functions: GCN [48], GraphSAGE [49], GAT [55], and GIN [71]. We
consider both homogeneous and heterogeneous graphs; for all nodes, we use
their centroid and bounding box size as node features. In some of our
comparisons, we also examine the use of relative node positions as edge
features, and discard the centroid from the node features. For the GNN
baselines, we construct heterogeneous graphs consisting of two node types:
rooms and objects. For the neural tree, we obtain graphs with four node types:
room cliques, room-object cliques, room leaves, and object leaves; see Section
III-D2. There are few message passing functions that are compatible with both
heterogeneous graphs and edge features; therefore, we compare all
heterogeneous approaches using only GAT [55]. For all tests on Stanford3d, we
report average test accuracy over 100 runs. For each run, we randomly generate
a 70%, 10%, 20% split across all nodes (_i.e.,_ objects and rooms) for
training, validation, and testing, respectively.

For the 6180 scene graphs in the MP3D dataset, we test the neural tree and
baselines for room classification on object-room graphs. The goal of this
experiment is to understand the impact of the node features and the
connectivity between rooms on the accuracy of room classification. We only
consider heterogeneous graphs for this dataset, and as such only use GAT [55]
for message passing. The heterogeneous node types are the same as for
Stanford3D. We use the bounding box size as the base feature for each node,
and use relative positions between centroids as edge features. We also
evaluate the impact of using semantic labels as additional features for the
object nodes using word2vec [69]. The MP3D dataset contains scene graphs with
partially explored rooms. We discard any room nodes where the IoU between the
2D footprint of the places within the room and the 2D footprint of the ground
truth room is less than a specified ratio (60% in our tests). Further details
on the construction and pre-processing of the dataset are provided in Section
A-E. We predict a set of 25 room labels which are provided in Section A-D. For
training, we use the scene graphs from the official 61 training scenes of the
MP3D dataset. For the remaining 29 scenes, we use graphs from two trajectories
of the five total trajectories for validation and the other three trajectories
for testing. For use with Hydra, we select the best-performing heterogeneous
neural tree architecture; this architecture uses bounding box size and
word2vec embeddings of the semantic labels of the object nodes as node
features, as well as relative positions between nodes as edge features.

All training and testing is done using a single Nvidia A10G Tensor Core GPU.
For both datasets, we use cross entropy loss between predicted and ground-
truth labels during training, and save the models with the highest validation
accuracy for testing. All models are implemented using PyTorch 1.12.1 and
PyTorch Geometric 2.2.0 [58]. We base our implementation on our previous open-
source version of the neural tree [16]. We provide additional training
details, including model implementation, timing, and hyper-parameter tuning in
Section A-E.

###  VI-C Results and Ablation Study

We begin this section with a comparison between Hydra and SceneGraphFusion [7]
(Section VI-C1). We then analyze the accuracy and provide an ablation of the
modules in Hydra (Section VI-C2), and show an example of the quality of scene
graph that Hydra is able to produce while running onboard the Unitree A1
(Section VI-C3). Finally, we report a breakdown of the runtime of our system
(Section VI-C4).

(a) Hydra (OneFormer)

(b) SceneGraphFusion

Figure 13: (a) Objects produced by Hydra for the uHumans2 apartment scene. (b) 3D scene graph produced by SceneGraphFusion for the uHumans2 apartment scene. Note that SceneGraphFusion directly infers geometric object relationships, and edges are drawn between objects for each detected relationship. | uH2 Apartment | uH2 Office
---|---|---
| % Correct | % Found | % Correct | % Found
SceneGraphFusion | $25.0$ | $17.9$ | $35.4$ | $36.0$
Hydra (OneFormer) | $68.4$ | $53.6$ | $71.6$ | $60.4$
Hydra (GT) | $\mathbf{95.5}$ | $\mathbf{82.1}$ | $\mathbf{86.5}$ | $\mathbf{83.3}$
TABLE I: Object accuracy for Hydra and SceneGraphFusion. Best results in bold.

####  VI-C1 Comparison against SceneGraphFusion

This section shows that Hydra produces better object maps compared to
SceneGraphFusion [7], while also creating additional layers in the scene graph
(SceneGraphFusion does not estimate rooms or places). We compare our system
with SceneGraphFusion [7] for both the uHumans2 apartment and office scene.
For this comparison, the only learned component used by our system is the off-
the-shelf 2D semantic segmentation network, hence for a fair comparison, we do
not retrain the object label prediction GNN used by SceneGraphFusion [7].
Examples of the produced objects by Hydra and SceneGraphFusion are shown in
Fig. 13. Note that SceneGraphFusion tends to over-segment larger objects
(_e.g.,_ the sofa in the lower right corner of Fig. 13 or the bed in the top
right corner of the same figure), while Hydra tends to under-segment nearby
objects (such as the dining table and chairs in the bottom left of Fig. 13).
For the purposes of a fair comparison, we use OneFormer [65] to provide
semantics for Hydra, and use ground-truth robot poses as input to both
systems. A quantitative comparison is reported in Table I, which also includes
results for Hydra using ground-truth semantics as a upper bound on
performance. We report two metrics. The first, _Percent Correct_ , is the
percent of estimated objects that are within some distance threshold
($0.5\text{\,}\mathrm{m}$) of a ground-truth object (as provided by the
simulator for uHumans2). The second, _Percent Found_ , is the percent of
ground-truth objects that are within some distance threshold (also
$0.5\text{\,}\mathrm{m}$) of a ground-truth object (as provided by the
simulator for uHumans2). As the label space of SceneGraphFusion for objects
does not line up well with the ground-truth object label space, and as
SceneGraphFusion does a poor job predicting object labels for the two scenes
used for comparison (see Fig. 13), we do not take the semantic labels into
account when computing the metrics (as opposed to the stricter metrics used in
Section VI-C2 for examining object accuracy, which do take the semantic labels
of the object into account).

Table I shows that Hydra largely outperforms SceneGraphFusion in terms of both
_Percent Correct_ and _Percent Found_ , even after disregarding the incorrect
semantic labels for the objects produced by SceneGraphFusion. In hindsight,
Hydra can benefit from more powerful 2D segmentation networks and estimate
better objects, while SceneGraphFusion directly attempts to extract semantics
from the 3D reconstruction, which is arguably a harder task and does not
benefit from the large datasets available for 2D image segmentation. The last
row in the table —Hydra (GT)— shows that Hydra’s accuracy can further improve
with a better 2D semantic segmentation. While SceneGraphFusion is less
competitive in terms of object understanding, we remark that it predicts a
richer set of object relationships for each edge between objects [7] (_e.g.,_
_standing on_ , _attached to_) compared to Hydra, which might be also useful
for certain applications.

####  VI-C2 Accuracy Evaluation and Ablation

Here, we examine the accuracy of Hydra, running in real-time, as benchmarked
against the accuracy attained by constructing a 3D scene graph in an offline
manner (_i.e.,_ by running Kimera [2]). We also present a detailed
quantitative analysis to justify key design choices in Hydra. To do this, we
break down this quantitative analysis across the objects, places, and rooms
layers. We examine the impact of (i) the choice of loop closure detection and
2D semantic segmentation on the accuracy of each layer of the 3D scene graph,
and (ii) the impact of model architectures and training parameters on the
accuracy of the room classification network. As such, we first present the
accuracy of the predicted object and place layers by Hydra. We then move to
looking at room classification accuracy, including two ablation studies on the
neural tree and an evaluation of our room clustering approach based on
persistent homology. Finally, we examine the quality of predicted loop
closures when using the proposed hierarchical descriptors.

When benchmarking the accuracy of the layers of Hydra, we consider five
different configurations for odometry estimation and loop closure detection.
The first configuration (“ _GT-Trajectory_ ”) uses ground-truth poses to
incrementally construct the scene graph and disregards loop closures. The
second, third, and fourth configurations (“ _VIO+V-LC_ ”, “ _VIO+SG-LC_ ”, and
“ _VIO+GNN-LC_ ”, respectively) use visual-inertial odometry (VIO) for
odometry estimation and use vision-based loop closures (_VIO+V-LC_), the
proposed hierarchical handcrafted descriptors for scene graph loop closures
(_VIO+SG-LC_), or the proposed hierarchical learned descriptors for scene
graph loop closures (_VIO+GNN-LC_). The last configuration, _VIO_ , uses
visual-inertial odometry without loop closures.

(a) uH2 Apartment (b) uH2 Office

(c) SidPac (d) Simmons Jackal

(e) Simmons A1 (f) Place Metrics

Figure 14: (a-e) Object accuracy metrics for all scenes across different 2D
semantic segmentation sources and different LCD and pose source
configurations. The metrics are averaged over 5 trials for each pair of
configurations (_i.e.,_ there were 5 trials for every combination of 2D
semantics and a pose source). For semantic segmentation sources, “OF” is
Oneformer, “HR” is HRNet and “GT” is the ground truth semantic segmentation
provided by the simulator. Each cell reports the mean and standard deviation.
Values are colored from highest (dark purple, best performance) to lowest
(light orange, worst performance) by mean. (f) Place layer accuracy for all
scenes across different configurations for Hydra. Each bar shows the average
distance error for a given scene and configuration over 5 trials; standard
deviation across the 5 trials is shown as a black error bar.

Accuracy Evaluation: Objects. Figure 14 evaluates the object layer of Hydra
across the five configurations of Hydra, as well as across various 2D semantic
segmentation sources: ground-truth semantics (when available), OneFormer, and
HRNet. For this evaluation, we benchmark against the ground-truth objects from
the underlying Unity simulation for uHumans2. As the recording for the
uHumans2 scenes only explores a portion of the scene, we only compare against
ground-truth objects that have a high enough number of mesh vertices inside
their bounding boxes (40 vertices). For the real datasets, where we do not
have ground-truth objects, we consider the object layer of the batch scene
graph constructed from ground-truth poses and OneFormer-based 2D semantic