PaperlayerView exact revision on arXiv

Source-linked AI summary

Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems

Yulun Tian, Yun Chang, Fernando Herrera Arias, Carlos Nieto-Granda, Jonathan P. How, Luca Carlone

arXiv:2106.14386v2cs.ROcs.CVcs.MA

TL;DR

Kimera-Multi addresses outlier-prone, communication-constrained multi-robot SLAM by combining distributed loop closures, robust distributed PGO, and dense metric-semantic mesh reconstruction. Across simulated, benchmark, and outdoor datasets, it reports robust and accurate distributed estimation, centralized-comparable accuracy, communication savings, and accurate meshes.

  • Problem

    Perceptual aliasing produces incorrect loop closures, while multi-robot SLAM lacks a common reference frame and global outlier-free odometry backbone.

  • Method

    Kimera-Multi combines local Kimera trajectory and mesh estimation with a two-stage distributed GNC-based robust PGO method and local mesh deformation.

  • Results

    Kimera-Multi provides robust and accurate distributed trajectory estimation, centralized-comparable accuracy, accurate metric-semantic meshes, and up to 70% communication reduction versus centralized baselines.

  • Takeaways & Limitations

    The system supports fully distributed collaborative localization and dense metric-semantic mapping while retaining modular options for trajectory-only or non-semantic reconstruction.

Abstract

from arXiv · show

This paper presents Kimera-Multi, the first multi-robot system that (i) is robust and capable of identifying and rejecting incorrect inter and intra-robot loop closures resulting from perceptual aliasing, (ii) is fully distributed and only relies on local (peer-to-peer) communication to achieve distributed localization and mapping, and (iii) builds a globally consistent metric-semantic 3D mesh model of the environment in real-time, where faces of the mesh are annotated with semantic labels. Kimera-Multi is implemented by a team of robots equipped with visual-inertial sensors. Each robot builds a local trajectory estimate and a local mesh using Kimera. When communication is available, robots initiate a distributed place recognition and robust pose graph optimization protocol based on a novel distributed graduated non-convexity algorithm. The proposed protocol allows the robots to improve their local trajectory estimates by leveraging inter-robot loop closures while being robust to outliers. Finally, each robot uses its improved trajectory estimate to correct the local mesh using mesh deformation techniques. We demonstrate Kimera-Multi in photo-realistic simulations, SLAM benchmarking datasets, and challenging outdoor datasets collected using ground robots. Both real and simulated experiments involve long trajectories (e.g., up to 800 meters per robot). The experiments show that Kimera-Multi (i) outperforms the state of the art in terms of robustness and accuracy, (ii) achieves estimation errors comparable to a centralized SLAM system while being fully distributed, (iii) is parsimonious in terms of communication bandwidth, (iv) produces accurate metric-semantic 3D meshes, and (v) is modular and can be also used for standard 3D reconstruction (i.e., without semantic labels) or for trajectory estimation (i.e., without reconstructing a 3D mesh).

I. INTRODUCTION

Kimera-Multi addresses robustness, distribution, and dense metric-semantic mapping in collaborative SLAM. It combines robust trajectory estimation with real-time mesh reconstruction and reports strong performance across simulated and real datasets.

  • Collaborative SLAM must support situational awareness over large-scale environments for applications including search and rescue, transportation, and surveillance.
  • Kimera-Multi enables real-time dense metric-semantic 3D mesh estimation under communication-bandwidth constraints, addressing the limitations of sparse collaborative maps for dense navigation tasks.
  • Perceptual aliasing can create incorrect inter-robot loop closures, while missing a common reference frame and global outlier-free odometry complicates multi-robot estimation.
  • Each robot runs Kimera locally, then performs distributed place recognition, relative pose estimation, robust trajectory estimation, and local mesh deformation when communication is available.
  • The two-stage distributed GNC-based PGO method initializes a global reference frame and solves robust PGO through a distributed RBCD extension.
  • Experiments report robust and accurate distributed estimation, centralized-comparable accuracy, accurate metric-semantic meshes, modularity, and up to 70% communication reduction versus centralized baselines.

II. RELATED WORK

Kimera-Multi builds on multi-robot SLAM and distributed PGO research by combining local metric-semantic mapping with distributed loop closures, robust trajectory optimization, and mesh correction.

  • Prior multi-robot systems predominantly use dense geometric or sparse landmark representations, while Kimera-Multi targets dense metric-semantic mapping.
  • Inter-robot loop closures align robot trajectories in a common reference frame and improve estimates, but existing systems may rely on centralized servers or distributed alternatives.
  • Standard least-squares PGO is vulnerable to outlier loop closures that can severely affect trajectory estimation.
  • Its decentralized architecture contains local Kimera, distributed loop closure detection, robust distributed PGO, and local mesh optimization, with communication limited to the middle two modules.
  • Kimera-Multi uses Kimera-VIO for visual-inertial odometry and Kimera-Semantics for dense metric-semantic mesh reconstruction.
  • Robots exchange bag-of-words descriptors, geometrically verify candidate matches, and pass accepted relative poses to robust distributed trajectory estimation.
  • The system can produce dense metric meshes without semantic labels or optimized trajectories without dense reconstruction, reflecting its modular design.

