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

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

This paper presents a multi-robot Simultaneous Localization and Mapping (SLAM) system named Kimera-Multi. Real-world applications require multi-robot SLAM systems to be robust, capable of identifying and rejecting erroneous inter-robot and intra-robot loop closures caused by perceptual confusion, and to rely solely on local (point-to-point) communication for distributed localization and mapping; furthermore, capturing semantic information is also essential for advanced decision-making and spatial artificial intelligence. Kimera-Multi is achieved through a team of robots equipped with visual-inertial sensors. Each robot uses Kimera to construct local trajectory estimates and local meshes.

When communication is available, the robots initiate a distributed location recognition and robust pose graph optimization protocol based on a distributed graduated nonconvexity algorithm. The proposed protocol allows robots to improve their local trajectory estimates by utilizing inter-robot loop closures while resisting the interference of outliers. Finally, each robot uses its improved trajectory estimate to correct its local mesh using mesh deformation techniques. Kimera-Multi has been demonstrated in realistic simulations, SLAM benchmark datasets, and challenging outdoor datasets collected with ground robots.

Experiments show that Kimera-Multi: 1) outperforms existing technologies in terms of robustness and accuracy; 2) achieves estimation errors comparable to centralized SLAM systems while being fully distributed; 3) has low communication bandwidth; 4) generates accurate metric-semantic 3D meshes; 5) is modular and can also be used for standard applications. The Kimera-Multi system described in this paper can be used for multi-robot simultaneous localization and mapping (SLAM), characterized by robustness and full distribution, and is capable of capturing semantic information to enable advanced decision-making and spatial artificial intelligence. The system is implemented through a team of robots equipped with visual-inertial sensors, where each robot can construct local trajectory estimates and local meshes, and utilize distributed algorithms for robust pose graph optimization and inter-robot loop closure.

Ultimately, Kimera-Multi can generate accurate metric-semantic 3D meshes. Additionally, the system can also be used for 3D reconstruction without semantic labels or solely for trajectory estimation. Keywords: multi-robot systems, simultaneous localization and mapping, robotic vision systems.

Multi-robot collaborative Simultaneous Localization and Mapping (SLAM) is an important topic in robotics research, as it can provide situational awareness of large-scale environments over extended periods. This capability is fundamental for various applications such as factory automation, search and rescue, intelligent transportation, planetary exploration, and military and civilian surveillance and monitoring.

This paper advances the state-of-the-art in collaborative SLAM by developing a system that estimates real-time dense metric-semantic 3D mesh models under realistic communication bandwidth constraints. The 3D mesh captures complete and dense geometric information of the environment in which the robots operate. Additionally, by annotating the mesh with human-understandable semantic labels (such as ‘building’, ‘road’, and ‘object’), our system provides a high-level abstraction of the environment, which is crucial for enabling next-generation spatial awareness or spatial artificial intelligence and advanced decision-making.

In single-robot SLAM, metric-semantic models have been utilized in pioneering works such as SLAM++ and SemanticFusion. Recent works include systems that build metric-semantic 3D models in real-time using multi-core CPUs, such as Kimera and Voxblox++. In multi-robot SLAM, many existing systems rely on sparse landmarks (e.g., [6] and [7]). While these systems perform well in collaborative localization, they do not provide a complete solution for dense mapping, which is essential for critical navigation tasks (such as collision avoidance and motion planning).

On the other hand, recent multi-robot systems have started to leverage semantic information to aid collaborative SLAM, but the underlying representations remain sparse (e.g., objects). Recent works have adopted dense semantic segmentation, but this method can only perform pairwise matching of local maps.

Overall, there is currently no complete multi-robot system available for dense metric-semantic SLAM, partly due to the additional communication and computational costs involved in building such models. This paper addresses this gap by developing a collaborative metric-semantic SLAM system. Moreover, the proposed system is fully distributed and operates under real communication constraints.

