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

In plain words

A robot with a camera and a motion sensor (an IMU) can build a map of a room as it moves. Most such maps are either a cloud of geometry or a list of objects. This paper proposes a map with layers, from the raw surface of the room at the bottom up to the whole building at the top. Each layer holds things at a different level of meaning: surfaces, objects and people, places, rooms, and the building. Links between them record facts such as which room a chair is in, or where a person was at a given time.

Kimera is the system that fills in this map from the robot's sensors. The bottom layer is built live while the robot moves, and it is corrected when the robot recognises a place it has been before. The upper layers are worked out after the run. A planner can then ask for "the nearest place in the kitchen" instead of raw coordinates, and it can search the high levels first.

TL;DR

• Kimera is a SLAM system that builds a 3D Dynamic Scene Graph (DSG) with five layers from stereo or RGB-D images and IMU data §2 §3.
• Its online part is visual-inertial odometry with fixed-lag smoothing, a per-frame and multi-frame mesh, a semantic TSDF mesh, and a loop-closure step that deforms the mesh §3.1 §3.2 §3.3 §3.4. The upper layers are built after the run §3 §8.
• Author-reported: VIO and pose-graph accuracy on EuRoC is competitive §4.2 tab. 1; hierarchical planning on the scene graph is far faster than planning on the ESDF in the paper's office test §6.3 tab. 9.
• The code repository at the linked commit holds only the README, a license and docs. The module code is in other repositories, which were not checked README.md.

Problem

Robots are often given geometric commands, and their internal maps are either partial geometry or collections of objects. They lack a representation that supports reasoning at several levels of abstraction §1. Geometric reconstruction (SLAM) and semantic segmentation have largely been developed separately §1.

The paper targets dynamic indoor and outdoor scenes with people in them §3.5 §4.1. Its inputs are stereo or RGB-D frames, IMU samples, and 2-D semantic labels per keyframe, with the labels needed for dynamic masking and object parsing §3 §3.3. The experiments use a simulator and a few hand-held real recordings, so the real-world evidence is qualitative §4.10.

Method

Dynamic Scene Graph (§2)

A DSG is a layered directed graph. Nodes are spatial concepts (for example objects, rooms and agents), and edges are pairwise spatio-temporal relations such as "agent A is in room B at time t" §2. Every node has a unique ID, and each node carries spatial attributes, such as coordinates, shape or a bounding box §2. A single-storey indoor DSG has five layers §2:

1. Metric-semantic mesh (§2.1). Vertices carry a 3-D position, a normal, an RGB colour and a panoptic label. Static geometry only; dynamic objects are stored separately §2.1.
2. Objects and agents (§2.2). Objects are static, non-structural nodes with a pose, a bounding box and a semantic class. Agents are time-varying nodes (humans and the robot) with a pose graph over time, a mesh model and a class §2.2.
3. Places and structures (§2.3). Places are free-space positions whose edges mean straight-line traversability, forming a topological map. Structures (walls, floor, ceiling and pillars) have pose, bounding box and class §2.3.
4. Rooms (§2.4). Rooms have pose, bounding box and class. Adjacent rooms share an edge when a door connects them, and each room links to the places it contains §2.4.
5. Building (§2.5). A single node for the building, linked to all rooms §2.5.

The paper argues the layer choice from planning queries and notes that layers can be added on top or inserted between existing ones §2.6.

Kimera pipeline (§3)

Kimera has two parts §3. Kimera-Core builds the metric-semantic mesh in real time. It has four modules: Kimera-VIO, Kimera-PGMO (or Kimera-RPGO, which does not deform the mesh), Kimera-Mesher and Kimera-Semantics §3. Kimera-DSG builds layers 2 to 5 from that mesh: objects, humans and the building parser. Layer 1 is the Kimera-Core mesh §3.

