Fast Loop Closure Selection Method with Spatiotemporal Consistency for Multi-Robot Map Fusion

Abstract

This paper presents a robust method based on graph topology to find the topologically correct and consistent subset of inter-robot relative pose measurements for multi-robot map fusion. However, the absence of good prior on relative pose gives a severe challenge to distinguish the inliers and outliers, and once the wrong inter-robot loop closures are used to optimize the pose graph, which can seriously corrupt the fused global map. Existing works mainly rely on the consistency of spatial dimension to select inter-robot measurements, while it does not always hold. In this paper, we propose a fast inter-robot loop closure selection method that integrates the consistency and topology relationship of inter-robot measurements, which both conform to the continuity characteristics of similar scenes and spatiotemporal consistency. Firstly, a clustering method integrating topology correctness of inter-robot loop closures is proposed to split the entire measurement set into multiple clusters. Then, our method decomposes the traditional high-dimensional consistency matrix into the sub-matrix blocks corresponding to the overlapping trajectory regions. Finally, we define the weight function to find the topologically correct and consistent subset with the maximum cardinality, then convert the weight function to the maximum clique problem from graph theory and solve it. We evaluate the performance of our method in a simulation and in a real-world experiment. Compared to state-of-the-art methods, the results show that our method can achieve competitive performance in accuracy while reducing computation time by 75%.

1. Introduction

Various schemes of simultaneous localization and mapping (SLAM) have been proposed and applied in robotic communities, while most of these approaches focus more on a single robot SLAM system. Multi-robot SLAM is attracted by the fact that collaboration-based operations, such as exploration and mapping in large scenes, can be efficiently done by multiple robots in a cooperative manner, compared to a single robot [1,2]. The key to implementing multi-robot SLAM is to integrate the available information among robots to establish a global reference frame; thus, the robots can estimate where the robots are relative to each other and build a consistent global map [3].

Pose graph SLAM [4,5] formulates the map estimation problem as a factor graph and plays an attractive role in the mentioned map fusion process. Inter-robot relative pose measurements can be obtained by indirect observations or direct observations. In practice, indirect observation is more flexible and effective than direct observation, because it does not require encounters between robots. This allows different robots to repeatedly visit the same position at different times, which is more suitable for multi-robot systems. However, perception-derived indirect measurements are highly susceptible to perceptual aliasing due to the lack of good prior for relative pose measurements and thus produce false positive measurements. It is a troublesome issue in appearance-based place recognition systems, where two scenes are wrongly recognized as the same position even if they are far apart in the environment. Multi-robot SLAM system usually uses the inter-robot loop closures to optimize the pose graph and then fuse the local maps into a global map, while the false positive inter-robot loop closures may corrupt the back-end optimization results.

Following pose graph SLAM formulation, wrong inter-robot loop closures are called outliers. In [6], outliers are more commonly defined as “any observation in a set of data that is inconsistent with the remainder of the observations in that dataset”. Therefore, effectively identifying outliers and choosing a consistent subset of measurements is the key to fusing the local maps into a global map.

Existing methods follow the idea of building a high-dimensional consistency matrix at the level of the entire measurement set to find consistent subsets with high cardinality. Unfortunately, on the one hand, solving the maximum clique problem directly in the constructed high-dimensional consistency matrix takes a long time, which is not conducive to real-time performance. On the other hand, this method considers the consistency in the spatial dimension but not in the temporal dimension, while the neighboring constraints in the region where the trajectories overlap are also consistent in the temporal dimension. The topology relationship in the time dimension is a meaningful factor that should be considered for the inter-robot loop closure selection method.

As we know, the accurate inter-robot measurements show that robots repeatedly visit the same position at different times, meaning that the two trajectories are coincident within the regions. This is the case when the appearance-based SLAM front-end generates constraints. Once a constraint is detected, it is certain that other constraints will also be generated in its neighboring regions or adjacent time intervals due to the continuity characteristics of similar scenes. Further, a reasonable assumption can be made that the correct constraints are most likely to occur in regions with a high degree of coincidence between trajectories since both temporal and spatial dimensions are considered. On this basis, the high-dimensional consistency matrix can be decomposed into multiple matrix blocks to efficiently solve the consistency subset, significantly improving the real-time performance.

This paper proposes a fast inter-robot loop closure selection method with spatiotemporal consistency for multi-robot map fusion by integrating the graph topology of loop closures and consistency between constraints. Then, we introduce a clustering method that considers the degree of topology correctness of inter-robot loop closures, partitioning the entire measurement set containing inlier and outlier loop closures into clusters, where the constraints within the cluster are topologically correct. A weight function is defined to find the topologically correct and consistent subset with the maximum cardinality, in a block way, instead of the traditional constraint by constraint. Determining the solution to the weight function can be solved by transforming it into the maximum clique problem from graph theory. The experimental results show that our method can effectively find the proper pairwise internally consistent set for map fusion and has high real-time performance. Although some of the potential true measurements may be discarded, the experimental results demonstrate that the proposed method does not lose any important, meaningful information. The main contributions of our work can be summarized as follows:

•

We present a fast, robust method based on spatiotemporal consistency to find the topologically correct and consistent subset of inter-robot loop closure for map fusion.

•