Additionally, this paper aims to enhance the robustness of collaborative SLAM in challenging real-world environments. In practice, perceptual confusion often leads to erroneous inter-robot data associations (i.e., outlier loop closures) due to similar scenes, resulting in catastrophic failures of standard estimation backends. In multi-robot SLAM, this issue is further complicated by the lack of a common reference frame and a globally outlier-free milestone backbone. While recent works have proposed several robust estimation techniques for collaborative SLAM, they either rely too heavily on initialization or adopt heuristic search methods, leading to low recall rates (i.e., missing correct loop closures).

This paper addresses this challenge by developing a robust distributed backend based on graduated nonconvexity (GNC) (“Graduated nonconvexity” refers to a technique used in optimization that controllably transforms the objective function into a non-convex function to better explore the solution space. This technique is often used in deep learning because the objective function is highly non-convex and difficult to optimize. By gradually introducing non-convexity, the optimization process can escape local minima and find better solutions. This method is sometimes referred to as “curriculum learning”).

Contribution: The main contribution of this paper is Kimera-Multi, a fully distributed system for multi-robot dense metric-semantic SLAM. Our system enables a group of robots to collaboratively estimate a semantically annotated 3D mesh model of the environment in real-time. Each robot runs Kimera to process onboard visual-inertial sensor data and obtain local trajectory and 3D mesh estimates. When communication is available, a fully distributed process is triggered to perform inter-robot location recognition, relative pose estimation, and robust distributed trajectory estimation. From the jointly optimized trajectory estimates, each robot performs real-time local mesh deformation to correct local mapping drift and improve the consistency of the global map. The implementation of Kimera-Multi is modular and allows for the disabling or replacement of different components. Figure 1 shows the application of Kimera-Multi on three collaborative SLAM datasets collected in Medfield, Massachusetts, USA.

The second technical contribution of this paper is a new, outlier-robust distributed pose graph optimization (PGO) method that serves as the distributed backend for Kimera-Multi. The first stage estimates the relative transformations between the coordinate systems of the robots using GNC, initializing the local trajectories of the robots in a global reference frame. This approach is robust to outlier loop closures and is very efficient as it does not require iterative communication. The second stage solves the complete robust PGO problem. To this end, we propose a GNC distributed extension built on top of a state-of-the-art Riemannian block-coordinate descent (RBCD) solver. Compared to previous techniques, our method achieves more robust and accurate trajectory estimates and is less sensitive to parameter tuning.

Our third contribution is extensive experimental evaluation. We quantitatively evaluate Kimera-Multi on a series of large-scale realistic simulations and SLAM benchmark datasets. Additionally, we demonstrate the performance of Kimera-Multi using challenging real-world datasets collected by autonomous ground robots. Our results show that Kimera-Multi: 1) provides more robust and accurate distributed trajectory estimates compared to alternative techniques employed in previous works; 2) achieves estimation accuracy similar to centralized systems while being fully distributed; 3) exhibits efficient communication, achieving up to 70% reduction in communication compared to baseline centralized systems; 4) constructs accurate metric-semantic 3D meshes; 5) is modular and can disable or replace different functionalities according to user needs, such as mesh reconstruction and semantic annotation.

Innovations compared to previous research: An earlier version of this paper introduced Kimera-Multi in [18], but this paper expands on that work and adds two new contributions. First, we developed an outlier-robust fully distributed trajectory estimation method based on GNC. In [18], we used an incremental, log-likelihood based pairwise consistency maximization (PCM) to reject outlier loop closures. However, PCM relies on a graph-theoretic formulation and, in practice, depends on heuristic maximal clique searches, leading to low recall rates. In this paper, we adopt a more efficient and accurate GNC method that does not require iterative communication and guarantees optimal solutions. The second contribution is our development of a distributed PGO algorithm that only spans a small subset of variables in each polling step, significantly reducing communication load. Additionally, we introduce a novel block-coordinate descent solver, which is one of the fastest solvers in distributed Riemannian optimization.

Another innovation compared to previous work is that we introduce metric-semantic 3D mesh capture and annotation into multi-robot SLAM for the first time. Our system can correct local mapping drift and generate dense and semantically annotated global maps, which is crucial for many key applications. Our system not only contains traditional geometric information but also includes semantic labels, enabling high-level abstractions of the environment. This information can be used for further computer vision tasks such as object detection, semantic scene understanding, and decision-making.