The agent nodes and the metric-semantic mesh are built incrementally. Objects, places, structures and rooms are built when the run ends §3. The paper runs these modules in five threads at different rates §3.

# Online part of Kimera, one sensor update  [§3]
# Input:  stereo or RGB-D frame, IMU samples, 2-D semantic labels
# Output: pose, per-frame mesh, global metric-semantic mesh

def kimera_online_step(frame, imu_samples, labels):
    # Front-end: preintegrate IMU between keyframes  [§3.1]
    preintegrated = preintegrate(imu_samples)
    # Track features; detect and verify at keyframes  [§3.1]
    tracks = track_features(frame, preintegrated)
    # Back-end: fixed-lag smoothing over the VIO horizon  [§3.1]
    pose = fixed_lag_smoother(tracks, preintegrated)
    # Per-frame mesh from a 2-D Delaunay triangulation  [§3.2]
    frame_mesh = delaunay_back_project(tracks, pose)
    # Multi-frame mesh: fuse per-frame meshes over the horizon  [§3.2]
    horizon_mesh = fuse_frame_meshes(frame_mesh, pose)
    # Global semantic TSDF mesh, one update per keyframe  [§3.3]
    global_mesh = bundled_raycast(frame, labels, pose)
    # Loop closures correct the trajectory and the mesh  [§3.4]
    apply_loop_closures(pose, global_mesh)
    return pose, frame_mesh, horizon_mesh, global_mesh

Kimera-VIO (§3.1)

The estimator is keyframe-based maximum a posteriori with IMU preintegration on the manifold and a structureless vision model §3.1. The front-end detects Shi-Tomasi corners and tracks them with the Lucas-Kanade tracker §3.1. The tracker is seeded with an IMU-predicted rotational flow, because the robot moves through crowds §3.1. Feature detection, stereo matching and geometric verification run at keyframes only; tracking runs at every frame §3.1.

Geometric verification uses RANSAC. Mono uses 5-point and stereo uses 3-point RANSAC. The code also offers 2-point and 1-point variants that use the IMU rotation to prune matches. The paper's default is 2-point for stereo and 1-point for mono §3.1.

The back-end is a factor graph solved with iSAM2 in GTSAM §3.1. Each feature's 3-D point is triangulated with DLT at each iteration and then eliminated analytically from the state §3.1. Points behind the camera, with too little parallax, or with large reprojection error are removed before elimination §3.1. States that leave the smoothing horizon are marginalised §3.1. The horizon is fixed-lag in typical use §3.1.

Kimera-Mesher (§3.2)

The per-frame mesh is a 2-D Delaunay triangulation of the tracked features in the current keyframe, back-projected with the VIO back-end's 3-D point estimates §3.2. It can be labelled by texturing it with 2-D labels §3.2.

The multi-frame mesh merges per-frame meshes over the VIO horizon §3.2. The steps are: add new vertices and faces that are missing from the merged mesh; update all vertex positions to the latest VIO estimates; remove vertices and faces that belong to features outside the horizon §3.2. When planar surfaces are detected, regularity factors are added to the VIO back-end, which couples mesh regularisation to the estimator §3.2.

Kimera-Semantics (§3.3)

Kimera-Semantics adapts bundled raycasting on a Voxblox TSDF §3.3. At each keyframe, depth comes from dense stereo (semi-global matching) or from RGB-D §3.3. The mesh is extracted by marching cubes §3.3.

Each 3-D point gets a label from the 2-D segmentation. For each bundle of rays, a label-probability vector is formed from label frequencies §3.3. Labels are carried along a ray only in the band of voxels near the surface, within the TSDF truncation distance. Empty-space voxels get no label update §3.3. Each voxel is updated with a Bayesian rule, and its most likely label is taken at the end §3.3. The paper says the resulting mesh is more accurate than the multi-frame mesh but slower, about 0.1 s per update §3.3.

Kimera-PGMO: loop closure and mesh deformation (§3.4)

