Sage Journals: Discover world-class research

Abstract

The exploration of large-scale unknown environments can benefit from the deployment of multiple robots for collaborative mapping. Each robot explores a section of the environment and communicates onboard pose estimates and maps to a central server to build an optimized global multi-robot map. Naturally, inconsistencies can arise between onboard and server estimates due to onboard odometry drift, failures, or degeneracies. The mapping server can correct and overcome such failure cases using computationally expensive operations such as inter-robot loop closure detection and multi-modal mapping. However, the individual robots do not benefit from the collaborative map if the mapping server provides no feedback. Although server updates from the multi-robot map can greatly alleviate the robotic mission strategically, most existing work lacks them, due to their associated computational and bandwidth-related costs. Motivated by this challenge, this paper proposes a novel collaborative mapping framework that enables global mapping consistency among robots and the mapping server. In particular, we propose graph spectral analysis, at different spatial scales, to detect structural differences between robot and server graphs, and to generate necessary constraints for the individual robot pose graphs. Our approach specifically finds the nodes that correspond to the drift’s origin rather than the nodes where the error becomes too large. We thoroughly analyze and validate our proposed framework using several real-world multi-robot field deployments where we show improvements of the onboard system up to 90% and can recover the onboard estimation from localization failures and even from the degeneracies within its estimation.

Keywords

Multi-robot mapping spectral graph theory

1. Introduction

Over recent years, an abundance of localization and mapping frameworks have been proposed and successfully deployed in various robotic scenarios. As part of this research, many traditional SLAM challenges have been fully or partially addressed. Despite this development, new challenges readily arise with the need for more robotic autonomy and the deployment of heterogeneous robotic teams in large-scale environments. In particular, an increase in the number of deployed robots and autonomy requires a higher degree of robustness and efficiency. At the same time, scalability and persistence across all systems become a pertaining issue. While it is difficult to maintain a consistent estimate of the environment across all employed systems, it is an essential prerequisite for operating robotic teams in applications like disaster response or search and rescue.

With the recent advent of high-bandwidth mobile networks such as 5G networks, collaborative robotic approaches have received increased attention in the robotics community due to their improved practical feasibility. A promising research direction is to employ a centralized mapping approach. Centralized servers running in the local network or a remote cloud environment have more computational capacity than individual robots. Therefore, they can perform expensive operations such as global optimizations, loop closing, and exploitation of all available sensor data to improve accuracy and overcome onboard failures.

Most collaborative mapping approaches focus on building accurate maps on the server and ignore the use of global multi-robot information to provide localization corrections to individual robots. Especially in centralized settings without feedback, pose estimation discrepancies may arise between robots during large missions leading to severe drift between robot and server maps resulting in increased optimization time at the server for collaborative mapping. Therefore, it is desirable to provide additional constraints to improve onboard estimation and collaborative mapping performance for large-scale multi-robot missions.

Furthermore, multi-robot missions often deploy a heterogeneous set of robots, for example, aerial and ground robots, which additionally might rely on heterogeneous sensory systems. Carrying a diverse set of multi-modal sensors onboard and effectively utilizing different algorithms for, for example, localization and mapping, can be highly beneficial for the deployment as it becomes more flexible and robust. However, no common layer sharing data to improve pose estimation and mapping estimates among the employed robots is readily available with diverse sensory systems. Hence, a sensor modality-invariant approach that can incorporate and communicate relevant consistency information among robots while maintaining low network bandwidth requirements is essential for large-scale multi-robot field deployments.

This paper proposes a novel multi-robot pose graph consistency approach independent of the underlying robot pose estimation processes. Our proposed approach relies only on a sparse abstraction of the estimated poses in SE(3). Moreover, the framework operates in the graph spectral domain of the pose graphs to identify structural anomalies in the individual robot pose graphs using a multi-scale analysis. By examining the structural components of the pose graphs at different scales, our system identifies discrepancies in the local and coarser neighborhoods and adds corresponding constraints to improve the pose estimation accuracy of individual robots and make the individual robot and collaborative server maps consistent with each other. The key contributions of this paper are:

• Graph spectral analysis of pose graphs to identify discrepancies between onboard and server pose graphs.

• Automatic adaptive inference of multi-scale constraints to correct onboard estimation failures.

• Comparison against current state-of-the-art approaches on datasets and a thorough quantitative analysis on large-scale multi-robot field deployments are presented to validate the proposed approach.

2. Related work

In this section, we review the state-of-the-art collaborative multi-robot localization and mapping approaches as well as the current applications of graph signal processing and degeneracy and failure detection.

2.1. Collaborative multi-robot mapping

Collaborative multi-robot approaches can be distinguished into centralized (Deutsch et al., 2016; Karrer et al., 2018; Schmuck and Chli, 2019) and distributed solutions (Cunningham et al., 2013; Dong et al., 2015). Deutsch et al. (2016) proposed a vision-based centralized multi-robot SLAM approach where a mapping server performs loop closures and replaces robot pose graphs with corrected graphs. A similar approach was proposed by Schmuck and Chli (2019) in which robots send local maps to a mapping server which then returns optimized keyframes and landmarks to each robot to include in their onboard optimizations, thus increasing the bandwidth requirements for real-world robot deployments. The work of Van Opdenbosch and Steinbach (2019) proposes an encoding and decoding of visual features during the transmission of the maps to reduce the required bandwidth. CoSLAM (Zou and Tan, 2013) proposes to make use of GPU computing to circumvent the need for large computational processes and improve the speed of onboard optimizing tasks, hence requiring a GPU onboard individual robots. Zhang et al. (2021) presents an incremental and centralized solver for multi-robot SLAM based on a Bayes tree.

Different from vision-only approaches, LAMP (Chang et al., 2022; Ebadi et al., 2020) proposes a large-scale collaborative multi-modal SLAM framework. However, their proposed approach does not provide any pose corrections from the centralized server to the individual robots.

In contrast to centralized approaches, distributed approaches require each robot to run a full onboard SLAM solution (Dong et al., 2015) and share marginalized information with other robots (Cunningham et al., 2013), thus making full information available to each robot. Additionally, they have the advantage of scaling well to large swarms of robotic systems (Ziegler et al., 2021) but typically increase the onboard compute requirements significantly. Tian and How (2022) proposes a distributed algorithm for communication-efficient rotation and translation estimation in multi-robot SLAM by collaboratively solving a Laplacian system.

A crucial aspect of multi-robot SLAM is the ability to incorporate inter-robot loop closures. Kim et al. (2010) aims to achieve consistent maps across multiple robots independently of the employed sensing modalities by detecting loop closures between robots and connecting their pose graphs. In the same direction, Mangelson et al. (2018, 2019) aim to robustly select inter-robot loop closure candidates by maintaining pair-wise consistent measurements. More recently Lajoie et al. (2020) proposed a distributed system with distributed loop closure detection.

The more robots are deployed for a specific task, the more information needs to be processed, potentially leading to delays or longer processing times, especially for components such as the factor graph optimization. Recently, COVINS (Schmuck et al., 2021) demonstrated a collaborative deployment of 12 individual agents while maintaining a reasonable collaborative trajectory error. Although their system propagates optimized poses from the centralized server back to individual agents, the poses are only used for drift quantization by comparing the optimized to the onboard estimate. Thus, the onboard pose estimations are not corrected.

Concluding, many existing approaches are limited to a single modality only (Deutsch et al., 2016; Karrer et al., 2018; Lajoie et al., 2020). These methods are often incorporated in tightly coupled multi-robot frameworks, exchanging large data structures such as descriptors (Tian et al., 2022), partial or complete (Schmuck and Chli, 2019) factor graphs. As a consequence, the systems become less flexible and maintain little versatility for the application of different robotic tasks. Conversely, this paper proposes to detect discrepancies between the robot graphs using spectral analysis and a sparse abstraction of the server graph to generate an individual set of constraints for each robot. This has the benefit that only the pose information is shared between the robots and we can retain a low profile by means of bandwidth requirements. Moreover, since our framework only transmits the poses, our approach does not require any expensive marginalization procedures. Hence, the proposed approach achieves high accuracy and mapping consistency while maintaining low network and compute requirements.

2.2. Failure and degeneracy detection

Pose estimation from onboard sensors is subject to drift (accumulation of small errors) and to degeneracies (errors due to specific sensor modality’s deficiency). Recognizing such errors enables corrective actions to avoid possible catastrophic losses (e.g., platform crashes and wrong decision making). However, evaluating the quality of poses or maps is not trivial when no ground truth is available for comparison. In Schwertfeger and Birk (2013), a metric to assess the quality of the maps was proposed by matching topological graphs from the robot with a ground truth map. Some research also approaches the problem using redundant estimation systems (Sundvall and Jensfelt, 2006) to find inconsistencies. Moreover, the recent work of Nobili et al. (2018) learns a model to predict failures for pointcloud alignments. Akai et al. (2019) infers a failure type based on the distribution of the residual error. The work of Zhang et al. (2016), proposes to analyze the structure of the constraints using the eigenvalues to derive a degeneracy factor.

We approach the problem differently by taking into account the underlying graph structure, precisely its spectral properties. Thus, making our approach independent of the employed sensor system and enabling us to evaluate the discrepancies at multiple scales to be more precise when resolving the spurious estimations.

2.3. Spectral graph and graph signal processing

Spectral graph theory is an active research area and has gained popularity in the past years in the context of robotics. Spectral graph theory approaches have been proposed for robotic mapping (Brunskill et al., 2007), planning (Indelman, 2018), and more recently, in combination with graph neural networks for various robotic tasks (Chandra et al., 2020; Moon and Lee, 2020). In general, graphs are irregular structures that do not depend on the underlying manifold of the nodes. Thus, graphs are perfectly capable of modeling large, complex, or distributed problems (Mateos et al., 2019). Khosoussi et al. (2019) evaluates the correlation between the Fisher information matrix and the Laplacian matrix in the context of SLAM.