Our method decomposes the traditional high-dimensional consistency matrix into sub-matrix blocks corresponding to the overlapping trajectory regions, improving the real-time performance greatly.

•

Compared to state-of-the-art methods, the results show that our method can achieve competitive performance in accuracy while reducing computation time by 75%.

The remaining paper is organized as follows. Section 2 gives a review of the literature. We present the formulation for the single robot pose graph and its extension to the multi-robot pose graph in Section 3. Section 4 presents a complete overview of our method. The experimental results are shown in Section 5. Lastly, Section 6 concludes this paper.

2. Related Works

SLAM has made significant development in the past 30 years [7,8,9]. As one of the indispensable modules, various typical SLAM frameworks (e.g., VINS [10], ORB-SLAM2 [11], LeGO-LOAM [12], Gr-LOAM [13]) have integrated loop closures modules to enhance the robustness of the system.

Nevertheless, the robots performing long tasks are prone to perceptual aliasing and generate incorrect loop closures, posing severe challenges for graph SLAM sensitive to outliers. Regarding the susceptibility of graph SLAM to wrong loop closures, many works try to establish a robust back-end optimizer to detect and filter outliers introduced by the front-end algorithms [14,15]. Realizing, reversing, and recovering algorithm [16] can realize that the appearance-based place recognition system has generated wrong constraints, remove them if required, and re-optimize the state estimation. A robust solver is proposed in [17] to address heavy-tailed measurement noise and develops convex relaxations for pose graph optimization that approximately solve nonconvex optimization problems via semidefinite programming. In order to build a globally consistent map, a robust back-end solver is proposed in [18], which disables inaccurate loop closures and changes part of the graph topology during optimization. The relative pose measurements are easy to estimate, for intra-robot loop closures, because they can be obtained by odometry or estimated from a visual frame or lidar frame matching algorithm [19,20,21]. In comparison, these methods for outliers rejection are not suitable for multi-robot SLAM.

The vital issue of multi-robot SLAM is relative pose estimation, and most existing works deal with this issue by analyzing the inter-robot loop closures and then detecting and filtering the outliers. Researchers present a comprehensive review to illustrate the challenges for multi-robot SLAM map fusion [22,23], many of which attempt to propose a robust loop closure method to address the problem of outliers introduced by perceptual aliasing and perform map fusion. For initialization problems, the concept of “anchor nodes” is proposed to convert individual nodes of each pose graph into a global frame [24]. The pairwise consistency maximization (PCM) method [25] applies the binary switch variables as weights to distinguish whether each pair of measurements is consistent. It then utilizes the maximum clique problem derived from graph theory to determine the largest pairwise internally consistent set. Following the solution of PCM, the definition of proposes the maximum edge weight clique (MEWC) integrating consistency and data similarity is proposed and then applies them as the weight for the objective function [26]. The expectation maximization algorithm is an effective solution for multi-robot pose graph localization [27]. It is capable of inferring the initial relative pose of the robot and solving the problem of multi-robot data association, in the absence of prior knowledge about the initial relative pose measurements. When there is overlap between two maps, those landmarks that appear repeatedly can provide additional information that is added to the optimization process in the form of constraints to improve alignment accuracy [28]. Some researchers propose to construct a semantic map in the unknown environment based on the rich features of visual images for map fusion between robots [29,30]. DOOR-SLAM [31] is a typical distributed SLAM solution with low communication bandwidth, which integrates the PCM module at the back-end and includes a distributed SLAM front-end to detect inter-robot loop closures. Distributed systems often rely on additional communication channels to exchange maps for feature data, making it more challenging to implement. The mentioned solutions do not consider the temporal dimension making it impossible to determine the topology relationship of the measurements. In addition, as the definition of consistency only involves the spatial dimension, this case does not conform to the spatiotemporal consistency.

3. Multi-Robot SLAM

Thanks to the development of the SLAM technology, pose estimation for single robot SLAM can be solved effectively through practical optimization algorithms such as g2o [32], iSAM2 [33], and HOG-Man [34]. A set of measurements $\mathbf{Z}$ consists of odometry measurements with sequential nodes ${\mathbf{z}}_{ij}(j=i+1)$ and loop closure measurements ${\mathbf{z}}_{ij}(j\ne i+1)$ with non-sequential nodes. Without loss of generality, the measurements are also named constraints. Pose graph SLAM aims to estimate the full trajectory of robots, which can be generalized as the maximum a posteriori (MAP) problem as below:

$$\begin{array}{c}\hfill {\mathbf{X}}^{*}=\underset{\mathbf{x}}{argmax}P(\mathbf{X}\mid \mathbf{Z})=\underset{\mathbf{x}}{argmin}\{\underset{\mathrm{Odometry}\mathrm{Constraints}}{\underbrace{\sum _i{∥{\mathbf{x}}_{i+1}-f\left({\mathbf{x}}_i,{\mathbf{z}}_{ij}\right)∥}_{{\mathsf{\Sigma }}_i}^2}}+\underset{\mathrm{Loop}\mathrm{Closure}\mathrm{Constraints}}{\underbrace{\sum _{ij}{∥{\mathbf{x}}_j-f\left({\mathbf{x}}_i,{\mathbf{z}}_{ij}\right)∥}_{{\mathsf{\Lambda }}_{ij}}^2}}\}\end{array}$$