Loop candidates come from DBoW2 with ORB descriptors. Each is checked with mono 5-point or stereo 3-point RANSAC §3.4. The paper's outlier filter is a modified PCM (Pairwise Consistent Measurement set maximisation) for a single robot and online use §3.4. The odometry edges and loop closures are stored separately. A loop passes an odometry cycle check, and then a pairwise consistency test against earlier loops §3.4. The paper keeps the largest consistent set with a maximum-clique solver §3.4.

# Incremental PCM check for one new loop closure  [§3.4]
# Odometry edges and loop closures are stored separately  [§3.4]

def accept_loop(new_loop, odometry, loops, adjacency):
    # Odometry check: the loop must close the odometry cycle  [§3.4]
    if not cycle_consistent(new_loop, odometry):
        return
    # One new row and column; compare only with earlier loops  [§3.4]
    adjacency.add_row_and_column(new_loop)
    for old_loop in loops:
        consistent = pairwise_consistent(new_loop, old_loop)
        adjacency.set_pair(new_loop, old_loop, consistent)
    loops.append(new_loop)
    # Largest mutually consistent set, by maximum clique  [§3.4]
    inliers = maximum_clique(adjacency)
    # The inliers join the pose graph with the odometry  [§3.4]
    pose_graph.add(odometry, inliers)

After outlier rejection, Kimera-PGMO optimises one deformation graph that holds both the robot poses and a simplified mesh §3.4. Mesh vertices carry an SE(3) transform; the pose vertices are the robot's trajectory nodes §3.4. Mesh vertices are simplified by octree clustering, with voxel size between 1 and 4 m §3.4. Pose vertices connect to mesh vertices that are visible from their camera §3.4. The optimisation minimises three terms: odometry and loop closure residuals, local rigidity between mesh vertices, and local rigidity between each pose and its visible mesh vertices eq. 1. The translation-only terms use weights with zero rotation weight §3.4. The objective is rewritten as a pose graph over a combined vertex set, which the paper solves with GTSAM eq. 5 eq. 6.

# Deform one mesh vertex from the optimised deformation graph  [§3.4]
# nodes: positions g_j; rotations R_j and translations t_j are solved

def deform_vertex(vertex, nodes, rotations, translations, k=4):
    # Support radius: distance to the (k+1)-th nearest node  [eq. 8]
    radius = distance_to_rank(vertex, nodes, k + 1)
    # Raw weight per node  [eq. 8]
    weights = [(1 - distance(vertex, g) / radius) ** 2 for g in nodes]
    # Normalise so the weights sum to one  [eq. 8]
    weights = normalise(weights)
    # Blend the node transforms, applied to the vertex  [eq. 7]
    deformed = 0
    for j in range(len(nodes)):
        deformed += weights[j] * (
            rotations[j] @ (vertex - nodes[j]) + translations[j]
        )
    return deformed

Kimera-RPGO is the same robust front-end with a pose-graph optimiser only; the paper calls Kimera-PGMO a strict generalisation of it §3.4. Kimera-PGMO optimises a larger graph, so it runs slower: 728 pose and 1031 mesh nodes on EuRoC V1_01 §3.4, with up to 140 ms against 50 ms for Kimera-RPGO §4.8.

Humans: Kimera-Humans (§3.5)

Humans are detected by GraphCMR, which regresses SMPL vertices from a crop around each 2-D human segmentation §3.5. The crop is passed through the network, the weak-perspective camera is used to project the mesh, and PnP refines the camera pose using reprojection error §3.5. Depth at the pelvis gives the translation, and the result is transformed to the world frame with the VIO pose §3.5.