IV. DISTRIBUTED LOOP CLOSURE DETECTION

Kimera-Multi detects inter-robot loop closures through descriptor exchange and geometric verification, then uses robust distributed optimization to incorporate the resulting relative poses.

  • When robots enter communication range, they exchange bag-of-words descriptors for newly collected keyframes and search for matching observations.
  • Candidate correspondences are verified with five-point or three-point relative-pose estimation combined with RANSAC.
  • A loop closure is accepted after geometric verification succeeds with more than five correspondences and is forwarded to trajectory estimation.
  • The trajectory backend uses odometry and putative loop closures, with robust initialization followed by distributed GNC-based optimization.

A. Background: Graduated Non-Convexity

Graduated non-convexity (GNC) gradually transforms an initially convex surrogate into the target robust cost while alternating trajectory, measurement-weight, and control-parameter updates. Distributed GNC initializes weights and coordinates robot updates to produce globally framed optimized trajectories.

  • GNC principle: GNC optimizes a sequence of increasingly non-convex surrogate cost functions, beginning near a convex function and converging toward the original robust cost.This reduces sensitivity to poor initial guesses associated with directly optimizing non-convex robust costs.
  • Distributed GNC: Distributed GNC initializes all loop-closure weights to one and repeatedly performs trajectory, weight, and parameter updates until convergence.Trajectory updates use RBCD for a default of T = 15 iterations per communication phase.
  • GNC iterations: The method alternates weighted variable updates, closed-form measurement-weight updates, and control-parameter updates toward the target surrogate regime.Variable updates solve weighted least squares, while weights depend on residuals, the surrogate, and the TLS threshold.
  • Robust initialization: Robust distributed initialization rejects outlier frame alignments generated by incorrect loop closures while constructing a robot-level spanning tree.Inlier loop closures produce mutually similar frame-alignment estimates; outliers can be rejected with GNC.

B. Robust Distributed Initialization

Kimera-Multi initializes each robot’s locally chained trajectory in a shared global frame by robustly estimating pairwise coordinate-frame transformations from inter-robot loop closures. A spanning tree propagates these pairwise alignments using only local communication.

  • Pairwise coordinate frame estimation: Each robot first chains odometry in its local reference frame, then requires relative poses between local frames to express trajectories globally.The local reference frame of each robot coincides with its first pose.
  • Pairwise coordinate frame estimation: Each inter-robot loop closure provides a candidate alignment between two local frames, while inlier-generated candidates are expected to agree mutually.The method estimates the reliable relative transformation by robust pose averaging with a TLS cost.
  • Pairwise coordinate frame estimation: For each robot pair, one robot solves the robust pose-averaging problem locally with GNC and transmits the solution to the other robot.The implementation uses a diagonal covariance with standard deviations of 0.1 rad for rotation and 0.5 m for translation.
  • Multi-robot coordinate frame estimation: A robot-level spanning tree propagates pairwise transformations along unique paths so every robot can express its initial trajectory in the global frame.The tree can be grown incrementally using local communication, and initial trajectories need not be shared.

C. Robust Distributed Pose-Graph Optimization

Kimera-Multi solves robust distributed pose-graph optimization by combining TLS-weighted loop-closure residuals with decentralized RBCD trajectory updates. Alternating approximate variable and independent weight updates enables robots to reject outlier loop closures while optimizing trajectories from odometry and loop closures.

  • Problem formulation: The robust distributed PGO problem optimizes all global robot poses using odometry and putative intra-robot and inter-robot loop closures.Loop-closure residuals use a robust TLS cost, while odometric measurements use a standard quadratic cost because they are treated as outlier-free.
  • Variable update: GNC replaces robust loop-closure costs with weighted squared residuals during each variable-update step, yielding a standard weighted PGO problem.The resulting problem is solved with RBCD in the distributed implementation.
  • Variable update: RBCD decentralizes trajectory estimation: each robot updates its own trajectory using partial exchanges with neighboring robots that share inter-robot loop closures.Robots receive only neighboring public poses, preserving privacy and reducing communication.
  • Distributed optimization: Distributed variable updates may use approximate solutions rather than full convergence because first-order distributed solvers can make exact convergence slow.The approach relaxes the centralized GNC requirement that every variable update be solved to full convergence.
  • Weight update: GNC weight updates are independently and in parallel computed for each residual, including internal and involved inter-robot loop closures.This independence makes the weight-update stage suitable for fully distributed computation.