In summary, this paper presents Kimera-Multi, a fully distributed multi-robot collaborative metric-semantic SLAM system, which is a real-time, efficient, and robust solution that can provide situational awareness of large-scale environments over extended periods. We developed a novel outlier-robust distributed PGO algorithm and introduced metric-semantic 3D mesh annotation into multi-robot SLAM. Kimera-Multi has been extensively evaluated on large-scale simulations and real-world datasets, demonstrating outstanding performance. In the future, we plan to extend Kimera-Multi to more domains and apply it to even more challenging and practical scenarios.

In the field of single-robot SLAM, recent research trends have focused on combining metric and semantic information to build maps. Kimera is a framework that provides accurate real-time visual-inertial odometry (VIO) and lightweight mesh reconstruction, and the multi-robot metric-semantic SLAM system presented in this paper is built upon Kimera.

In multi-robot SLAM, most methods concentrate on dense geometric representations (such as occupancy maps) or sparse landmark maps. Recently, some works have begun to incorporate sparse objects or dense semantic information into multi-robot perception. This paper extends the earlier work on Kimera-Multi and adds a new outlier-robust distributed PGO algorithm and other experimental evaluations.

Inter-robot loop closures are crucial for aligning the trajectories of robots to a common reference frame and improving their trajectory estimates. Traditional centralized visual SLAM systems perform loop closure detection by sending global descriptors and local visual features to a central server. Recent works have developed distributed and communication-efficient methods for loop closure detection between robots.

PGO is a commonly used estimation method in modern SLAM systems. Centralized PGO methods collect all measurements and compute the trajectory estimates of all robots at a central site. Meanwhile, there are many efforts to design distributed PGO methods. The standard least-squares PGO formulation is susceptible to biased loop closures, so it must be improved, such as through RANSAC, M-estimation, IRLS, etc. This paper proposes a novel distributed GNC method and demonstrates its superiority in robustness and accuracy over PCM.

In Kimera-Multi, each robot runs a fully decentralized metric-semantic SLAM system as shown in Figure 2. The system consists of four main modules: 1) local (single-robot) Kimera; 2) distributed loop closure detection; 3) robust distributed trajectory estimation through PGO; and 4) local mesh optimization (LMO). Among these modules, distributed loop closure detection and robust distributed PGO are the only modules involving communication between robots. Figure 3 shows the data flow between these modules.

Kimera [1] runs on each robot and provides real-time local trajectory and mesh estimates. Specifically, Kimera-VIO [16] serves as the VIO module, processing raw stereo images and inertial measurement unit (IMU) data to obtain the robot’s odometric trajectory estimates.

Kimera-Semantics [16] processes depth images (which may be obtained from RGB-D cameras or through stereo matching) and 2D semantic segmentation [76], using VIO pose estimates to produce dense metric-semantic 3D meshes. Additionally, Kimera-VIO uses ORB features and DBoW2 [37] to compute a BoW representation for each keyframe, which is used for distributed loop closure detection. Interested readers can refer to [1] and [16] for more technical details.

Distributed loop closure detection (see Section IV) is performed when two robots, α and β, are within communication range. The robots exchange BoW descriptors of the keyframes they have collected since their last rendezvous. When the robots find a pair of matching descriptors (typically corresponding to observations of the same location), they perform relative pose estimation using standard GV techniques. The relative pose corresponds to a hypothesized loop closure between the robots and is used in the robust distributed trajectory estimation process.

Robust distributed trajectory estimation (see Section V) optimally estimates the trajectories of all robots in a global reference frame by using odometric measurements from Kimera-VIO and all detected hypothesized loop closures to solve the PGO problem collaboratively. Initially, a robust initialization scheme is used to find the rough relative transformations between the reference frames of the robots. Then, a robust optimization process based on the distributed GNC [15] is employed using the RBCD solver [17] to simultaneously select inlier loops and recover optimal trajectory estimates. Compared to the incremental PCM technique [13] used in the conference version of Kimera-Multi [18], our new method makes trajectory estimation more robust and accurate while being less sensitive to parameter tuning.