Furthermore, Egilmez and Ortega (2014) proposes an anomaly detection for spatial proximity of graph nodes using spectral graph filtering. Furthermore, graph signal processing aims at applying signal processing techniques on graph structures, thus allowing the use of existing concepts such as the Laplacian operator (Sandryhaila and Moura, 2014) and multi-scale analysis (Hammond et al., 2011, 2019). Similarly, Donnat et al. (2018) aims to learn a multi-scale structural embedding using graph wavelets by treating the wavelet coefficients as a probability distribution. A good introduction and overview of graph signal processing are presented in Ortega et al. (2018). Recently, Doherty et al. (2022) proposes a spectral sparsification of pose graphs to deal with the limited computational capacity in long-term SLAM.

Our approach performs a structural analysis of graph signals using a spectral representation to detect discrepancies between two factor graphs. Consequently, our approach fits in a centralized multi-robot SLAM scenario where the robots are susceptible to drift. Our approach exploits graph Wavelets, localized on the individual nodes in the graph to perform a multi-scale analysis of the structural properties. Therefore, our approach enables estimating the severity of the inconsistency of the individual pose graphs.

3. Preliminaries

This section introduces the fundamental and necessary concepts for analyzing and comparing graph structures in the graph spectral domain. We first introduce the underlying methods to use graphs for modeling complex problems. Next, the analysis of harmonic signals in the Euclidean and graph domain are covered.

3.1. Fundamental graph theory review

In this work, we exploit the graph structure that serves as the primary foundation for most modern SLAM backends (Cadena et al., 2016). In particular, we extract the pose information of the factor graphs, that is, disregarding any other sort of constraints to visual landmarks, GPS sensors, etc. Thus, we consider in this work, weighted undirected graphs $G = (E, V, w)$ consisting of a set of nodes $V$ with cardinality N, edges $E$ and weights $w : E \mapsto R^{+}$ denoting how strong two nodes are connected with each other.

A graph $G$ is uniquely described by $E$ , $V$ and w in the form of a weighted symmetric adjacency matrix $A \in R^{N \times N}$ with A_n,m > 0 if two nodes n and m are connected. The weight can be chosen freely, such as, for instance, the spatial proximity, or the number of co-observed landmarks between nodes, but ought to measure the relationship between the nodes. Another fundamental construct in graph theory, is the degree matrix D, defined as a diagonal matrix with entries D_n,n = ∑_n′A_n,n′ where n′ are all incident nodes of n.

Finally, signals in traditional signal processing are often expressed as functions over time, such as $x (t) : R \to R$ , mapping a scalar value to each discrete point in time t. In a similar vein, signals on graphs are defined as $x (n) : V \to R$ , associating a scalar value to each node n in the graph. While a traditional signal x(t) changes over time, a signal defined on a graph x(n) alters between the nodes in the graph, leading to certain variations within the signal. Analyzing these signal variations and, consequently, their trends can lead to a more fundamental understanding of the signal’s nature and is generally termed spectral or frequency analysis.

3.2. Euclidean and graph spectral analysis review

Integral transformations such as the Fourier transform project a signal onto a Hilbert space enabling the analysis of the characteristic properties of that signal. The data in the projected space is a compact representation denoting the information that is more or less prevalent in the input data. However, different aspects, such as the duration, bandwidth, and discretization, need to be carefully considered for a correct analysis of band-limited signals and to avoid unfavorable effects such as leakage and Gibbs ringing.

The Fourier transform is a fundamental tool for various applications in many different fields ranging from image-processing to data science (Rao et al., 2010). Specifically, the Fourier transform expresses a signal x(t) as a sum of basis functions for which a typical choice is the complex wave: $\cos (2 π f t) + j \cdot \sin (2 π f t) = \exp (2 j π f t)$ . Thus, the traditional Fourier transform is the inner product of a time-dependent signal x(t) with the harmonic oscillation,

X (f) = {〈 x (t), \exp (2 j π f t) 〉}_{t} = \int_{- \infty}^{\infty} x (t) \exp (- 2 j π f t) d t,

(1)where X(f) is the spectrum of x(t). Most interestingly, the basis function

\exp (2 j π f t)

plays an important role w.r.t. the one-dimensional Laplace operator Δ in

R

\begin{array}{l} Δ \exp (2 j π f t) & = \frac{\partial^{2}}{\partial t^{2}} \exp (2 j π f t) \\ = 2 j π f \frac{\partial}{\partial t} \exp (2 j π f t) \\ = - {(2 π f)}^{2} \exp (2 j π f t) . \end{array}

(2)Based on the Helmholtz equation, −Δx = λx, the initial term (2πf)² corresponds to the eigenvalue while the later term

\exp (2 j π f t)

denotes the eigenfunction.

The Laplace operator is a linear operation that expresses the divergence of a given function’s gradient. Likewise, a similar intuition can be applied for deriving a Laplace operator for irregular graph domains. In concrete, the Laplace operator Δ is replaced by the Laplacian matrix $L$ (Ricaud et al., 2019) measuring the variations within graphs. Generally, for an undirected graph with the two matrices A and D, the Laplacian matrix can be obtained by

L = D - A,

(3)which is, by definition, a symmetric and positive semidefinite matrix. Hence, it can be decomposed into its eigenvalues Λ and eigenvectors U, that is,

L = U Λ U^{⊤}

. Based on the findings in equation (2), the eigenvector U can be used to define the Fourier basis functions and, therefore, also to derive a Fourier transformation of graph signals. As a matter of fact, other constructions of the graph Laplacian

L

such as the normalized graph Laplacian are also possible and are widely used in literature as different graph Fourier bases. We refer the interested reader to the work of Shuman et al. (2016) for a discussion on differences between the bases.

In particular, the graph Fourier transform of a graph function x is given by the expansion of x with the eigenfunctions u of the graph Laplacian $L$ (cf. equation (3)), that is,

X (λ_{l}) = {〈 x (n), u_{l} (n) 〉}_{n} = \sum_{n} x (n) u_{l}^{*} (n),

(4)where λ_l is the l-th non-negative eigenvalue of

L

and

u_{l}^{*}

is the complex conjugate of u_l. The graph Fourier transform of a graph signal x is then given by X = U^⊤x. Moreover, the eigenvalues Λ are real values, and thus can be ordered and correspond to the graph frequencies, allowing a similar intuition as for traditional frequency analysis. Consequently, most of a graph signals energy is preserved in the lower bands of Λ, and higher bands correspond to high oscillating frequencies.

The construction of the graph Fourier transform using Equations (3) and (4) implies that all the spectral properties are given by the connection of the nodes. Thus, the modeling of the relationship between the individual nodes constitutes a crucial component of the system.

4. Collaborative multi-robot mapping

This section details the proposed method, for which an overview is presented in Figure 1. Overall, the aim is to identify graph nodes with high drift that will lead to large errors and correct them with only a few constraints. The proposed approach comprises the following core components: (i) Onboard localization and mapping, (ii) Mapping server at the base station, and (iii) Pose graph comparison and correction.

Figure 1.

Overview of our approach. We consider multiple robots simultaneously exploring an environment and sending incremental maps to a centralized mapping server. The server accumulates all robot maps and jointly optimizes them. A relaxation of the collaborative multi-robot map is sent back to the robots, where a multi-scale graph spectral analysis is performed to identify discrepancies onboard and server maps and to generate necessary constraints for making them consistent.

4.1. Centralized mapping and localization

Each robot performs onboard mapping and localization to provide an odometry estimate of its current position as well as to create a local map of the environment. Particularly, we don’t anticipate that the onboard system must perform any loop closure detection or re-localization methods to reduce their drift. These methods typically require large computational capacities and might, in real-world deployments, inhibit crucial processes like control, odometry, and navigation. Hence, in our setup, each robot solely focuses on estimating the relative motion and building an onboard map without optimizing it. In more detail, the maps are factor graphs with visual landmarks and IMU constraints in our setup. Each map is keyframed before sending it to the mapping server. The onboard map including visual descriptors and pose estimates are then incrementally serialized and sent to a centralized mapping server for additional processing. Figure 2 provides an overview of the employed graph structures.

Figure 2.

Different employed graphs: (a) Onboard visual-inertial graph, which will be incorporated into the global server map. (b) Graph abstractions, including Kron reduction. (c) Synchronized graphs. (d) Multi-scale spectral graph analysis. With the results from (d) the system queries poses in SE(3) to generate constraints.

More specifically, the mapping server takes the role of a high-performance computing resource and acts as a centralized communications hub for all the robots. Hence, the mapping server constitutes the core component of our collaborative mapping approach. In particular, all incoming robot maps are accumulated, merged into a single collaborative multi-robot map, loop closed, and globally optimized (see Figure 2(a)). Consequently, all the computational-intense multi-robot operations are delegated to the central mapping server while the robots only perform the initial map building.

Since the onboard maps are sent incrementally, the mapping server needs to ensure continuous operation on the global multi-robot map, that is, it does not run a set of operations once but rather constantly iterates over the global multi-robot map and, thereby, considering the most recent data.

Furthermore, at the end of each iteration, the mapping server constructs a global multi-robot graph from the optimized pose information and broadcasts it on the network (see Figure 2(b)). Notably, the sent global graph is an undirected graph that does not contain any sort of constraints such as IMU factors, visual terms, or LiDAR terms but rather only comprises the positional and rotational information of the nodes in the form of vertices. Furthermore, we create additional edges to model the relationships between the nodes.

Each time a robot receives a global graph, it replaces the previously received graph with the newly received graph. Thus, the comparison of the onboard estimation and the globally optimized solution of the server happens at the individual robots using the pose information in SE(3).