Each human gets a pose graph of pelvis positions. Consecutive poses are linked by a zero-velocity prior with a permissive noise model, and the GraphCMR estimate is a prior factor §3.5. Data association assigns a new detection to the nearest existing graph, provided three checks pass §3.5. The centroid speed must stay under 3 m/s, joints must move less than 3 m, and the mean difference of the 8 SMPL shape parameters must be below 0.1 §3.5. Detections are rejected if the box approaches the image border or is 30 pixels or smaller §3.5. Graphs with fewer than 10 nodes are pruned §3.5. Human pixels are used in dynamic masking: only the free-space part of the depth ray is integrated for them §3.5 §4.3.

Kimera-Objects (§3.6)

Objects with unknown shape come from the semantic mesh. The mesh is split by class, then clustered with Euclidean clustering at 0.1 m, which the paper gives as twice a 0.05 m voxel size §3.6. Each cluster gets a centroid, an axis-aligned canonical orientation and a bounding box §3.6.

Known shapes are fitted in three steps. Harris keypoints (radius 0.15 m, NMS threshold 1e-4) are taken from the CAD model and from the object mesh. Every CAD keypoint is matched to every mesh keypoint. TEASER++ then finds the alignment that is robust to the many wrong matches §3.6.

Building parser (§3.7)

Places come from a topological graph built on the ESDF §3.7. Each object and agent pose is linked to its nearest place §3.7.

Structures are the walls, floor and ceiling classes of the semantic mesh. Each gets a centroid, a world-aligned orientation and a bounding box. Each wall vertex votes for a room. It looks at places within 0.5 m in the direction of its normal, and each vote is weighted by the dot product of the normal with the vector to the place, divided by the squared length of that vector §3.7.

Rooms come from an ESDF slice cut 0.3 m below the detected ceiling. Values above 0.2 m are kept, so small openings between rooms close. Each remaining 2-D region is one room §3.7. Places that fall in no region, such as those near walls or in door openings, take the majority label of their neighbours in the topological graph, repeated until every place is labelled §3.7.

Debugging tools (§3.8)

The paper points to an evaluation suite, Kimera-Evaluation, for VIO, SLAM and reconstruction, and to Jenkins continuous integration on EuRoC with evo §3.8.

Hyperparameters

Only values the paper gives are listed. Values from the code are not checked here (see Paper vs code).

Results

Author-reported. Setups: EuRoC V1 and V2 sequences with ground truth §4.1; simulated UnityHumans and UnityHumansTwo with ground-truth geometry and semantics §4.1; real recordings for qualitative results only §4.10. The paper gives no CPU model for the main timings. GraphCMR timing is on an RTX 2080 Ti §4.8. TX2 timings use MAXN mode §4.9.

Headline numbers:

• VIO on EuRoC is "competitive" with the compared pipelines §4.2 tab. 1. On V1_1, Kimera-VIO's ATE is 0.05 m against 0.09 m for OKVIS and 0.34 m for MSCKF tab. 1.
• Global mesh RMSE on EuRoC V1/V2 is 0.353 to 0.480 m, and the multi-frame mesh is 0.374 to 0.530 m tab. 3.
• Dynamic masking with 60 humans and GT poses: mesh RMSE 0.19 m without, 0.06 m with tab. 5.
• Simulation, GT depth and DVIO poses: mIoU 80.03%, accuracy 94.50%. Dense stereo drops this to mIoU 57.23%, accuracy 80.74% tab. 6.
• Kimera-PGMO, Neighborhood with 0 humans: semantic accuracy 62.9% to 93.3%, geometric RMSE 0.27 m to 0.09 m tab. 7.
• Humans, UnityHumansTwo Office with 6 humans: localisation error 1.87 m for a single image, 0.63 m with pose graph and beta check tab. 8.
• Room parsing precision and recall: Office 99% and 99%; Subway 87% and 92%; WhiteOwl 93% and 94%; School 91% and 90%; Apartment 68% and 61%; BldgThirtyOne 71% and 67% §4.7.
• Timing on the paper's platform §4.8: IMU preintegration about 40 µs; feature tracking 7.5 ms per frame; keyframe processing 51 ms; per-frame mesh under 7 ms; multi-frame mesh 15 ms; back-end under 60 ms; loop detection up to 180 ms; Kimera-RPGO up to 50 ms; Kimera-PGMO up to 140 ms. Building a Subway DSG takes about 30 min; the ESDF, the topological map and object segmentation are the slowest steps §4.8.
• TX2 (MAXN): Kimera-Semantics updates average 65.8 ms on EuRoC §4.9.
• Hierarchical path planning on a scaled office (1 to 6 buildings): total 0.340 s to 1.244 s, against 420 s to 21,389 s on the ESDF tab. 9.