VI. LOCAL MESH OPTIMIZATION

Kimera-Multi locally corrects each robot’s mesh after distributed pose-graph optimization by deforming a unified graph that couples mesh vertices with trajectory keyframes. The optimization anchors keyframes to updated poses while preserving local mesh and keyframe–mesh geometry, then propagates deformation to the complete mesh without changing semantic labels.

  • Creating the deformation graph: The deformation graph combines a simplified mesh with trajectory keyframes, linking adjacent mesh vertices and mesh vertices observed by keyframes.The mesh is subsampled online through octree-based vertex clustering before graph construction.
  • Creating the deformation graph: Each mesh node receives a local transformation initialized from its Kimera-Semantics position, while keyframe poses start from Kimera-VIO estimates.Mesh transformations start with identity rotations; keyframe vertices encode the visual-inertial trajectory.
  • Optimizing the deformation graph: The local optimization anchors keyframe poses to distributed PGO estimates while enforcing rigidity among neighboring mesh vertices and between keyframes and observed mesh vertices.Levenberg–Marquardt optimization solves the deformation problem at each robot.
  • Mesh recovery: After optimization, the remaining mesh vertices are recovered as affine transformations of deformation-graph nodes using distance-based weights normalized to sum to one.The weighting uses the distance to the k + 1 nearest node, with k = 4.
  • Mesh recovery: Semantic labels attached to Kimera-Semantics mesh nodes remain unchanged during mesh deformation.The deformation corrects geometry while preserving semantic annotations.

VII. EXPERIMENTS

The experiments evaluate Kimera-Multi’s robustness, estimation quality, communication usage, and modularity across numerical, simulated, benchmark, and real-world settings. The evaluation includes challenging ground-robot datasets in addition to synthetic and photo-realistic scenarios.

  • Evaluation scope: The evaluation covers robust trajectory and metric-semantic mesh estimation, communication efficiency, and modularity.The experiments are organized around robustness analysis, simulation and benchmarking datasets, and challenging real-world datasets.
  • Evaluation scope: Single-robot synthetic PGO solvers are compared across 10 Monte Carlo runs.These tests provide a controlled robustness analysis before the multi-robot experiments.

A. PGO Robustness Analysis

The robustness analysis shows that GNC is more reliable than least-squares and PCM-based alternatives, while multi-robot GNC requires robust initialization to avoid outlier-contaminated starting estimates. The proposed D-GNC configuration consistently performs best across the tested synthetic settings.

  • Overall findings: D-GNC achieves the best overall performance across synthetic robust trajectory-estimation experiments and is not sensitive to parameter tuning.The analysis also shows that robust initialization is important and that PCM can miss correct loop closures.
  • Single-robot tests: GNC yields smaller absolute trajectory error than the compared methods across the single-robot outlier spectrum.Standard LM is not robust even with 10% outliers, while PCM can reject inliers and increase error.
  • Single-robot tests: Under 70% outliers, PCM has larger errors at high probability thresholds because it cannot reject all outliers.Applying GNC after PCM helps, but GNC alone remains best across the tested thresholds.
  • Multi-robot tests: In three-robot tests, naïve GNC initialization fails more often as outlier frequency increases because wrong inter-robot loop closures contaminate the initial guess.At 70% outliers, the naïve initial guess is almost always contaminated by incorrect loop closures.
  • Multi-robot tests: Robust initialization followed by GNC consistently outperforms naïve initialization, PCM, and other tested techniques in multi-robot problems.PCM avoids catastrophic failures but retains low recall for correct loop closures.
  • Qualitative comparison: With 70% outlier loop closures and a 99% probability threshold, PCM fails to reject all outliers and produces a distorted trajectory estimate.Naïve GNC also fails to recover inlier loop closures under the same qualitative comparison.

B. Evaluation in Simulation and Benchmarking Datasets