(1)

where $\mathbf{X}={\left({\mathbf{x}}_0,{\mathbf{x}}_1,\dots {\mathbf{x}}_i,\dots {\mathbf{x}}_j,\dots \right)}^T$ presents the robot trajectory that consists of all pose nodes ${\mathbf{x}}_i\in \mathrm{SE}\left(3\right)$ or ${\mathbf{x}}_i\in \mathrm{SE}\left(2\right)$ at the time $i,f(·)$ is the nonlinear motion model, ${\sum }_i$ and ${\mathsf{\Lambda }}_{ij}$ are the odometry covariance and loop closure covariance, respectively.

The pose graph SLAM formulation mentioned above can be further extended to multi-robot SLAM for fusing local maps to a global map. In the absence of a common reference frame, the anchor nodes are introduced to estimate the relative pose transformation between the local coordinate frames [24]. Based on all measurements, the multi-robot SLAM can be formulated as

$${\mathbf{X}}^{*},{\mathbf{T}}^{g^{*}}=\underset{\mathbf{X},{\mathbf{T}}^g}{\underbrace{argmax}}P\left(\mathbf{X},{\mathbf{T}}^g\mid \mathbf{Z},{\mathbf{Z}}^{ab}\right).$$

(2)

For the convenience of description, let us assume that the scene contains two robots. $\mathbf{X}=\left\{{\mathbf{X}}^a,{\mathbf{X}}^b\right\}$ is the set of robot trajectories representing the trajectory ${\mathbf{X}}^a$ of robot a and the trajectory ${\mathbf{X}}^b$ of robot b. $\mathbf{Z}=\left\{{\mathbf{Z}}^a,{\mathbf{Z}}^b\right\}$ is the set of intra-robot loop closure corresponding to the measurements ${\mathbf{Z}}^a$ of robot a and the measurements ${\mathbf{Z}}^b$ of robot b. ${\mathbf{Z}}^{ab}=\left\{{\mathbf{z}}_{ij}^{ab}\mid {\mathbf{z}}_{ij}^{ab}\in SE\left(2\right)\mathrm{or}SE\left(3\right)\right\}$ represents the set of inter-robot loop closures, and its element ${\mathbf{z}}_{ij}^{ab}$ refers to the relative measurement of robot a at the time i and robot b at time j. The variable ${\mathbf{T}}^g=\left\{{\mathbf{T}}_a^g,{\mathbf{T}}_b^g\mid {\mathbf{T}}_a^g,{\mathbf{T}}_b^g\in SE\left(2\right)\mathrm{or}SE\left(3\right)\right\}$ denotes the relative transformation of robot b and robot a to the global frame, respectively.

Inter-robot loop closures usually involve wrong measurements due to the lack of a prior for the initial transformation. Existing work does not filter the inaccurate constraints from the entire measurement set but attempts to select a subset that are free of wrong loop closures, and it also names consistency [25]. In this paper, we adopt the metric proposed in [35] to define the consistency

$$c\left({\mathbf{z}}_{ik}^{ab},{\mathbf{z}}_{jl}^{ab}\right)={∥\left(\boxminus {\mathbf{z}}_{ik}^{ab}\right)⊞{\widehat{\mathbf{x}}}_{ij}^a⊞{\mathbf{z}}_{jl}^{ab}⊞{\widehat{\mathbf{x}}}_{lk}^b∥}_{\mathsf{\Sigma }}.$$

(3)

where the notations ⊞ and ⊟ are used for pose composition and inversion [36], respectively, and ${\widehat{\mathbf{x}}}_{ij}^a$ is the pose estimation of robot a from the time i to time j. For inter-robot loop closures, we make some necessary explanations for the convenience of description. In (3), assume that node i and node j belong to the trajectory of robot a, the node k and node l belong to robot b. $t_i$ denotes the timestamp related to node i. We further assume that robot poses ${\mathbf{x}}_{ij}^a$ on a single trajectory are directed in time, where transformations are from past node to current $\left(t_j>t_i,t_l>t_k\right)$. The underlying hypothesis of consistency is that the measurements in the selected subset are consistent with each other in the graph, implying that the error is within a certain statistical threshold. Mathematically, for a set $\tilde{\mathbf{Z}}$ to be pairwise internally consistent, it can be formulated as

$$c\left({\mathbf{z}}_{ik}^{ab},{\mathbf{z}}_{jl}^{ab}\right)\le {\chi }_{\alpha ,{\delta }_c}^2,\forall {\mathbf{z}}_{ik}^{ab},{\mathbf{z}}_{jl}^{ab}\in \tilde{\mathbf{Z}},$$

(4)

where ${\parallel ·\parallel }_{\mathsf{\Sigma }}$ is the Mahalanobis distance, therefore the threshold follows the chisquared distribution (using the ${\chi }^2$ threshold with $\alpha =0.1$), ${\delta }_c$ is the degrees of freedom of the graph.

4. Proposed Method