Full tables follow. The source hides one of two values in each VINS cell of table 1 (see Pitfalls).

Table 1. RMSE ATE in metres on EuRoC tab. 1. VINS-Mono and VINS-LC show the second value in each cell.

Fixed-lag methods are OKVIS, MSCKF, ROVIO, VINS-Mono and Kimera-VIO. Loop-closure methods are Basalt, ORB-SLAM3, VINS-LC, Kimera-RPGO and Kimera-PGMO tab. 1.

Table 2. RMSE ATE in metres against the loop-closure threshold α, V1_01 tab. 2.

Table 3. Multi-frame and global mesh error, RMSE in metres, ICP threshold 1.0 m tab. 3. The table caption says "completeness"; the text describes accuracy and completeness.

Table 4. RMSE ATE in metres for 5-point, 2-point and DVIO VIO, and for the loop-closure variants. Drift in percent for DVIO in brackets tab. 4.

The source shows "×" in some cells without defining it in the caption tab. 4.

Table 5. Mesh RMSE in metres with and without dynamic masking (DM), using GT poses and DVIO poses tab. 5.

Table 6. Kimera-Semantics on the simulated dataset without humans, with ground-truth or dense-stereo depth and ground-truth or DVIO poses tab. 6.

Table 7. Kimera-Semantics against Kimera-PGMO, both with dynamic masking and DVIO poses. Geometric RMSE in metres, semantic accuracy in percent. EuRoC has no semantic labels tab. 7.

Table 8. Localisation error in metres for humans (five variants) and objects (unknown shape, and known shape for couch, chair and car) tab. 8.

The paper's text says the beta check helps everywhere except the Apartment scene, where the best results come with no proposed technique §4.6.

Table 9. Hierarchical semantic path planning with A* (buildings B, rooms R, places P), against A* on the ESDF. Timing in seconds; nodes and edges as P nodes / P edges; path lengths in metres tab. 9.

Paper vs code

Repository: https://github.com/MIT-SPARK/Kimera, BSD-2-Clause, linked from the paper's supplementary material [supplementary]. Cloned at commit fc33760636068ea0b2dca5e9d7958ee9a03888b1.

The clone contains a README, LICENSE.BSD and docs/ only. It has no source files, configuration, tests or parameter files README.md LICENSE.BSD. The README names three module repositories: Kimera-VIO, Kimera-RPGO and Kimera-Semantics README.md. The Kimera-Mesher bullet links to the Kimera-VIO repository README.md. These modules were not cloned or read. So no algorithm or hyperparameter here has been checked against code. No discrepancies can be stated from the code.

Data: the README lists uHumans2 at http://web.mit.edu/sparklab/datasets/uHumans2/ README.md. The paper releases the dataset but gives no URL §1 §4.1.

The paper names a second repository, Kimera-Evaluation, as its evaluation tool §3.8. It was not checked.

Pitfalls