Simulation and benchmarking experiments evaluate distributed trajectory estimation, mesh quality, communication, and runtime across photo-realistic and EuRoC environments. D-GNC achieves lower trajectory errors, communication can fall substantially versus centralized transmission, and early stopping preserves accuracy while reducing runtime.

  • Experimental settings: The evaluation uses three photo-realistic simulation environments and three real-world EuRoC environments, including three-robot and five-robot scenarios.Experiments assume robots remain continuously within communication range, so inter-robot loop closures are established at the earliest possible time.
  • Trajectory estimation: D-GNC achieves significantly lower absolute trajectory errors than L2 and PCM on all evaluated datasets.Centralized GNC is reported as a reference in Table I.
  • Trajectory estimation: Early stopping does not significantly affect trajectory-estimation accuracy and remains comparable to centralized GNC.This supports terminating distributed optimization before full convergence when runtime matters.
  • Communication and runtime: Kimera-Multi reduces communication by 70% on the Vicon Room 2 dataset compared with a centralized system transmitting all keypoints and descriptors.Place recognition and geometric verification consume more communication than distributed PGO.
  • Communication and runtime: Machine Hall requires more data transmission because its larger robot count and loose loop-closure thresholds increase geometric-verification traffic.The paper identifies communication-efficient place recognition and geometric verification as avenues for further reduction.
  • Communication and runtime: Early stopping keeps runtime close to the centralized counterpart without heavily compromising estimation accuracy, although full D-GNC convergence takes longer on larger datasets.Runtime is approximately 10 seconds on the smaller Vicon Room datasets.
  • Metric-semantic mesh quality: Metric mesh quality is evaluated against ground-truth point clouds or meshes using nearest-neighbor metric error and correctly labeled-point percentages.The evaluation covers individual robot meshes and merged global meshes.
  • Metric-semantic mesh quality: Figure 9 compares mesh error in meters for Kimera-Semantics and Kimera-Multi’s local mesh optimization on EuRoC sequences.The comparison targets the metric accuracy of the reconstructed meshes.

C. Evaluation in Large-Scale Outdoor Datasets

Kimera-Multi was evaluated on challenging outdoor datasets with long multi-robot trajectories, where robust distributed optimization rejected many outlier loop closures and reduced trajectory drift. The resulting system also produced optimized metric-semantic meshes and matched centralized performance on Medfield.

  • Mesh reconstruction: Kimera-Multi generated dense metric-semantic meshes for the simulated Camp and City scenes and a dense metric mesh for Medfield.The mesh evaluation compares Kimera-Semantics with Kimera-Multi’s LMO, while the scene figures show three-robot reconstructions.
  • Experimental setup: The outdoor evaluation used a Clearpath Jackal UGV with RGB-D and inertial sensing across Medfield and Stata trajectories exceeding 500 meters.The longest Medfield trajectory was 860 meters.
  • Loop closures: D-GNC successfully rejected many outlier loop closures on both outdoor datasets, although Stata had significantly fewer inter-robot closures than Medfield.The table reports accepted loop closures over total detected loop closures, including outliers.
  • Trajectory estimation: Kimera-VIO accumulated approximately 15-25 m of drift on each Medfield trajectory, while Kimera-Multi significantly reduced the error through loop closures and robust distributed PGO.The drift was mostly vertical, and the global pose graph contained 15650 poses; D-GNC used 100 RBCD iterations in 53 seconds.
  • Trajectory estimation: On Medfield, Kimera-Multi achieved the same performance as the centralized solver despite using distributed pose graph optimization.The evaluation used end-to-end position error because ground-truth trajectories were unavailable; this metric is not equivalent to ATE.
  • Stata dataset: The Stata evaluation was more challenging because it lacked sufficient inter-robot loop closures, leading to higher Kimera-VIO drift and requiring distributed optimization with 11184 poses.Default D-GNC used 120 RBCD iterations and took 50 seconds on Stata.

VIII. CONCLUSION

The conclusion presents Kimera-Multi as a fully distributed system combining robust pose-graph optimization with dense metric-semantic mapping. Across simulations, indoor benchmarks, and outdoor datasets, it delivered robust trajectory estimates, accurate meshes, and communication reductions relative to centralized systems.

  • System: Kimera-Multi estimates dense metric-semantic 3D meshes collaboratively while each robot independently builds local trajectories and meshes in real time.Local maps are corrected through mesh deformation after jointly optimizing the trajectories.
  • Robust optimization: D-GNC is a novel two-stage robust distributed pose-graph optimization method that outperforms prior outlier-rejection methods.It serves as Kimera-Multi’s estimation backbone.
  • Evaluation: Kimera-Multi’s evaluation covered photo-realistic simulations, indoor SLAM benchmarks, and large-scale outdoor datasets.The experiments assessed trajectory estimation, mesh reconstruction, and communication behavior.
  • Results: The results demonstrated robust and accurate distributed trajectory estimation, improved metric-semantic mesh accuracy, and significant communication reductions compared with centralized baselines.These findings summarize the paper’s reported outcomes across the evaluation settings.
  • Modularity: The system’s modular design permits trajectory estimation or standard 3D reconstruction when other components are disabled or replaced.The cited conclusion also describes the full metric-semantic SLAM configuration.
Loading 2106.14386v2…