Due to the presence of wrong constraints, not all the measurements ${\mathbf{Z}}^{ab}$ are inserted into the pose graph, and only a part of the available ones can be accepted. PCM seeks to find the largest subset of inter-robot measurements ${\mathbf{Z}}^{ab}$ that satisfies the consistency. Based on the PCM, the MEWC method [26] determines such a subset with a high degree of cardinality, consistency, and data similarity. However, exiting works on the consistency of measurements are usually based on spatial dimension in (3). In fact, the accurate measurements are also topologically (i.e., time) correct between two trajectories to a certain extent, while the wrong ones do not hold. Given the inter-robot loop closures, the degree of coincidence corresponding to the trajectories of two robots can be used to further assess the consistency of the measurements. This case is reflected in the time dimension and has not yet been introduced into the concept of consistency. Therefore, we propose a fast loop closure selection method that integrates the essentials of spatiotemporal consistency. Specifically, our method considers the degree of topology correctness, cardinality, and consistency of inter-robot measurements.

4.1. Topologically Related Loop Closures

Following the definition of consistency proposed in [25], we know those accurate measurements ${\mathbf{Z}}^{ab}$ are consistent with each other and further satisfy the consistency metric (3). In terms of topology, accurate inter-robot loop closures are also all topologically correct with each other, which refers to the sequence of measurements related to the overlapped regions of robot trajectories. However, perceptual aliasing is nearly inevitable in natural environments and can lead to false loop closures that will be mixed into the measurement set as part of the measurement. This issue drives us to distinguish between true and false measurements at the initial stage. The potential fact is that the false measurements may be inconsistent or topologically incorrect.

The definition of consistency allows us to determine which subset is trusted from the perspective of entire measurement set in the inter-robot ${\mathbf{Z}}^{ab}$, whereas it cannot be used to evaluate the degree of coincidence between two trajectories. It is certain that the presence of measurements ${\mathbf{Z}}^{ab}$ means that the robots repeatedly observed the same location at different moments or by different robots. The distribution of inter-robot loop closures, to a certain extent, agrees with the distribution of regions where the two robot trajectories overlap. Accordingly, we seek to determine which portions of the two trajectories are overlapped, where the overlapped portions only consist of topologically related links (sequences of loop closures that relate coincident parts of the robot trajectories). For the sake of brevity, we embody each of these portions as a cluster. The measurements ${\mathbf{Z}}^{ab}$ consist of the several clusters such that

$${\mathbf{Z}}^{ab}=\left\{\bigcup _{c=1}{\mathbf{Z}}_c^{ab}\mid {\mathbf{Z}}_m^{ab}\cap {\mathbf{Z}}_n^{ab}=⌀,\forall (m\ne n)\right\}$$

(5)

Assuming that if there are no false constraints in the measurement set, the constraints within each cluster agree with each other and are topologically correct, the inter-clusters are mutually consistent, and the true subset of the inter-robot measurements should consist of all clusters. If the front-end introduces false measurements, the subsets can only be composed of partial constraints within clusters. Once the measurement set is divided into multiple clusters by the proposed method, it allows to search the entire measurement set in a block-wise way, instead of the traditional one-constraint-by-constraint, and then segmentally identify the regions of loop closures. In this paper, we demonstrate that the proposed clustering method is feasible and practical since the idea of segmentation conforms to the characteristics of the continuity of similar scenes.

Now, we explain how the proposed method divides the measurement set into multiple clusters consisting of topologically related loop closures. As previously described, the cluster is initialized and maintained according to the timestamp of measurements, which belongs to the time category. We initialize a cluster ${\mathbf{Z}}_c^{ab}=\left\{{\mathbf{z}}_{ik}^{ab}\right\}$ with the measurement ${\mathbf{z}}_{ik}^{ab}$ generated by the front-end. Next, the formulation (6) is introduced to evaluate whether the new measurement ${\mathbf{z}}_{jl}^{ab}$ belongs to an existing cluster or reinitializes a new cluster.

$$\left\{\exists {\mathbf{z}}_{jl}^{ab}\in {\mathbf{Z}}_c^{ab}:∥t_i-t_j∥\le t_g\oplus ∥t_k-t_l∥\le t_g\right\}$$

(6)

where $t_g$ is the threshold to distinguish cluster area, which is set by the running frequency of the front-end place recognition. The operator ⊕ is AND operation in the Boolean logic function. Specifically, once the first measurement ${\mathbf{z}}_{ik}^{ab}$ generates in the front-end, it assembles a new cluster. When the second measurement ${\mathbf{z}}_{jl}^{ab}$ generates, both node j and node l of this measurement should be within threshold $t_g$ from node i and node k respectively of some measurements already present in the cluster, in order to be inserted to the same cluster. If not, a new cluster will be created comprising this measurement.

Building on this solution, only measurements that link the overlapped portions of trajectories are allowed to construct the corresponding clusters. As measurements are continuously generated by the front-end, they will be dynamically assigned to already existing clusters if they satisfy the criterion in (6); otherwise, new clusters are formed. For example, a set of public datasets New College [16] was run to illustrate the effectiveness of the proposed clustering method for topology correctness of inter-robot constraints, as shown in Figure 1. The dataset was split into two parts representing two completely independent pose graphs. We draw the two original 2D trajectories and measurements in a 3D form for readability, and the scale of the z-axis has no meaning in the plot. Our method successfully processed the raw inter-robot measurements into multiple clusters, where the largest cluster includes 93 constraints and the smallest one consists of only one measurement. The threshold $t_g$ was set to 50, where the timestamp interval between measurements was smaller than the threshold to be believed the same cluster. To maintain the scale of the cluster, the cluster is believed completed if no measurement is inserted within the threshold period.