LMO (see Section VI) is performed after the robust distributed trajectory estimation phase. This module executes local processing steps to deform each robot’s mesh to be consistent with the trajectory estimates from the distributed PGO.

Kimera-Multi is implemented in C++ and uses the Robot Operating System [77] as the communication layer between the robots and the modules executed on each robot. The system runs online using CPUs and is modular, allowing for the replacement or removal of modules. For instance, if no semantic labels are available, the system can still generate dense metric meshes, or if the user does not require dense reconstruction, it can generate only optimized trajectories.

IV. Distributed Loop Closure Detection

This section describes the front end of Kimera-Multi, which is responsible for detecting loop closures between the robots. The information flow is illustrated in Figure 3. When communication becomes available, robot α initiates the distributed loop closure detection process by sending the global descriptors of all new keyframes since the last communication to robot β. We implement these descriptors as BoW vectors using the DBoW2 library [37]. Upon receiving the BoW vector, robot β searches for candidate matches in its own keyframes, with a normalized visual similarity score exceeding a threshold (≥0.1 in our code). When a potential loop closure is identified, the robot performs standard GV to estimate the relative transformation between the two matching keyframes. In our implementation, robot β first requests the 3D keypoints and associated descriptors of the matching keyframes from robot α (see Figure 3). Subsequently, robot β utilizes nearest neighbor search implemented in OpenCV [78] to match the presumed correspondences of the two sets of feature descriptors. From the assumed correspondences, robot β attempts to compute the relative transformation using Nistér’s five-point method [79] and Arun’s three-point method [80] combined with RANSAC [56]. Both techniques are implemented in the OpenGV library. If GV is successful with more than five correspondences, the loop closure is accepted and sent to the robust distributed trajectory estimation module.

V. Robust Distributed Trajectory Estimation

In Kimera-Multi, the robots collaboratively estimate their trajectories by solving the PGO problem, utilizing odometric measurements from the entire team and loop closures between the robots. Some of these loop closures may be outliers (e.g., caused by perceptual confusion), so we need a robust method to solve the PGO. In previous versions of Kimera-Multi [18], we used an incremental variant of PCM [13] for outlier rejection, performing trajectory estimation through maximal clique computation. However, even with parallelization [82], the running time of precise maximal clique searches exceeds 10 seconds in graphs with 700 loop closures, which is impractical for our application. Therefore, in practice, PCM must rely on heuristic maximal clique algorithms, often resulting in poor recall rates, as shown in Section VII-A.

In this paper, we propose a novel distributed robust trajectory estimation method based on GNC [15]. The key idea of GNC is to start from a convex approximation of a robust cost function and gradually introduce non-convexity to prevent convergence to false solutions. Although GNC generally does not require initial guesses [15], it has been observed that global solvers for 3D SLAM become too slow in the presence of outliers [74]. Therefore, in [74], local optimization is performed in each iteration of GNC (starting from an initial guess without outliers), which has proven to be very effective. In single-robot SLAM, a no-outlier initial guess can be obtained by linking odometric measurements. In the multi-robot case, there are no odometric measurements between different robot poses, so the challenge is to construct an initial guess that is insensitive to outliers.

To address the above challenge, the proposed DGNC method involves two stages. In the first stage (see Section V-B), we use an outlier-robust and communication-efficient method to initialize the robot trajectories in a global reference frame. In the second stage (see Section V-C), we develop a fully distributed process to execute GNC, using the RBCD distributed solver as a subroutine. Algorithm 1 provides the pseudocode for D-GNC.

A. Background: GNC

We first briefly review GNC [15],[84]. One challenge associated with traditional M-estimation [85],[86] is that the robust cost function ρ used may be highly non-convex, making local search techniques sensitive to initial guesses. The key idea behind GNC is to optimize a series of easier (i.e., less non-convex) surrogate cost functions that gradually converge to the original robust cost function. Each surrogate problem adopts the same form as classical M-estimation, and the algorithmic flow is as follows:

Original reference paper section three

(https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9686955)A. Background: GNC

(https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9686955)B. Robust Distributed Initialization

(https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9686955)C. Robust Distributed PGO

(https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9686955)

Kimera-Multi is a distributed multi-robot collaborative SLAM system, for each individual robot, they estimate their local poses and local meshes using Kimera’s Kimera-VIO and Kimera-Semantics modules through visual-inertial sensors. When two robots can communicate with each other, distributed location recognition detection and pose graph optimization functionalities based on the distributed graduated nonconvexity algorithm are initialized, achieving robustness against outliers through loop closure detection between robots, ultimately improving the accuracy of pose estimation and the precision of mesh reconstruction.

The core algorithm flow of Kimera-Multi is: distributed loop closure detection, distributed trajectory optimization, and finally local mesh map optimization;

Distributed loop closure detection: When robots can communicate with each other, they send the global descriptors of all keyframes from the last communication to the current, converting the descriptors into BoW vectors and performing keyframe intra-search matching. Upon successful matching, the relative transformation relationship between the two keyframes is calculated;

Distributed trajectory optimization: The trajectory optimization part mainly uses PGO to address the issue of outliers, where the authors propose a distributed robust trajectory estimation method based on GNC, allowing the cost function to start from a convex approximation and gradually transition to non-convex, thereby avoiding spurious solutions;

Local mesh map optimization: This primarily utilizes the loop closure detection from the distributed PGO to optimize each robot’s mesh map; here, the authors propose a mesh optimization method based on deformation graphs.

The effects of Kimera-Multi in simulation datasets and real-world scenarios; the final results achieved by Kimera-Multi are as follows:

1. Provides robust and accurate trajectory estimates in fully distributed settings; 2. Offers accuracy in 3D Mesh metric-semantic estimation compared to Kimera; 3. Significant improvement in communication efficiency.

Kimera-Multi was posted on arXiv in 2019 and is one of the earlier works. Following Kimera-Multi, the Luca group combined scene understanding and 3D Dynamic Scene Graph in multi-robot collaborative SLAM systems, leading to this year’s work “Hydra-Multi: Collaborative Online Construction of 3D Scene Graphs with Multi-Robot Teams”.

Kimera is a C++ library for real-time metric-semantic simultaneous localization and mapping, which uses camera images and inertial data to build a semantically annotated 3D mesh of the environment. Kimera is modular, ROS-enabled, and runs on a CPU.

Kimera comprises four modules:

A fast and accurate Visual Inertial Odometry (VIO) pipeline (Kimera-VIO) A full SLAM implementation based on Robust Pose Graph Optimization (Kimera-RPGO) A per-frame and multi-frame 3D mesh generator (Kimera-Mesher) And a generator of semantically annotated 3D meshes (Kimera-Semantics)

Click on the following links to install Kimera’s modules and get started! It is very easy to install!

Kimera-Semantics (Chart)

This paper presents Kimera-Multi, a distributed multi-robot system for robust and dense metric-semantic SLAM. Our system advances state-of-the-art multi-robot perception technology by estimating 3D mesh models that can capture both dense geometry and semantic information of the environment. Kimera-Multi is fully distributed: each robot can navigate independently, using Kimera to estimate local trajectories and meshes in real-time. When communication becomes available, the robots perform local communication to detect loop closures and execute distributed trajectory estimation. Based on globally optimized trajectory estimates, each robot performs LMO to optimize its local map. We also introduce D-GNC, a novel two-stage method for robust distributed PGO, which serves as the estimation backbone for Kimera-Multi and outperforms previous outlier rejection methods.

We conducted extensive evaluations of Kimera-Multi using photorealistic simulations, indoor SLAM benchmark datasets, and large-scale outdoor datasets. Our results demonstrate that Kimera-Multi provides robust and accurate trajectory estimates while being fully distributed; it can more accurately estimate 3D meshes with metric-semantic information compared to Kimera input; and Kimera-Multi exhibits high communication efficiency, achieving significant reductions in communication compared to baseline centralized systems.