• Voxel size conflicts within the paper. §3.6 gives a Kimera-Semantics voxel size of 0.05 m (clustering threshold is twice that, 0.1 m). §4.9 says the default Kimera-Semantics settings use a voxel size of 0.1 m on TX2. Pick one and test; the paper does not say which configuration produced tab. 6–8.
• The mesh speed-up claim does not match the timings. §4.3 says the multi-frame mesh takes "two orders of magnitude less time" than the global mesh. §3.3 gives about 0.1 s for the global mesh, and §4.8 gives 15 ms for the multi-frame mesh. That is about 7 times, not 100.
• Loop-closure mesh deformation is underspecified. Eq. 8 has no cutoff at d_max, so farther nodes still get non-zero weight unless the sum is restricted to the nearest nodes. The paper does not say which nodes enter the sum. The weights ω_R and ω_t in eq. 1 are not given, and the chi-squared significance level in PCM is not given §3.4.
• Eq. 5 and 6 rewrite a deformation graph as a pose graph. The paper notes that the original embedded deformation graph equals pose-graph optimisation only when rotations are used instead of affine transforms §3.4. Implement eq. 1 and eq. 7 as written, and do not assume the rewritten pose graph is exact.
• Layers 3 to 5 are not online. They are built after the run, and the conclusion lists incremental construction as future work §3 §8. The abstract says tens of objects and humans are parsed "in minutes". §4.8 gives about 30 min for the Subway scene, the largest in the paper.
• Table 1 values are hidden in the source. Each VINS cell uses \vinVersion{first}{second}, which shows only the second value tab. 1. The first value is not shown in the paper. The card uses the displayed value.
• Table 3 caption and text disagree. The caption says "completeness", while the text describes accuracy and completeness, and the columns are RMSE tab. 3 §4.3. Check the definition before comparing with other work.
• Trajectory alignment choice. The paper aligns with SE(3), not Sim(3), and says Sim(3) would give a smaller error for Kimera §4.2. Compare with other VIO papers only under the same alignment.
• Dynamic masking needs human labels. Human pixels come from the 2-D segmentation, and only the free-space part of the depth ray is used for them §3.5. The paper does not name the label set.
• Beta and pose-graph checks are tuned. The 0.1 beta threshold, 3 m/s speed bound, 3 m joint bound and 10-node pruning rule come from the paper's own experiments §3.5. No sensitivity analysis appears in the sections read.
• Human noise model values are missing. The zero-velocity prior is described as "permissive", with no numbers §3.5.
• Known-shape fitting needs a CAD model per class. The experiments use couch, chair and car models §4.6. Classes without a model get a centroid and a bounding box §3.6.
• Real-life sensor setup. One RealSense emitter was disabled to allow feature tracking, and the cameras were calibrated with Kalibr §4.1. Without this the infrared pattern corrupts tracking.
• Real-life results are qualitative. The paper reports spurious object detections, over-segmented rooms in BldgThirtyOne, and a fragmented apartment room layout §4.7 §4.10.
• Timing hardware is not stated for the main timings. Only GraphCMR's RTX 2080 Ti and the TX2 are named §4.8 §4.9.
• Section and table numbers come from source order. They were counted from the LaTeX source (sagej class), not from a rendered PDF.

Lineage

• Builds on: Kimera, the ICRA 2020 open-source library, which provides Kimera-Core. The paper calls it its previous work §1. arXiv:1910.02490 is listed in the README README.md.
• Builds on: 3D Dynamic Scene Graphs (RSS 2020) for the DSG representation §1. The paper also uses the UnityHumans dataset introduced there §4.1. arXiv:2002.06289 is listed in the README README.md.
• Uses: Voxblox for TSDF and ESDF §3.3 §3.7; DBoW2 for loop candidates §3.4; GTSAM and iSAM2 for estimation §3.1; PCM for outlier rejection §3.4; GraphCMR and SMPL for humans §3.5; TEASER++ for known objects §3.6.
• Compared against: OKVIS, MSCKF, ROVIO, VINS-Mono, Basalt and ORB-SLAM3 on EuRoC tab. 1. SVO-GTSAM was compared in the earlier ICRA 2020 paper §4.2.
• Superseded by: not stated in the paper or in the content of this wiki.