A larger cluster means that the two trajectories have a higher degree of coincidence; there is a greater probability that the robot will repeatedly pass the same place. Because similar scenes usually appear in the form of local regions, rather than flashing by in the form of isolated points. Following this logic, clusters that are scattered and consist of only one or two constraints are more likely to be outliers. Based on the above analysis, we can make a reasonable assumption as follows.

Assumption 1.

The scale of the cluster is positively correlated with the coincident area between two trajectories and is more likely to dominate the distribution of the pairwise internally consistent set.

Our method requires that every pair of measurements ${\mathbf{z}}_{jl}^{ab}$ and ${\mathbf{z}}_{ik}^{ab}$ are topologically correct, i.e., consistent in the temporal dimension, and consistent in the spatial dimension. Accordingly, a weight function is defined to evaluate the clusters and then estimate the proper pairwise internally consistent set ${\mathbf{Z}}^{*}$ as follows.

$${\mathbf{Z}}^{*}=\underset{\mathbf{Z}\in {\mathbf{Z}}^{ab}}{argmax}\sum _{{\mathbf{z}}_{jl}^{ab},{\mathbf{z}}_{ik}^{ab}\in {\mathbf{Z}}^{ab}}h\left({\mathbf{z}}_{jl}^{ab},{\mathbf{z}}_{ik}^{ab}\right)\oplus c\left({\mathbf{z}}_{jl}^{ab},{\mathbf{z}}_{ik}^{ab}\right)$$

(7)

where $h\left({\mathbf{z}}_{jl}^{ab},{\mathbf{z}}_{ik}^{ab}\right)$ is to calculate whether the two constraints are topologically correct, $c\left({\mathbf{z}}_{jl}^{ab},{\mathbf{z}}_{ik}^{ab}\right)$ is to compute the pairwise consistency value. The maximization of the weight function means that the subset should include as many inter-robot measurements as possible, which is consistent with the existing assumptions. Meantime, the larger the cumulative sum of weight function, the more measurements that satisfy the topology correctness at the same time, and the case is compatible with the proposed assumption in this paper. The formulation (7) estimates the topologically correct and consistent set with the maximum cardinality. It is reasonable to find the pairwise internally consistent set in the region where the coincident segment of the two trajectories is the largest without prior knowledge. Inspired by [25,26], ${\mathbf{Z}}^{*}$ can be calculated by introducing graph theory to solve the Formulation (7) and then adding it into the existing SLAM back-end optimizer for map fusion.

4.2. Graph Theory for Clique

In this section, we first perform the outlier rejection for a pair of pose graphs and then conduct to merge the local robot maps into a global map. Before starting, some basic concepts of graph theory are presented for ease of description. In an undirected graph $G=\{V,E\}$, an edge $e(i,j)$ connects two vertices, i and j, where $e\in E$ and $i,j\in V$. The graph may contain more than one clique $G_c=\left\{V_c,E_c\mid V_c\subseteq V,E_c\subseteq E\right\}$. The clique is a subset of the graph in which there is an edge between each pair of vertices, and the maximum clique is the one with the most vertices in such a subset in the graph.

By leveraging the associated definitions of cliques, we discuss how the problem defined in (7) is reformulated into the maximum clique problem using a small case. Assume that there are two pose graphs containing 13 inter-robot loop closures, which constitute the entire measurement set ${\mathbf{Z}}^{ab}$, as shown in Figure 2a. According to the clustering method, it can be seen that there are three overlapped segments between trajectories.

Based on the previous assumption, accurate inter-robot measurements are consistent and topologically correct. The proposed clustering method based on (6) introduces the topology dimension to segment the entire measurement set that contains outliers and inliers into multiple clusters. We redefine function $h\left({\mathbf{z}}_{jl}^{ab},{\mathbf{z}}_{ik}^{ab}\right)$ in (7) to evaluate the topology relationship of inter-robot measurements as follows.