4.1.1. Global multi-robot graph

The global graph sent to the robots encapsulates the global knowledge of the environment in a compact representation and contains crucial information such as the last known positions of all robots. The global graph is built by defining representative nodes for each incoming submap. The representative nodes can be freely chosen but ought to reflect the robot trajectories to some degree. We found that keyframing heuristics such as minimum distance and rotation between consecutive nodes work well in our experiments.

Moreover, since the global map is continuously optimized, the graph is not immediately built after every operation, but only a reference to each node is maintained. When an update is triggered after the global optimization, the graph monitor (cf. Figure 1) retrieves the latest estimate of the multi-robot pose graph, builds the graph, and sends it to all robots.

This graph captures the relationship between all the different vertex within the factor graph. Since we cannot only rely on connectivity information such as odometry constraints or co-visible landmarks to build this graph, a radius search is performed around each vertex in the graph. The weight of adjacent nodes is calculated using a squared exponential function where the weight decreases with increasing distance Δ,

w_{D} (n, m) = \exp (- \frac{Δ (n, m)}{2 σ^{2}})

(5)

The distance function Δ(n, m) measures how much the nodes n and m are related to each other. Possible realizations of Δ(n, m) include measures of spatial proximity or viewpoint coherence, such as distance metrics using the global position $p \in R^{3}$ or global orientation R ∈ SO(3), among others. Although less generic, high-level information, such as the number of co-observed visual landmarks and semantic objects, can also be incorporated into Δ(n, m).

In our previous work (Bernreiter et al., 2022), we employed an abstraction from the factor graph, the so-called proxy graph, that only consisted of the positional information in $R^{3}$ of the nodes in the factor graph. Since a drift in the onboard estimation or a degenerate state will always be recognized as a difference in the positions of the estimates. However, this does not accurately capture rotational drifts since the positional difference as a result of the drift will only manifest on the consecutive nodes due to the initial rotational offset.

Therefore, in this paper, we employ a combination of spatial proximity and viewpoint coherence using a distance metric in SE(3) (Barfoot, 2017). Specifically, between two poses T_n, T_m ∈ SE(3) we compute

Δ (n, m) = \sqrt{〈 ξ_{n, m}^{\land}, ξ_{n, m}^{\land} 〉} = \sqrt{- tr (ξ_{n, m}^{\land} M ξ_{n, m}^{\land^{⊤}})}

(6)where M is a set of weights for rotation and translation and ^∧:

R^{6} \to R^{4 \times 4}

. The distance metric

ξ_{n, m} \in s e (3)

. is defined as

ξ_{n, m} = \ln {(T_{n}^{- 1} T_{m})}^{\lor}

with ^∨:

R^{4 \times 4} \to R^{6}

and

\ln : S E (3) \to se (3)

. It is noteworthy that our framework only utilizes the variable nodes of the factor graph and does not take the current factors, that is, the edges in the factor graph, into account.

Moreover, it is important to note that other realizations are possible distance metrics, for example, in $R^{3}$ using the Euclidean distance or in SO(3) using the distance between rotations (Trefethen and Bau, 1997),

Δ_{r} (n, m) = - tr R_{m} R_{n}^{⊤},

(7)where R ∈ SO(3) and represents the orientation.

4.1.2. Graph reduction

Our framework allows for multiple different reduction schemes, for which all approaches operate on the server graph $G_{s e r v e r}$ at the centralized mapping server.

When the global graph $G_{s e r v e r}$ is broadcasted over the network, each robot synchronizes the onboard estimation with the nodes in $G_{s e r v e r}$ based on their timestamp (cf. Figure 2(c)). Since $G_{s e r v e r}$ was built based on the odometry estimates, there is a direct mapping between the nodes.

Moreover, it is important to note that the larger the server graph $G_{s e r v e r}$ becomes, the higher the computational and bandwidth requirements are. This makes it undesirable to send the full graph to all robots in time-critical applications. As a consequence, $G_{s e r v e r}$ is reduced using a Kron reduction (Dorfler and Bullo, 2013), when the server graph reaches a predefined threshold of nodes. The Kron reduction removes a subset of nodes of the server graph without considerably altering its properties, that is, it preserves the graph’s spectral representation and the adjacency matrix (Loukas, 2019). The removal of the nodes is implemented as a Schur complement of $L$

L_{reduced} = L_{n, n} - L_{n, \tilde{n}} {(L_{\tilde{n}, \tilde{n}})}^{- 1} L_{\tilde{n}, n},

(8)where

\tilde{n}

are the nodes marked for removal.

The choice of which nodes to keep in the reduced graph should rely upon the information content and how prominent a specific node is. Most interestingly, the magnitude of the eigenvalues of $L$ denotes how much information each graph frequency contains. Therefore, the nodes to keep during the Kron reduction are selected according to the largest eigenvalues in Λ.

It is important to note that the Kron reduction introduces more edges to the graph as a result of preserving the spectral properties of that graph. Despite more edges in the graph, the required bandwidth for transmission is still little as nodes require significantly more memory while adding more edges solely changes the adjacency matrix. Therefore, communication-wise, our proposed approach benefits from having fewer entries in the adjacency matrix and fewer poses to transmit.

As a next step, we perform a graph spectral analysis using $G_{s e r v e r}$ to identify drifts in the onboard estimation (cf. Figure 2(d)).

4.2. Spectral analysis of graph signals

The analysis of spectral components of band-limited signals is a well-established and widely used technique in engineering and research. This paper benefits from the spectral representation by exploiting the theory of spectral analysis and signal processing defined on graphs to improve collaborative multi-robot mapping.

Figure 3 denotes an overview of our proposed spectral analysis of factor graphs by showing an exemplary comparison. In contrast to the standard spectral analysis that operates, for example, in the Euclidean domain, graphs do not assume any underlying manifold of the nodes such as $R^{3}$ or SE(3). Therefore, they are well-suited for many robotic applications where problems are represented as graph structures, such as pose graphs or scene graphs (Hughes et al., 2022).

Figure 3.

Overview of the proposed spectral analysis of the factor graphs, one from the mapping server (blue) and one from the onboard estimation (red). (A) Shows the server graph with eight nodes and corresponding graph abstractions along with the signal $\tilde{f}$ , while (B) shows the onboard estimation with two drifting nodes (red nodes) and the corresponding signal $\tilde{g}$ . The graph signals indicate that there is a significant difference in their estimation, that is, a drift in the onboard estimation. In (C), we computed a graph Wavelet with six scales from the server graph abstraction. (D) and (E) show the corresponding Wavelet coefficients when expressing the individual signals in the Wavelet’s basis. In (F), we computed a feature vector by evaluating (D) and (E) with subsequent grouping into three categories for small, mid, and large discrepancies corresponding to the low, mid, and high-frequency components. Additionally, (F) shows a large dissimilarity in nodes two and five, which require additional support. Finally, in (G), we added additional constraints (blue lines) to the onboard estimation to reduce the drift. Since the discrepancy in (F) was in the higher frequencies, we added additional constraints between the adjacent nodes (G).

4.2.1. Graph comparison

After the robot has received an update containing a global multi-robot map, chronological synchronization of the estimates is necessary due to the keyframing at the centralized server. We assume that the robots have synchronized clocks with the mapping server, thus, the synchronization results in a one-to-one mapping of the global graph $G_{s e r v e r}$ and onboard estimates. Next, the pose information at the nodes of each graph is used to create two functions $\tilde{f}$ and $\tilde{h}$ defined on the graph for the server and robot estimates, respectively. In more detail, $\tilde{f}$ and $\tilde{h}$ operate on the nodes $V$ and express a representative value for this node, that is, $\tilde{f} : V \mapsto R$ In particular, the pose information for each node in the graph is used to compute the relative distance to the origin, producing a unique expression for each node.

Generally, Wavelets are a well-known time-frequency representation and a very efficient and flexible for a variety of different tasks in signal processing problems Hammond et al. (2019), such as compression (Finder et al., 2022), image-processing (Huang and Dragotti, 2022), and classification (Gorgel et al., 2009). In traditional Wavelet analysis, a signal x(t) is projected onto a scaled (a) and shifted (b) Wavelet ψ,

W (a, b) = {〈 x (t), ψ_{a, b} (t) 〉}_{t} = \int_{- \infty}^{\infty} \frac{1}{a} ψ^{*} (\frac{t - b}{a}) x (t) d t .

(9)Using Parseval’s theorem, that is, ⟨f, g⟩ = ⟨F, G⟩, equation (9) can also be expressed with the Fourier-transformed signal:

W (a, b) = {〈 X (ω), Ψ_{a, b} (ω) 〉}_{ω}

Analogously, the graph Wavelet transform can be derived using the graph Fourier transform and a Wavelet filter kernel on $L$ . For more details on the derivation of graph Wavelets, we refer the interested reader to the work of Hammond et al. (2011, 2019). Furthermore, graph Wavelets, by construction, have the property of being localized on the graph (Donnat et al., 2018; Tremblay and Borgnat, 2014) and, therefore, graph Wavelets directly relate with the structural properties of the specific nodes. The realization of a graph Wavelet ψ_s,n for a scale s and node n is given by

\begin{array}{l} ψ_{s, n} & = U G_{s} (Λ) U^{⊤} δ_{n} \end{array}

(10)

= U G_{s} (Λ) Δ_{n},

(11)where δ_n and Δ_n are a Dirac centered at vertex n in the graph and graph spectral domain, and G_s the Wavelet filter bank at scale s. In other words, the filter bank G_s acts only on the eigenvalues of the graph, that is,

G_{s} (Λ) = diag (g (s λ_{1}), \dots, g (s λ_{N}))

and is multiplied with the graph Fourier-transformed Dirac, followed by an inverse transform U. An exemplary computation of a graph Wavelet with six scales is depicted in Figure 3(c).

Next, the graph Wavelet coefficients are computed by expanding the graph signals into the basis of ψ (cf. Figure 3(d)–(e)). Since ψ_s,n lies in the graph domain, we can compute the Wavelet coefficients for the graph signals $\tilde{f}$ and $\tilde{h}$ using

\begin{array}{l} W_{s, n}^{\tilde{f}} = ψ_{s, n}^{⊤} \tilde{f}, \\ W_{s, n}^{\tilde{h}} = ψ_{s, n}^{⊤} \tilde{h} . \end{array}

(12)It is important to note that the Wavelet ψ is calculated only for the server graph, while we compute graph signals for both server and onboard estimation. Since ψ does not change as long as the global server graph does not change, we only need to compute equation (10) once per version of the global graph.

We employ the Meyer Wavelet due to its good localization in the graph and frequency domain. Generally, larger values for s compress the filter function g, while smaller values of s stretch g. Thus, very large scales do not capture drifts precisely anymore. We found that using less than 10 scales provides a good amount of information with different granularity on the inconsistencies.

The graph signal $\tilde{f}$ is localized for each node in the graph and can be chosen arbitrarily but should reflect the characteristics of each node, similar to the edge weights in equation (5). In this work, we utilize the distance metric defined in equation (6) from the local map origin o to every node n in the graph,

\tilde{f} = Δ (n, o) .

(13)

In summary, we compute Wavelet coefficients W up to scale s_max for the server and onboard graphs, which constitute a major component of our approach that represents multi-scale structural information for each node in the graph. Moreover, by comparing the coefficients for the server and onboard estimates, our algorithm can efficiently identify structural differences between them.

4.3. Correcting onboard estimation

We propose to compute a similarity metric between the graph Wavelet coefficients of the individual signals to identify the differences between the estimations. Therefore, for a node n in the graph, we define the scale-wise distance as

d_{s, n} = {‖ W_{s, n}^{\tilde{f}} - W_{s, n}^{\tilde{h}} ‖}_{2},

(14)where

W_{s, n}^{\tilde{f}}

and

W_{s, n}^{\tilde{h}}

were computed using equation (12) utilizing the graph signals for server and onboard graph, respectively. The vector d forms then a feature that can be used to classify the severity of the difference between the estimations by evaluating the individual scales.

Intuitively, since graph Wavelets are localized at a specific node n in the graph (Donnat et al., 2018; Tremblay and Borgnat, 2014), and large scales compress g, the process focuses on the lower frequencies and spreads further into the graph, leading to a description of the larger neighborhood. In contrast, since small scales stretch g, they concentrate on the higher frequencies and yield a description of the closer neighborhood of n. Consequently, equation (14) is computed for all scales s, resulting in the feature vector d (cf. Figure 3(f)).