$$h\left({\mathbf{z}}_{jl}^{ab},{\mathbf{z}}_{ik}^{ab}\right)=\left\{\begin{array}{cc}1,\hfill & \hfill {\mathbf{z}}_{jl}^{ab},{\mathbf{z}}_{ik}^{ab}\in {\mathbf{Z}}_c^{ab}\\ 0,\hfill & \hfill \mathrm{others}.\end{array}\right.$$

(8)

Each cluster ${\mathbf{Z}}_c^{ab}$ corresponds to a topology matrix ${\mathbf{H}}_c$, which is used to store the topology values in (8). The matrix ${\mathbf{H}}_c$ is a symmetric matrix whose main diagonal elements are zero as the topology with itself is meaningless; the rest of the elements are $h_{i,j},(i\ne j)$, as shown in Figure 2b. The proposed clustering method divides the measurement set into two clusters of different scales following the definition of topology correctness. On the one hand, inter-robot loop closures tagged 6, 7, 12, and 13 are not included in these two clusters because they do not satisfy the criterion of topology correctness. On the other hand, for those isolated inter-robot measurements, those smaller clusters consisting of only one or two measurements can generally be considered outliers based on the above analysis of the continuity of similar scenes. Therefore, the cluster consisting of inter-robot loop closure tagged 8 is likely to be outliers and can be discarded. These discarded measurements are no longer used for subsequent consistency evaluation.

Our method takes each cluster as the basic unit to calculate its corresponding pairwise consistency values. Similar to matrix ${\mathbf{H}}_c$, a new matrix is constructed to store the pairwise consistency values calculated in (4). According to the definition of an undirected graph, this matrix is also symmetric, and the elements on the main diagonal are all zeros because the consistency with itself has no practical meaning. Next, all items $m_{i,j}$ in the matrix that are smaller than the threshold are reset to zeros, indicating that these measurements do not meet the definition of consistency with each other. The matrix is redefined as a topologically correct and consistent matrix ${\mathbf{M}}_c$, as shown in Figure 2c.

The latest matrix ${\mathbf{M}}_c$ can be further transformed into its corresponding undirected graph in the form of an adjacency matrix, as shown in Figure 2d. It can be seen that each topologically correct and consistent set corresponds to a clique in the graph, and accordingly, the definitions of its nodes and edges have also changed. Nodes in an undirected graph represent inter-robot measurements, and two nodes are connected by an edge if and only if the corresponding element in the matrix ${\mathbf{M}}_c$ is not zero. Considering that the clique is fully connected, all inter-robot measurements represented by nodes in the clique are pairwise internally consistent. In other words, the topologically correct and consistent subset with the largest cardinality in the graph is the solution to the weight function, and this subset has the highest cumulative sum. Finally, the inter-robot loop closures corresponding to the selected subset ${\mathbf{Z}}^{*}$ are represented in the pose graphs, as shown in Figure 2e. Now, the solution of the weight function has been transformed into the maximum clique problem.

In graph theory, finding the maximum clique in a given undirected graph is the maximum clique problem. We select the method proposed in [37] for the maximum clique problem because it is easy to perform and has been open-sourced. Once the proper loop closure selection is solved, the trusted subset of inter-robot measurements can be inserted into the existing SLAM back-end optimizer for map fusion, such as factor graph and graph optimization.

Although the focus of this paper is to discuss map fusion in terms of two pose graphs, this solution can seamlessly be extended and applied to multiple pose graphs. We first compute the topology and consistency matrices of each pair of local maps, and then construct the corresponding adjacency matrix and solve for the maximum clique to determine the proper subset of the inter-robot loop closure set. Next, the two trajectories with the highest degree of coincidence and consistency are fused into a new map according to the selected subset, and the newly fused map is used as a new local map. This process can be performed iteratively until all local maps are merged into a global map. In the process of map fusion, it should be noted that the local maps with the highest degree of coincidence and consistency are preferentially selected each time, which can maintain the certainty of map fusion.

5. Experiments Evaluation

In this section, To verify the practicability of our method in the natural environments, several sets of experiments in two different scenarios were carried out based on the self-built mobile robot platform to determine whether the inter-robot measurements are accurate or not. Figure 3 shows the physical platform that a Leishen C-16 lidar and a MicroStrain 3DM-GX3-25 IMU are installed on the ground mobile robot-TIABBOT TOM08Q2. We also implemented two other state-of-the-art methods, including PCM and MEWC, to compare the simulation and experimental results with our method. All the experiments and simulations are run in C++ and executed on an Intel NUC equipped with a Core i7-7567 CPU and 8Gib memory using the robot operating system (ROS) in Ubuntu 18.04 Linux.

The system framework of the multi-robot map fusion algorithm is shown in Figure 4. The system consists of three main modules: the local single-robot SALM module, the inter-robot loop closure detection module, and the multi-robot pose graph optimizer. Each robot builds its local map, and the inter-robot loop closure detection module estimates the relative pose among robots. The robust inter-robot loop closure selection method proposed in this paper selects the proper subset of measurements that satisfy the conditions. The measurements contained in the subset are then inserted into the optimizer to fuse local maps into a global map.

In order to qualitatively analyze the experimental results, the SLAM accuracy assessment tool evaluation of odometry (EVO) [38] is used to calculate the absolute trajectory error (ATE). In terms of accuracy, we use ATE as the metric to evaluate the maps generated by existing algorithms. We introduced some basic concepts in the loop closure, where TP, TN, FP, and FN correspond for true positive, true negative, false positive, and false negative, respectively. The true positive rate (TPR) and false positive rate (FPR) are defined as follows:

$$TPR=\frac{TP}{TP+FN},FPR=\frac{FP}{FP+TN}$$

(9)

5.1. Simulations in Synthetic Datasets

The main goal of the first set of simulations is to demonstrate the effectiveness of the proposed method and to further qualitatively evaluate the performance of the methods on varying numbers of outliers. To simulate the scenarios of two pose graphs, we divided a complete robot trajectory released with dataset CASIL [39] into two independent segments representing pose graphs. In this way, a part of intra-robot loop closures is converted to inter-robot loop closures. Therefore we can quickly determine whether an inter-robot constraint is accurate or not as the relative pose between two robots is known. The newly inter-robot measurements that link two pose graphs were used as inliers. We added randomly generated inaccurate measurements to the pose graphs to evaluate the performance of methods under the variable number of outliers, where the number of outliers was from 0 to 250 with a step of 50. It is necessary to explain that some of the outliers are the false positive loop closures corresponding to perceptual aliasing, and the other is randomly produced with random mean and covariance. In our simulations, the threshold $t_g$ was set to 50 to distinguish cluster. Lastly, based on the selected trust subset of inter-robot measurements, the pose graphs were optimized with open-source iSAM [33] and the concept of anchor node [24]. Figure 5 presents the maps estimated by our method.

Table 1. Simulation Results: CASIL.

	Outliers	Ave Time (s)	Max Size of Cluster	Num of Cluster	TPR	FPR
	50	0.09	28	14	0.35	0.00
	100	0.10	30	20	0.35	0.00
Our	150	0.13	36	21	0.33	0.00
	200	0.15	40	20	0.29	0.00
	250	0.24	62	19	0.26	0.00
PCM	250	1.35	–		0.70	0.00

quantitatively shows the numerical results of the estimated maps shown in Figure 5. According to the results, we use three metrics to evaluate the performance of the methods in detail, including time, TPR, and FPR. In addition, there are also two related metrics, the maximum size cluster and number of clusters, to describe clusters generated by our method in Table 1. The maximum size cluster is the one that contains the most inter-robot measurements.

As the number of outliers increases, the number of clusters gradually increases to 21 first, and then reduces to 19 when the number of outliers is 250. The reason is that those originally isolated clusters are associated with gradually increasing outliers, and then multiple discrete measurements are merged into a new cluster, so the number of clusters first increases and then decreases. In terms of computation time, our method takes clusters as the basic unit to estimate the topologically correct and consistent subset, dividing the entire measurement set into multiple sub-matrix blocks for solving. Compared to existing methods for building a high-dimensional consistency matrix over the entire measurement set, our method reduces the optimization time by 82%. The TPR of our method is lower than PCM, and this case is in accordance with the assumption proposed in this paper. Our method selects the pairwise internally consistent set from the region where the two trajectories overlap the most rather than the entire measurement set. In the following simulations, we will show that this assumption is feasible and meaningful.

This set of simulations is used to compare the performance of the methods in the presence of dense inter-robot measurements. We evaluate the consistency and accuracy of methods by running the manhattan dataset [40], which simulates a world with 3500 nodes and 2099 ground truth loop closures. Similarly, we split the single complete trajectory generated on a full dataset into two pose graphs. The number of inter-robot loop closures is not a fixed value but is related to the respective lengths of the two trajectories. In this simulation, the number of inliers was 140, and we generated two versions of the dataset, including 200 outliers and 500 outliers, respectively, based on the number of outliers. Ten random experiments were conducted for each level of outliers to compare the estimated maps. Figure 6 presents the sample plots of the estimated maps. Intuitively, all three methods effectively reject outliers in dense inter-robot measurements. Once the outliers introduced by perceptual confusion are accepted, they corrupt the consistency of the fused map.

We compared the results of multiple experiments, as shown in

Table 2. Simulation Results: manhattan.

Method	Outliers	Ave Time (/s)	TPR	FPR	Ave ATE (/m)	Max ATE (/m)
PCM	200	19.81	0.98	0.01	0.27	0.60
	500	53.73	0.92	0.008	0.37	0.62
MEWC	200	19.75	0.98	0.008	0.29	0.60
	500	53.90	0.92	0.008	0.37	0.62
Our	200	2.61	0.59	0.005	0.30	0.60
	500	3.51	0.54	0.002	0.36	0.67

. In terms of real-time, when the number of outliers is 500, the computation time of both PCM and MEWC methods exceeds 50 s, while our method only takes 3.5 s, which reduces the computation time by 93%. In addition, PCM and MEWC determine consistent subset of inter-robot loop closures at the level of the entire measurement set, and the corresponding TPR both exceed 0.9, which is higher than the TPR estimated by our method. Note that the similarity in MEWC is all set to 1, as the dataset does not provide data similarity of loop closures.

Analysis of the above two sets of simulation results, Table 1 and Table 2, both prove that different numbers of outliers will have an impact on TPR, FPR, and time:

(1) Associating TPR and ATE for analysis, the worst result of TPR corresponding to our method is 0.54 in Table 2. However, the average ATE between the global map estimated by our method and the ground truth is only 0.36, which is approximately the same as the ATE corresponding to the PCM and MEWC. The quantitative experimental result is transformed into an intuitive map for comparison. The EVO [38] is used to compare the trajectory estimated by our method and the ground truth of the manhattan dataset and calculate the statistics of the whole trajectory, as shown in Figure 7. The results are suitable for testing the global consistency of the trajectory, and it can be seen that the maximum error in this experiment does not exceed 0.5.

(2) For existing methods, the larger the scale of the measurement set, the higher the corresponding consistency matrix dimension, and the longer the time required to solve the high-dimensional matrix. Our method takes the cluster corresponding to the trajectory overlap region as the basic unit to estimate the consistency subset. It decomposes the high-dimensional matrix into multiple sub-matrix blocks, significantly reducing the computational time required to solve the maximum clique problem.

Combined with the experimental results, it can be concluded that the lower TPR means that our method discards a part of the potential true inter-robot loop closures, but the lower ATE demonstrates that the proposed method does not lose any important, meaningful information.

5.2. Real-World Experiments

The primary sensor of the front-end place recognition system was a 3D lidar, where the iterative closest point (ICP) [21] algorithm was performed to calculate the inter-robot loop closures. For MEWC, the residual generated by ICP was used to represent the degree of data similarity between two frames.

In this experiment, one of the challenges comes from the lack of ground truth, so we collected a complete dataset in the same scene, as a reference metric, to provide a global trajectory for the local maps. The metric has no other practical meaning, as this section aims to verify the algorithm’s performance for outlier rejection of inter-robot loop closures, not navigation. We collected multiple local maps of this scene at different moments for each robot, where every pair of local maps contained repeatedly visited areas to generate potential inter-robot measurements. In addition, the end position of robot 1 is the starting position of the trajectory of robot 2, which is convenient for quantitative analysis because, theoretically, these two positions should be coincident in the fused map. We took the deviation between these two positions in the fused map as a new evaluation metric. The complete datasets were run multiple times to determine the number of loop closures within the overlapping region of the two maps. Figure 8 represents two independent local robot trajectories, and solid green lines represent inter-robot loop closures. From the local maps, the starting positions of two trajectories are coincident due to the lack of prior information on relative measurements.

The self-collected dataset is not as large as the synthetic dataset, so we reset the threshold $t_g$ to 15 to define the clusters, making it more suitable for real situations. Likewise, to increase the difficulty of outlier rejection, we added 30 outliers generated randomly with random mean and covariance to the measurement set. Figure 9 and Figure 10 shows example plots of the map fusion results. For convenience, the dataset corresponding to robot 1 is abbreviated as session 1, and robot 2 is referred to as session 2.

Figure 9a and Figure 10a represent the complete trajectories of the two experimental scenarios, respectively, as global references. Figure 9 shows the test results of the different algorithms in scene 1, where the red and blue lines indicate the local pose graphs of the two robots, and the thick green lines represent the inter-robot measurements corresponding to the selected subset. Then, the subset of inter-robot measurements selected by each algorithm can be inserted into the existing SLAM back-end optimizer for map fusion. The maps estimated by MEWC, PCM, and our method are shown in Figure 9b–d. Although there is a certain deviation between the red square and blue triangle in the fused map, it can be intuitively seen that the three methods are succeeded in map fusion, and the fused map approximately conforms to the reference trajectory. This case indicates that all three methods consider and apply the concept of consistency to select measurement subset. Next, we quantitatively analyze the performance of the algorithm by evaluating the experimental results. Due to the lack of ground truth in the experiments, ATE cannot be used as an available evaluation metric. The performance of the algorithms is evaluated based on four indicators: time, FPR, TPR, and deviation mentioned above, as shown in

Table 3. Results of real-world experiments.

Method	Scene	Time (s)	TPR	FPR	Deviation Trans (/m)	Deviation Rot (/rad)
PCM	Scene 1	19.81	0.80	0.03	0.8332	0.1260
	Scene 2	53.73	0.13	0.00	4.3790	0.0890
MEWC	Scene 1	19.75	0.60	0.00	0.6331	0.0169
	Scene 2	53.90	0.33	0.00	4.1805	0.0840
Our	Scene 1	2.61	0.80	0.00	0.6232	0.0169
	Scene 2	3.51	0.13	0.00	4.1110	0.0890

.

In the case of approximately the same deviation, our method takes the shortest computation time of 0.01 s while MEWC expenses the longest 0.071 s. For our method, the high-dimensional consistency matrix corresponding to the entire measurement set is decomposed into multiple sub-matrix blocks for solving, improving the real-time performance greatly. Second, MEWC considers consistency and introduces data similarity between frames as one of the weights, so the cardinality of the pairwise internally consistent set is not always the largest, which corresponds to its lower TPR. As PCM assumes that all constraints satisfying the threshold have the same weight, it has to determine the true subset at the level of the entire measurement set and cannot always reject false positive measurements caused by perceptual aliasing. The PCM method accepted a false positive measurement in scene 1, exacerbating the deviation between the positions corresponding to the red squares and blue triangles in the global map.

In the absence of prior information, not every inter-robot measurement is meaningful for map fusion. Combined with the simulation results, the experiments in this section again verify the feasibility of our method. Although there is a part of potential inter-robot measurements is discarded following the topologically correct and consistent subset proposed in this paper, our method does not lose any important, meaningful information and achieves high real-time.

6. Conclusions

In this paper, we introduced a robust inter-robot of measurements for map fusion in multi-robot SLAM in the absence of prior knowledge of relative pose. Considering the continuity characteristics of similar scenes, we make an assumption that the pairwise internally consistent set should be with a high degree of consistency, cardinality, and topology correctness of inter-robot loop closures. A newly defined clustering method is used to evaluate the topology relationship of inter-robot measurements. We exploit the proposed weight function to find an appropriate subset that is topologically correct and consistent, convert it to a clique problem in the graph, and solve it. Simulation and experimental results show that our method is more competitive in accuracy and time, compared to existing methods such as MEWC and PCM. In future work, we will focus on map fusion in multi-robot SLAM in large-scale scenarios and collaborative map fusion of more than two robots.