Our approach combines several scale levels by accumulating the distances in equation (14) over multiple scales. Accordingly, three separate cases are distinguished: a large difference in the lower, mid, and higher scales of the coefficients (cf. Figure 4(a)–(c)). In our previous work (Bernreiter et al., 2022), we introduced thresholds for $d_{n, n^{'}}^{s}$ to decide when to construct constraints for nodes exceeding it. Essentially, the threshold for the small, mid, and large scales needed to be set carefully for the graph and might require some tuning. In this work, we introduce the notion of only using the top distances for constructing the constraints. If a small-scale difference is amongst the top distances, a relative constraint is added between the direct neighbors of n′. Likewise, a corresponding constraint within a multi-hop distance of n′ is added for a top mid-scale difference. In the case of a top large-scale discrepancy, a constraint between the k-nearest submaps is added (cf. Figure 3(g)). It is important to note that the server and robot graph can be expressed in arbitrary and unknown frames since all different types of constraints are relative between nodes. Additionally, it should be noted that the constraints originate directly from the SE(3) graph that was used for the spectral comparison.

Figure 4.

Illustration of three different relative constraint types. Based on the scale of the structural difference, additional constraints are added to correct (a) adjacent, (b) close neighborhood, (c) between submaps, or (d) on degenerate poses.

4.3.1. Updating the onboard graph

Each robot runs an onboard incremental graph optimization proposed by Kaess et al. (2012) to incorporate the odometry constraints as well as the additional constraints provided by our algorithm.

Since the mapping server will continuously optimize the global multi-robot map and incorporate new inter- and intra-robot loop closures, the constraints can change over time, that is, existing constraints can be republished with some differences. Thus, the onboard graph manager module keeps a history of all the incorporated constraints and checks for each incoming constraint whether an already active constraint exists between the corresponding nodes. For efficiency, existing constraints are only updated when there is a reasonable difference in translation and rotation to the previous one. Otherwise, they remain unchanged.

New constraints are not directly added to the onboard graph, but we rather buffer all incoming constraints and apply them at once in a batch. This avoids the graph being accessed for every constraint that is identified.

Additionally, since the global multi-robot graph is modified over time, new constraints are also generated if nodes that were previously labeled as drifting have changed but have not been identified as drifting nodes during the current comparison. Consequently, we check the orientation and position of each labeled node and provide an updated constraint accordingly. The onboard graph system replaces old constraints when new constraints are provided. This is important since loop closures at the mapping server can significantly alter the global graph. Accordingly, the newly generated constraints could contradict the already existing constraints in the onboard graph and, therefore, need to be updated. Otherwise, the onboard optimization could diverge or produce impaired results due to conflicting constraints.

Lastly, to make our approach more flexible, we introduce a stopping heuristic when the feature vector d becomes too small. This heuristic allows our approach to be configured to consider the most prominent distances only and to avoid publishing constraints for slight differences in the multi-scale analysis.

5. Experiments

We thoroughly evaluate the proposed framework with its different configurations and demonstrate its real-world application using various datasets comprising aerial and legged robots. First, we validate our approach and compare its performance to the current state-of-the-art methods using the EuRoC (Burri et al., 2016) dataset sequences to simulate multi-robot deployments. Next, we demonstrate the real-world performance of our framework during a multi-robot autonomous exploration and mapping mission conducted in an underground tunnel system using ANYmal (Hutter et al., 2017) legged robots. Finally, a multi-robot experiment conducted in indoor and outdoor environments demonstrates the localization recovery for an individual agent in case of onboard localization failure.

For all experiments, we use a radius search of 7 m around the nodes in the global multi-robot map to construct the global graph using equation (5). The root-mean-square of the absolute trajectory error denoted as RMSE, is used as an evaluation metric for all experiments. Moreover, unless otherwise stated, our framework is continuously running along with the onboard estimation and performs the proposed spectral analysis every 20 s. In all experiments, the weighted adjacency matrix of the global multi-robot map is built using the SE(3) approach and is created using a keyframing heuristic of 0.5 m in positional difference. Lastly, the onboard odometry constraints are configured to have a standard deviation of 0.05 rad and 0.1 m, for rotation and position, respectively. In contrast, the relative 6DoF constraints that are added by our framework have a standard deviation of 0.001 rad and 0.001 m for rotation and position, respectively.

It is important to note that each robot also employs a SE(3) distance for computing the signals as in equation (6), unless otherwise stated. Similar, each robot does not employ graph reduction measures but utilizes the full graph to infer the constraints, if not noted otherwise. At the end of each comparison, the onboard graph will be provided with the top 15 constraints that are above a threshold of 0.2.

Finally, the mapping server provides an updated multi-robot graph to the robots after performing one cycle of operations.

5.1. EuRoC dataset: Validation and comparison

We compare the performance of our proposed approach against the current state-of-the-art collaborative mapping frameworks (Campos et al., 2021; Karrer et al., 2018; Schmuck and Chli, 2019) using the Machine Hall (MH) sequences from the EuRoC dataset to evaluate and validate our single- and multi-robot performance. For each sequence, ROVIO (Bloesch et al., 2017) is used to provide monocular visual-inertial odometry for individual aerial agents.

First, we evaluate the localization performance by comparing the onboard robot estimates, collaborative server estimates, and the proposed approach to the ground truth. In addition, we also compare current state-of-the-art approaches with results presented in Table 1. Since the onboard estimation does not have an initial collaborative result, we omit reporting any error. Furthermore, we report the individual errors of the server for each dataset. These errors serve as a baseline for comparison, since our approach cannot be better than the server estimates.

Table 1.

RMSE Comparison for the EuRoC Dataset. The top part shows the results of single and collaborative approaches, while the bottom row shows the individual corrected results.

EuRoC machine Hall—single and collaborative
Method/Seq	MH01	MH02	MH03	MH01-03
VINS-mono^a	0.12 m	0.12 m	0.13 m	0.074 m
ORB-SLAM3^b	0.062 m	0.037 m	0.046 m	0.037 m
Server	0.020 m	0.023 m	0.031 m	0.025 m
Onboard	0.21 m	0.29 m	0.41 m
CCM-SLAM^c	0.061 m	0.081 m	0.048 m	0.077 m
Proposed $R^{3}$ ^d	0.029 m	0.028 m	0.033 m	0.025 m
Proposed SE(3)	0.026 m	0.027 m	0.033 m	0.025 m

^aSingle robot results from Qin et al.(2018). Collaborative result from Schmuck and Chli (2019)

^bMonocular visual-inertial results from Campos et al. (2021)

^cAs reported in Schmuck and Chli (2019)

^dResults from our previous work Bernreiter et al. (2022)

It can be noted that despite the more significant individual onboard error, the proposed framework still attains the lowest collaborative error. Furthermore, by correcting the onboard estimation using our multi-scale spectral approach, the lowest single-robot errors are also achieved, demonstrating the proposed approach’s effectiveness in correcting large onboard estimation errors. Next, using the experimental setup described in CVI-SLAM (Karrer et al., 2018), we demonstrate that the proposed approach can facilitate accurate pose estimation for individual robots by providing collaborative corrections, as shown in Table 2.

Table 2.

Onboard pose RMSE after adding constraints from the centralized server for different dataset combinations.

EuRoC machine Hall—collaborative corrections
Sequences	CVI-SLAM		Proposed
Sequences	Single	Multi	Single	Multi
MH01 & MH02	0.224 m	0.139 m	0.29 m	0.027 m
MH02 & MH03	0.295 m	0.256 m	0.41 m	0.033 m
MH04 & MH05	0.412 m	0.34 m	0.62 m	0.085 m

Finally, we investigate and analyze the effects of applying different reduction levels to the server graph

G_{s e r v e r}

. This leads to a graph with fewer nodes being broadcasted to the individual robots, enabling a smaller network footprint. Thus, the server-to-robot communication requirements can be significantly reduced with a sparse relaxation of the dense server graph while still being able to reduce the onboard estimation error. This is particularly useful for very efficient bandwidth requirements or when the robots are only within connection range for a very short period of time. Table 3 states the RMSE of the onboard after applying three different levels of reduction to the server graph.

Table 3.

Onboard RMSE after correction w.r.t. different kron reduction levels. The right value denotes the number of nodes in the server graph after the reduction. With the increasing level of reduction, the efficiency of the algorithm decreases due to having less constrained nodes in the onboard factor graph.

EuRoC machine Hall—corrections with different reduction levels (RMSE / Nodes)
Sequences	Onboard	No reduction	20 % reduction	40 % reduction	60 % reduction
MH01 (80m)	0.21 m	0.026 m / 192	0.040 m / 153	0.041 m / 115	0.048 m / 76
MH02 (73m)	0.29 m	0.027 m / 178	0.032 m / 142	0.036 m / 142	0.042 m / 71
MH03 (130m)	0.41 m	0.033 m / 255	0.040 m / 204	0.048 m / 153	0.046 m / 102

After the reduction, the server graph contains fewer nodes that are sent to the robots. Thus, it requires less bandwidth for the transmission to the individual robots. However, the synchronization of the onboard estimate with $G_{s e r v e r}$ leads then also to fewer nodes which results in fewer constraints that can be added to the onboard factor graph, and the error steadily increases with each level. Nevertheless, even when removing more than half of the nodes in the server graph, the error does not significantly increase and remains at a few centimeters. Thus, a substantial improvement to the onboard estimation can already be achieved using only a few additional constraints from our approach. Notably, the reduced results are particularly in favor of our collaborative approach when comparing them to the results of the state-of-the-art collaborative mapping frameworks in Table 1. Despite the large reduction in the nodes and fewer constraints, our framework attains a comparable onboard estimation error.

5.2. Analysis of robotic drift recovery

We conducted an experiment in a particularly challenging environment for LiDAR localization due to the absence of surrounding geometric structure to demonstrate the utility of collaborative mapping towards localization recovery for an individual robot in case of an onboard estimation failure. Two ANYmal robots were simultaneously deployed in an indoor office environment connected to an outdoor rooftop terrace. Each robot is equipped with a Velodyne VLP-16 LiDAR and a Sevensense Alphasense visual-inertial sensor. Both sensors are synchronized within the onboard computer.

Onboard robot odometry estimation and mapping are performed by CompSLAM (Khattak et al., 2020) and, along with the required visual and pointcloud data, are sent to the mapping server whenever the robots are within the communication range. The first robot performs a loop indoors while the second robot transitions outdoors through a narrow doorway, navigates a rectangular path, and returns indoors.

5.2.1. Localization recovery

Due to the absence of surrounding structures on the outdoor terrace, the onboard localization drifts significantly, skewing the onboard robot map. Nevertheless, the collaborative mapping approach is able to generate a consistent map of the environment, as shown in Figure 5, due to its inter- and intra-robot loop closure capabilities.

Figure 5.

Mapping results for the indoor/outdoor dataset. Cyan denotes the ground truth trajectory, red the onboard estimation, and green the corrected trajectory. The robot map of ANYmal 1 (red) is misaligned due to the onboard localization failure (A) and (B), but can be fixed using constraints provided by the centralized mapping server.

We created a ground truth map using a Leica RTC360 scanner to evaluate the proposed collaborative mapping framework’s performance and quantify the effect of the integration of collaborative corrections on individual robot pose accuracy. Ground truth robot poses were then computed by registering individual robot pointclouds against the ground truth map following the approach of Ramezani et al. (2020).

Furthermore, the integration of collaborative constraints enables localization recovery for the individual robot, leading to a significant reduction in its pose error, as shown in Table 4, when compared to the ground truth robot trajectory. Moreover, we provide baseline estimations in addition to our proposed solution to provide a more comprehensive overview of our framework’s performance. The first baseline places an adjacent constraint at every corresponding node of

G_{s e r v e r}

, that is, each node except for the first and last one in the graph has two relative connections. On the contrary, the second baseline adds prior constraints to each corresponding node of

G_{s e r v e r}

. For both baseline solutions, we check whether the server estimate has changed and update only nodes that differ from a previous estimate. Particularly, we show for both baseline solutions, despite having full knowledge of the transmitted server graph and ultimately adding significantly more constraints to the onboard graph, the accuracy is improved but does not fully converge to the solution of the server estimates. This is related to the fact that the server graph contains fewer nodes than the onboard graph due to the performed keyframing before sending it to the mapping server. Lastly, it is important to note that these two baseline solutions come at the price of increasing the required time for onboard optimization due to the additional constraints but improve the overall runtime by omitting multi-scale analysis. Nevertheless, these two baseline approaches do not scale well for large-scale scenarios where the server and robot graph can include a vast number of nodes, which would result in a large optimization problem.

Table 4.

Comparison of the RMSE of the onboard estimation before and after the supplying additional constraints. The timings refer to the time needed to perform the final onboard factor graph optimization.

Indoor/Outdoor dataset—ground truth evaluation
Method	RMSE	Time	Factors	Server
ANYmal 1	2.22 m	5.2 ms	3317	0.21 m
Baseline (rel.)	0.42 m	12.0 ms	3412
Baseline (abs.)	0.36 m	11.8 ms	3507
Proposed	0.30 m	5.9 ms	3339
ANYmal 2	0.25 m	3.4 ms	2239	0.14 m
Baseline (rel.)	0.20 m	10.3 ms	2284
Baseline (abs.)	0.17 m	9.9 ms	2329
Proposed	0.16 m	4.1 ms	2253

In contrast, our approach adds not only fewer constraints to the optimization but also achieves higher accuracy. In particular, the multi-scale spectral analysis enables our approach to identify and construct different types of constraints for nodes that have a higher significance on the reduction of the drift and, thus, also on the overall error. In more detail, the submap constraints especially have a great potential to improve the accuracy but need to be placed carefully at drifting nodes to avoid additional computational overhead. Moreover, the submap constraints are notably useful for correcting large-scale drifts, while the smaller constraint types perform a local refinement. Consequently, the submap constraints play the most influential role in the correction and recovery of the onboard drift.

Lastly, we present an evaluation of the runtime per individual component in the graph client in Figure 6. The graph building component includes the building of the initial graph structure as well as the Wavelet estimation. The total size of this graph was 190 nodes, and six Wavelet scales were computed during the comparison. The Wavelet estimation always computes the Wavelet coefficients from scratch and does not reuse older computations. We leave it for future work how these coefficients can be computed incrementally by reusing the previous result (Andreopoulos and Van der Schaar, 2008). Additionally, it is important to note that this step only has to be performed once per graph update from the server, while the other component can run multiple times per graph. Consequently, the increased runtime for the graph building has important practical implications since it is significantly higher than the runtime required for adding the constraints and optimizing the resulting graph. As a result, the deployment of our proposed solution on robotic systems requires the evaluation and configuration of different components, for example, in terms of needed graph updates from the expected drift as some use cases require more or less frequent updates of the onboard system. Therefore, some scenarios could benefit in terms of runtime by only sending server updates when the server estimates have significantly changed. Likewise, the type of the constructed constraints plays an important role since the different constraints do not have the same effect on onboard runtime and accuracy. Hence, in the following, we will investigate the constraint generation process as well as the interrelations between the construction of the graph and the type of constraints.

Figure 6.

Evaluation of the runtime performance per individual component. These timings represent the result when using the final full graph of the indoor/outdoor dataset. It is evident that the graph generation and the wavelet estimation takes the biggest portion of the runtimes.

5.2.2. Analysis of the constraint generation

In this experiment, we investigate which and where constraints are generated to fix the onboard estimation. We additionally compare the performance of different distance metrics and construction approaches of the graphs in comparison to our previous work (Bernreiter et al., 2022).

In more detail, we configure our framework to perform the signal and edge weight calculations in different manifolds. Specifically, we investigate three different instances of our proposed framework where we use: (i) L₂-Norm in $R^{3}$ , (ii) using equation (7) in SO(3), and (iii) using equation (6) in SE(3). The specific manifold is not swapped for the graph and signal, that is, the same manifold is applied for computing the distance for both the edge weights and the signal comparisons.

The resulting constraints for each selection strategy are presented in Figure 7. It is evident that selecting only the top constraint candidates leads to primarily adjacent constraints for $R^{3}$ and n-hop and submap constraints for SO(3). For the SE(3) strategy, however, it can be seen as a combination of the other strategies, leading to a lower error than for the other approaches.

Figure 7.

Illustration of the different types of constraints added by using (A) $R^{3}$ , (B) SO(3), and (C) SE(3). At each comparison, only the top 30 constraints were propagated to the onboard correction node. The top right shows a top-down view of the new constraints.

We present the results in Table 5 for all the different signal and weight computation strategies. Each strategy is separated into top 10, 30, and 50 constraints to get a deeper insight into the constraint generation process. In this experiment, the onboard estimation is only updated once with the respective top n constraints at the end of the run when the server has received and processed all the information to allow for a fair comparison of the individual strategies. By providing constraints with the highest differences only once, it can be shown how the manifold influences the identification of the drifting nodes.

Table 5.

Evaluation of different graph construction and signal generation strategies for the ANYmal 1 indoor/outdoor dataset.

Indoor/Outdoor dataset—constraint generation
Constraint selection	$R^{3}$	SO(3)	SE(3)
Top 10 constraints	2.35 m	1.65 m	0.33 m
Top 30 constraints	2.05 m	0.35 m	0.32 m
Top 50 constraints	1.01 m	0.33 m	0.31 m

Notably, the top 10 constraints for $R^{3}$ even increase the error of the trajectory. This is due to the fact that solely adjacent constraints were applied to local regions, resulting in a larger error as the drift worsened when assessed globally. Subsequent evaluations of graphs would then result in more effective constraints.

Furthermore, the results also indicate superior performance of the SE(3) computations. Even by only evaluating the top 10 constraints, the approach attains a significant improvement to the baseline estimation. Most importantly, adding more constraints does not account for substantial improvements anymore when using SE(3) computations since the initial result using 10 constraints has already identified and corrected the most prominent discrepancies.

Moreover, we present an analysis of the spectral components in Figure 8. Here, we compare the estimated discrepancies when using $R^{3}$ or SE(3) as an underlying manifold. The spectral comparison in the case of using $R^{3}$ , the high differences at low scales are exhibited in nodes towards the end of test sequence where the drift creates the largest discrepancies (cf. Figure 5). This is due to the fact that the drift will result in a large positional discrepancy and even perturbs the origin. In the case of SE(3), the high differences are placed more at the drift’s origin and, consequently, provide a more accurate estimation of the differences. Therefore, the onboard estimation will converge faster to the correct solution by placing the constraints at the affected nodes. Lastly, the spectral analysis measures the changes of the signals between the nodes, that is, its variance, and therefore the approaches that only operate only on $R^{3}$ or SO(3) are always implicitly influenced by the missing counterpart, the rotation or translation.

Figure 8.

Illustration of the discrepancies in the Wavelet coefficients mapped onto each node in the graph. The left image shows the discrepancies for the case of $R^{3}$ while the right part shows the discrepancies as a result of using SE(3). Blue nodes indicate little discrepancies in the variations, while red nodes depict high discrepancies. (A) Shows that for the case of $R^{3}$ the high differences reside at the end of the trajectory where the drift culminates, while (B) shows that the SE(3) approach detects the difference at the place where drift starts becoming significant.

5.3. Large-scale multi-robot subterranean exploration

We demonstrate the suitability of our approach for complex real-world applications by utilizing it during an autonomous multi-robot exploration (Dang et al., 2020) and mapping mission conducted at the Hagerbach underground facility in Switzerland. In this experiment, three ANYmal quadrupedal robots were deployed during an hour-long mission and autonomously navigated distances of roughly 1.2 km, 1.1 km, and 600m, respectively. Each robot is equipped with the same sensor payload as described in the previous experiment.

The robots individually explore the environment and have multiple overlapping regions with inter-robot loop closures. In this experiment, the onboard estimation is stable but is subject to drift, especially given the large scale of the exploration. The collaborative multi-robot map and individual robot maps are shown in Figure 9, with quantitative results presented in Table 6. We demonstrate that the collaborative mapping approach greatly benefits from the feedback constraints by substantially reducing the onboard pose estimation error, leading to a globally consistent map between all the individual robotic systems. In particular, the combination of all three types of constraints facilitates the accurate reduction of onboard estimation errors. Hence, our spectral comparison and corrections especially promote long-term robotic mission where the onboard localization and mapping approach is susceptible for drift.

Figure 9.

Illustration of the multi-robot map built by our approach. The left image (A) shows the global multi-robot map at the mapping server comprising three individual robot missions. The grayscale images depict exemplary regions using the onboard camera of the robots. The right image (B) shows a top-down view of the individual corrected onboard maps.

Table 6.

RMSE Comparison for the Original onboard, Server, and Corrected onboard Graph. We additionally Provide Insights Into the Sparsity of the Problem by Showing the Total Number of Components (N) and the Number of Non-zero Components (NNZ) per Dataset.

Method	Underground tunnel—ground truth evaluation
	ANYmal 1			ANYmal 2			ANYmal 3
	RMSE	Factors	Sparsity	RMSE	Factors	Sparsity	RMSE	Factors	Sparsity
Onboard	1.15 m	2098368	79.2 %	0.97 m	1831320	79.2 %	0.43 m	1070280	79.2 %
Proposed	0.14 m	5335128	50.0 %	0.10 m	7194816	34.4 %	0.07 m	6140088	23.3 %

In this experiment, we configured our approach to provide as many constraints as possible to achieve the lowest possible onboard trajectory error. It is important to note that there is a trade-off between the incorporation of the constraints into the onboard graph and the resulting improvement it yields. Table 6 also lists the number of factors in the optimization problems along with how sparse the problem is.

Specifically, when incorporating more constraints in the onboard graph, it is evident that the optimization has to deal with more factors in it. In addition, the optimization problem becomes less sparse as there are more interrelations between the individual factors. Hence, when using an incremental solver, such as iSAM2 (Kaess et al., 2012), as it is running on each robot, it might happen that a large portion of the newly added constraints are factors between nodes that are significantly far away in the internal state matrix, invalidating the locality assumption of the solver along with all its performance optimizations. As a consequence, the optimization needs to perform more frequently expensive operations and can take a longer time to incorporate the constraints of our approach into the optimization problem. This is a particular problem for long-term and large-scale applications where our approach needs to be configured such that the number of constraints is at a reasonable level and does not introduce additional computational burden at the onboard systems.

5.4. Degeneracy in the state estimation

Next, we investigate the application of our approach in the case of a degenerate condition in the onboard state estimation. In particular, each robot employs a CompSLAM as a LiDAR-based state estimation (Khattak et al., 2020) in this investigation. Additionally, we incorporated for comparison a degeneracy check for the state hessian matrix (Zhang et al., 2016) during the scan-to-scan and scan-to-submap steps. More specifically, we evaluate the eigenvalues of the state hessian matrix and, if they are above a threshold of 30, consider the robot as degenerate, allowing us to compare the constructed constraints at the time of degeneracy.

The experiment took place in a subterranean cave in Switzerland and comprised two legged ANYmal robots and one flying tricopter (Tranzatto et al., 2022). Similar to the other experiments, all robots deploy the same sensor payload. The flying tricopter as well as one of the ANYmal robots, explore the cave environment without any issues. However, one of the ANYmal robots enters a long narrow tunnel where its onboard estimation system degenerates for a short period, causing shifted maps in the onboard mapping system. This is particularly critical as a safe exploration of the cave cannot be guaranteed any longer for this robot, and it needs to be grounded.

Since our approach is not directly applicable to the detection of degenerate states, we investigate whether, by providing additional constraints, the onboard estimation can recover from it. Our approach employed a constraint generation strategy of the top 15 constraints and can seamlessly recover from the incorrect state by only using a few constraints. The global multi-robot map comprises all three robots and, along with individually corrected robot trajectories, is shown in Figure 10 together with the degenerate and corrected onboard estimation.

Figure 10.

Illustration of the global multi-robot map with corrected robot trajectories (A). The grayscale images depict exemplary regions using the onboard camera of the robots. As the environment is not illuminated the robots also carry onboard illumination. The state estimation of ANYmal 2 became degenerate during the exploration of a long tunnel (B). The multi-modal mapping server was able to overcome the degeneracy and, by providing additional constraints, fixed the onboard estimation (C).

The degeneracy affects the relative position estimation in the direction of the tunnel, that is, the robot is stuck until the environment becomes unique enough again. Hence, the degeneracy is recognizable by the fact that the map is estimated as shorter in length on the way back than on the way in. In contrast, a drift in the estimation would have been roughly the same length but still shifted.

Although the multi-modal mapping server requires at least a few optimization cycles to repair the broken map, our approach can still recover the onboard map reasonably. Hence, enabling the robot to continue the exploration of the underground environment.

In addition, Figure 11 visualizes in detail which constraints our proposed approach sent to the onboard graph in comparison with the detected degeneracy using the approach proposed by Zhang et al. (2016). Most interestingly, the degeneracy in the state estimation of the robot leads to that the prevalence of constructed constraints being between adjacent nodes in the affected area. Only a few constraints are added as 5-hop constraints between the nodes. Since the degeneracy does not properly estimate the transformation between multiple consecutive steps, the small scales are more predominant than the other scales.

Figure 11.

Illustration of the employed constraints constructed by our proposed approach at the degenerate region. The degeneracy was detected after the turn on the way back into the cave.

5.5. DARPA subterranean challenge

Finally, to show our approach’s pertinence to a real-world autonomous multi-robot search and rescue mission, we employ it on the DARPA Subterranean (SubT) dataset of Team CERBERUS (Tranzatto et al., 2022). The SubT Challenge was an international robotics competition for fast and autonomous exploration in complex underground environments, where team CERBERUS won the final event. Each participating team had to deploy a robotic team that explores an unknown environment reporting the location of specific artifacts in it.

Team CERBERUS’ dataset comprises four ANYmal legged robots covering roughly 2 km of semi-autonomous exploration within a one hour mission. During the exploration, multiple robots communicated with a central mapping server, providing visual feedback to a human supervisor. The environment of the final SubT Challenge consisted of three individual regions: (i) a tunnel, (ii) an urban (iii) and a cave environment making it particularly challenging for onboard localization and mapping modules.

Although the proposed approach was not deployed during the competition, we show qualitative, post-processed results of the corrected onboard estimations in Figure 12 with respect to the ground truth map.

Figure 12.

Corrected robot trajectories using our approach with respect to the ground truth map.

Moreover, Table 7 shows quantified results for each individual robot and that our approach can significantly improve the onboard estimation modules. Since none of the robots employed an onboard loop closure detection, each robot accumulated an error over time that could not have been corrected. Only the mapping server at the base station runs a multi-robot loop closure detection. Hence, by using our approach to construct additional onboard constraints, each robot implicitly incorporates the loop closures from the mapping server and decreases its drift.

Table 7.

Comparison of the RMSE of the onboard estimation before and after the supplying additional constraints. Using our proposed approach, the corrected trajectories achieve similar accuracy as the globally optimized server trajectories.

DARPA subterranean final competition
Robot	Distance	Onboard RMSE	Server RMSE	Corrected RMSE
ANYmal 1	240 m	0.72 m (±0.41 m)	0.25 m (±0.13 m)	0.27 m (±0.14 m)
ANYmal 2	687 m	1.29 m (±0.90 m)	0.36 m (±0.28 m)	0.37 m (±0.41 m)
ANYmal 3	311 m	0.23 m (±0.43 m)	0.20 m (±0.34 m)	0.22 m (±0.22 m)
ANYmal 4	500 m	1.00 m (±0.71 m)	0.24 m (±0.17 m)	0.29 m (±0.14 m)

It is important to note that Team CERBERUS scored 23 points along with the second-best team and won due to a tie-breaker rule. During the exploration of ANYmal 4, it reported an incorrect location of a cell phone (artifact ID: L22) artifact for which no score was accounted then. Cell phone detection was implemented by reporting the current position of the robot when the Bluetooth RSSI values exceeded a specific threshold. Thus, the location of the artifact primarily depends on how close the robot walks to the artifact and how large its current drift is.

During the final run, the reported position of artifact L22 was slightly above the 5m threshold of the ground truth location of the artifact, therefore, was not scored. The drift of the robot was not significant but just enough to exceed the threshold. However, by employing our approach to the post-processed run, the onboard estimation can be greatly improved. Figure 13 illustrates the difference in the reported location of artifact L22 between the onboard estimation during the final run and the corrected trajectory using our approach. As a result, the reported position of the artifact would have been within the 5m threshold and, thus, would have scored.

Figure 13.

Illustration of the detection of artifact L22. The onboard estimation is above the 5 m threshold for scoring. Providing additional constraints from our approach leads to a correction that would allow a successful scoring of L22.

6. Conclusions

This paper proposed a novel collaborative multi-robot framework for updating the pose graph of individual robots with constraints from a centralized mapping server. Our approach benefits from spectral representations of the graph Laplacian matrix. In this context, we presented a graph-based spectral Wavelet analysis of the robot and server graphs to identify the underlying structural differences in the onboard estimation. In particular, our approach computes signals in SE(3) and, along with graph Wavelets, finds nodes in the onboard graph that contradict the globally optimized graph of the mapping server. Most importantly, by efficiently adding 6-DoF constraints on drifting nodes, minimal additional computation and communication resources are needed, while significantly improving the onboard estimation.

The presented results demonstrate the real-world potential of the proposed approach using small- and large-scale multi-robot field deployments in challenging environments. Additionally, we show results for datasets of up to four robots simultaneously, yielding important implications for both research and industry.

We intend to continue our research in two directions. First, we will investigate the construction of graph hierarchies to circumvent the accuracy trade-off for the reduction levels. Second, we plan to explore the possibility of keeping the onboard graph sparse using a minimal spanning tree that keeps only a limited set of the most prominent constraints in the onboard graph.

Footnotes

Acknowledgments

The authors are thankful to Marco Tranzatto, Patrick Pfreundschuh, Samuel Zimmermann and Timon Homberger, Gabriel Waibel for their assistance with field experiments.

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported as a part of NCCR Robotics, a National Centre of Competence in Research, funded by the Swiss National Science Foundation (grant number 51NF40_185543).

ORCID iDs

Lukas Bernreiter

Lionel Ott

References

Akai

Morales

Hirayama

, et al. (2019) Misalignment recognition using markov random fields with fully connected latent variables for detecting localization failures. IEEE Robotics and Automation Letters 4(4): 3955–3962. DOI: 10.1109/LRA.2019.2929999.

Andreopoulos

van der Schaar

(2008) Incremental refinement of computation for the discrete wavelet transform. IEEE Transactions on Signal Processing 56(1): 140–157. DOI: 10.1109/TSP.2007.906727.

Barfoot

(2017) State Estimation for Robotics. Cambridge: Cambridge University Press. ISBN 9781316671528. DOI: 10.1017/9781316671528.

Bernreiter

Khattak

Ott

, et al. (2022) Collaborative robot mapping using spectral graph analysis. 2022 international conference on robotics and automation (ICRA), Philadelphia, Pennsylvania, USA, 23–27 May 2022. DOI: 10.1109/ICRA46639.2022.9812102.

Bloesch

Burri

Omari

, et al. (2017) Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback. The International Journal of Robotics Research 36(10): 1053–1072. DOI: 10.1177/0278364917728574.

Brunskill

Kollar

Roy

(2007) Topological mapping using spectral clustering and classification. 2007 IEEE/RSJ international conference on intelligent robots and systems, San Diego, California, USA, 2 November 2007. DOI: 10.1109/IROS.2007.4399611.

Burri

Nikolic

Gohl

, et al. (2016) The EuRoC micro aerial vehicle datasets. The International Journal of Robotics Research 35(10): 1157–1163. DOI: 10.1177/0278364915620033.

Cadena

Carlone

Carrillo

, et al. (2016) Past, present, and future of simultaneous localization and mapping: toward the robust-perception age. IEEE Transactions on Robotics 32(6): 1309–1332. DOI: 10.1109/TRO.2016.2624754.

Campos

Elvira

Rodriguez

JJG

, et al. (2021) ORB-SLAM3: an accurate open-source library for visual, visual–inertial, and multimap SLAM. IEEE Transactions on Robotics 37: 1874–1890. DOI: 10.1109/TRO.2021.3075644.

10.

Chandra

Guan

Panuganti

, et al. (2020) Forecasting trajectory and behavior of road-agents using spectral clustering in graph-LSTMs. IEEE Robotics and Automation Letters 5(3): 4882–4890. DOI: 10.1109/LRA.2020.3004794.

11.

Chang

Ebadi

Denniston

, et al. (2022) LAMP 2.0: a robust multi-robot SLAM system for operation in challenging large-scale underground environments. IEEE Robotics and Automation Letters 7: 9175–9182.

12.

Cunningham

Indelman

Dellaert

(2013) DDF-SAM 2.0: consistent distributed smoothing and mapping. 2013 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE. DOI: 10.1109/ICRA.2013.6631323.

13.

Dang

Tranzatto

Khattak

, et al. (2020) Graph-based subterranean exploration path planning using aerial and legged robots. Journal of Field Robotics 37(8): 1363–1388.

14.

Deutsch

Liu

Siegwart

(2016) A framework for multi-robot pose graph SLAM. 2016 IEEE international conference on real-time computing and robotics, RCAR 2016, Angkor Wat, Cambodia, 6–10 June 2016. DOI: 10.1109/RCAR.2016.7784092.

15.

Doherty

Rosen

Leonard

(2022) Spectral Measurement Sparsification for Pose-Graph SLAM. arXiv:2203.13897v1.

16.

Dong

Nelson

Indelman

, et al. (2015) Distributed real-time cooperative localization and mapping using an uncertainty-aware expectation maximization approach. 2015 IEEE international conference on robotics and automation (ICRA), Volume 2015-June, Seattle, WA, USA, 26–30 May 2015. DOI: 10.1109/ICRA.2015.7140012.

17.

Donnat

Zitnik

Hallac

, et al. (2018) Learning structural node embeddings via diffusion wavelets. Proceedings of the 24th ACM SIGKDD International Conference on Knowledge Discovery & Data Mining. New York, NY, USA: ACM, 1320–1329. DOI: 10.1145/3219819.3220025.

18.

Dorfler

Bullo

(2013) Kron reduction of graphs with applications to electrical networks. IEEE Transactions on Circuits and Systems I: Regular Papers 60(1): 150–163. DOI: 10.1109/TCSI.2012.2215780.

19.

Ebadi

Chang

Palieri

, et al. (2020) LAMP: large-scale autonomous mapping and positioning for exploration of perceptually-degraded subterranean environments. Proceedings - IEEE international conference on robotics and automation, Paris, France, 31 May 2020. DOI: 10.1109/ICRA40945.2020.9197082.

20.

Egilmez

Ortega

(2014) Spectral anomaly detection using graph-based filtering for wireless sensor networks. 2014 IEEE international conference on acoustics, speech and signal processing (ICASSP), Ii, Florence, Italy, 4–9 May 2014. DOI: 10.1109/ICASSP.2014.6853764.

21.

Finder

Zohav

Ashkenazi

, et al. (2022) Wavelet Feature Maps Compression for Image-To-Image CNNs. Vancouver, BC: NeurIPS.

22.

Gorgel

Sertbaş

Kilic

, et al. (2009) Mammographic mass classification using wavelet based support vector machine. Istanbul University - Journal of Electrical and Electronics Engineering 9(1): 867–875.

23.

Hammond

Vandergheynst

Gribonval

(2011) Wavelets on graphs via spectral graph theory. Applied and Computational Harmonic Analysis 30(2): 129–150. DOI: 10.1016/j.acha.2010.04.005.

24.

Hammond

Vandergheynst

Gribonval

(2019) The spectral graph wavelet transform: fundamental theory and fast computation. Signals and Communication Technology. Berlin: Springer, 141–175. DOI: 10.1007/978-3-030-03574-7_3.

25.

Huang

Dragotti

(2022) WINNet: wavelet-inspired invertible network for image denoising. IEEE Transactions on Image Processing: A Publication of the IEEE Signal Processing Society 31: 4377–4392. DOI: 10.1109/TIP.2022.3184845.

26.

Hughes

Chang

Carlone

(2022) Hydra: a real-time spatial perception system for 3D scene graph construction and optimization. Robotics: Science and Systems (RSS). Piscataway, NJ: IEEE.

27.

Hutter

Gehring

Lauber

, et al. (2017) Anymal-toward legged robots for harsh environments. Advanced Robotics 31(17): 918–931.

28.

Indelman

(2018) Towards cooperative multi-robot belief space planning in unknown environments. Springer Proceedings in Advanced Robotics 2: 441–457. DOI: 10.1007/978-3-319-51532-8_27.

29.

Kaess

Johannsson

Roberts

, et al. (2012) ISAM2: incremental smoothing and mapping using the Bayes tree. The International Journal of Robotics Research 31(2): 216–235. DOI: 10.1177/0278364911430419.

30.

Karrer

Schmuck

Chli

(2018) CVI-SLAM—collaborative visual-inertial SLAM. IEEE Robotics and Automation Letters 3(4): 2762–2769. DOI: 10.1109/LRA.2018.2837226.

31.

Khattak

Nguyen

Mascarich

, et al. (2020) Complementary multi–modal sensor fusion for resilient robot pose estimation in subterranean environments. 2020 international conference on unmanned aircraft systems (ICUAS), Athens, Greece, 1–4 September 2020. DOI: 10.1109/ICUAS48674.2020.9213865.

32.

Khosoussi

Giamou

Sukhatme

, et al. (2019) Reliable Graphs for SLAM. London: Sage. DOI: 10.1177/0278364918823086

33.

Kim

Kaess

Fletcher

, et al. (2010) Multiple relative pose graphs for robust cooperative mapping. 2010 IEEE international conference on robotics and automation, Anchorage, Alaska, 3–8 May 2010. DOI: 10.1109/ROBOT.2010.5509154.

34.

Lajoie

Ramtoula

Chang

, et al. (2020) DOOR-SLAM: distributed, online, and outlier resilient SLAM for robotic teams. IEEE Robotics and Automation Letters 5(2): 1656–1663. DOI: 10.1109/LRA.2020.2967681.

35.

Loukas

(2019) Graph reduction with spectral and cut guarantees. Journal of Machine Learning Research 20: 1–42.

36.

Mangelson

Dominic

Eustice

, et al. (2018) Pairwise consistent measurement set maximization for robust multi-robot map merging. Proceedings - IEEE international conference on robotics and automation, Brisbane, Australia, 21–25 May 2018. DOI: 10.1109/ICRA.2018.8460217.

37.

Mangelson

Ghaffari

Vasudevan

, et al. (2019) Characterizing the uncertainty of jointly distributed poses in the lie algebra. IEEE Transactions on Robotics 36(5): 1371–1388. DOI: 10.1109/tro.2020.2994457.

38.

Mateos

Segarra

Marques

, et al. (2019) Connecting the dots: identifying network structure via graph signal processing. IEEE Signal Processing Magazine 36(3): 16–43. DOI: 10.1109/MSP.2018.2890143.

39.

Moon

Lee

(2020) Object-oriented semantic graph based natural question generation. 2020 IEEE International Conference on Robotics and Automation (ICRA). Piscataway, NJ: IEEE, 4892–4898. DOI: 10.1109/ICRA40945.2020.9196563.

40.

Nobili

Tinchev

Fallon

(2018) Predicting alignment risk to prevent localization failure. 2018 IEEE international conference on robotics and automation (ICRA), Brisbane, Australia, 21–25 May 2018. DOI: 10.1109/ICRA.2018.8462890.

41.

Ortega

Frossard

Kovacevic

, et al. (2018) Graph signal processing: overview, challenges, and applications. Proceedings of the IEEE 106(5): 808–828. DOI: 10.1109/JPROC.2018.2820126.

42.

Qin

Shen

(2018) VINS-mono: a robust and versatile monocular visual-inertial state estimator. IEEE Transactions on Robotics 34(4): 1004–1020. DOI: 10.1109/TRO.2018.2853729.

43.

Ramezani

Wang

Camurri

, et al. (2020) The newer college dataset: handheld lidar, inertial and vision with ground truth. 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway, NJ: IEEE, 4353–4360.

44.

Rao

Kim

Hwang

(2010) Fast fourier transform - algorithms and applications. Signals and Communication Technology. Dordrecht: Springer. DOI:10.1007/978-1-4020-6629-0.

45.

Ricaud

Borgnat

Tremblay

, et al. (2019) Fourier could be a data scientist: from graph Fourier transform to signal processing on graphs. Comptes Rendus Physique 20(5): 474–488. DOI: 10.1016/j.crhy.2019.08.003.

46.

Sandryhaila

Moura

(2014) Discrete signal processing on graphs: frequency analysis. IEEE Transactions on Signal Processing 62(12): 3042–3054. DOI: 10.1109/TSP.2014.2321121.

47.

Schmuck

Chli

(2019) CCM-SLAM: robust and efficient centralized collaborative monocular simultaneous localization and mapping for robotic teams. Journal of Field Robotics 36(4): 763–781. DOI: 10.1002/rob.21854.

48.

Schmuck

Ziegler

Karrer

, et al. (2021) COVINS: visual-inertial SLAM for centralized collaboration. 2021 IEEE international symposium on mixed and augmented reality adjunct (ISMAR-Adjunct), Bari, Italy, 8 October 2021. DOI: 10.1109/ISMAR-Adjunct54149.2021.00043.

49.

Schwertfeger

Birk

(2013) Evaluation of map quality by matching and scoring high-level, topological map structures. 2013 IEEE International Conference on Robotics and Automation. Piscataway, NJ: IEEE, 2221–2226. DOI: 10.1109/ICRA.2013.6630876.

50.

Shuman

Ricaud

Vandergheynst

(2016) Vertex-frequency analysis on graphs. Applied and Computational Harmonic Analysis 40(2): 260–291. DOI: 10.1016/j.acha.2015.02.005.

51.

Sundvall

Jensfelt

(2006) Fault detection for mobile robots using redundant positioning systems. Proceedings 2006 IEEE international conference on robotics and automation, 2006. ICRA 2006, Orlando, Florida, USA, 15–19 May 2006. DOI: 10.1109/ROBOT.2006.1642280.

52.

Tian

How

(2022) Spectral Sparsification for Communication-Efficient Collaborative Rotation and Translation Estimation. arXiv:2210.05020v4.

53.

Tian

Chang

Herrera Arias

, et al. (2022) Kimera-multi: robust, distributed, dense metric-semantic SLAM for multi-robot systems. IEEE Transactions on Robotics 38: 2022–2038. DOI: 10.1109/TRO.2021.3137751.

54.

Tranzatto

Miki

Dharmadhikari

, et al. (2022) CERBERUS in the DARPA subterranean challenge. Science Robotics 7(66): 9742. DOI: 10.1126/scirobotics.abp9742.

55.

Trefethen

Bau

IIID

(1997) Numerical Linear Algebra. Philadelphia, PA: Siam.

56.

Tremblay

Borgnat

(2014) Graph wavelets for multiscale community mining. IEEE Transactions on Signal Processing 62(20): 5227–5239. DOI: 10.1109/TSP.2014.2345355.

57.

Van Opdenbosch

Steinbach

(2019) Collaborative visual SLAM using compressed feature exchange. IEEE Robotics and Automation Letters 4(1): 57–64. DOI: 10.1109/LRA.2018.2878920.

58.

Zhang

Kaess

Singh

(2016) On degeneracy of optimization-based state estimation problems. 2016 IEEE international conference on robotics and automation (ICRA), Volume 2016-June, Stockholm, Sweden, 16–21 May 2016. DOI: 10.1109/ICRA.2016.7487211.

59.

Zhang

Hsiao

Dong

, et al. (2021) MR-iSAM2: incremental smoothing and mapping with multi-root Bayes tree for multi-robot SLAM. 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway, NJ: IEEE. DOI: 10.1109/IROS51168.2021.9636687.

60.

Ziegler

Karrer

Schmuck

, et al. (2021) Distributed Formation estimation via pairwise distance measurements. IEEE Robotics and Automation Letters 6(2): 3017–3024. DOI: 10.1109/LRA.2021.3062347.

61.

Zou

Tan

(2013) CoSLAM: collaborative visual SLAM in dynamic environments. IEEE Transactions on Pattern Analysis and Machine Intelligence 35(2): 354–366. DOI: 10.1109/TPAMI.2012.104.

A framework for collaborative multi-robot mapping using spectral graph wavelets

Abstract

Keywords

1. Introduction

2. Related work

2.1. Collaborative multi-robot mapping

2.2. Failure and degeneracy detection

2.3. Spectral graph and graph signal processing

3. Preliminaries

3.1. Fundamental graph theory review

3.2. Euclidean and graph spectral analysis review

4. Collaborative multi-robot mapping

4.1. Centralized mapping and localization

4.1.1. Global multi-robot graph

4.1.2. Graph reduction

4.2. Spectral analysis of graph signals

4.2.1. Graph comparison

4.3. Correcting onboard estimation

4.3.1. Updating the onboard graph

5. Experiments

5.1. EuRoC dataset: Validation and comparison

5.2. Analysis of robotic drift recovery

5.2.1. Localization recovery

5.2.2. Analysis of the constraint generation

5.3. Large-scale multi-robot subterranean exploration

5.4. Degeneracy in the state estimation

5.5. DARPA subterranean challenge

6. Conclusions

Footnotes

Acknowledgments

Declaration of conflicting interests

Funding

ORCID iDs

References