Robust Loop Closure Selection Based on Inter-Robot and Intra-Robot Consistency for Multi-Robot Map Fusion

Abstract

In multi-robot simultaneous localization and mapping (SLAM) systems, the system must create a consistent global map with multiple local maps and loop closures between robot poses. However, false-positive loop closures caused by perceptual aliasing can severely distort the global map, especially in GNSS-denied areas, where a good prior of relative poses between robots is unavailable. In addition, the performance of the consistency metric in existing map fusion methods relies on accurate odometry from each robot. However, in practice, cumulative noise is inevitably present in robot trajectories, which leads to poor map fusion with existing methods. Thus, in this paper, we propose a robust consistency-based inter-robot and intra-robot loop closure selection algorithm for map fusion. We consider both pairwise-loop consistency and loop-odometry consistency to improve robustness against false-positive loop closures and accumulative noise in the odometry. Specifically, we select a reliable inter-robot loop closure measurement with a consistency-based strategy to provide an initial prior of relative pose between two robot trajectories and update the pose variables of the robot trajectories. The loop closure selection problem is formulated as a maximum edge weight clique problem in graph theory. A performance evaluation of the proposed method was conducted on the ManhattanOlson3500, modified CSAIL and Bicocca datasets, and the experimental results demonstrate that the proposed method outperforms the pairwise consistency measurement set maximization method (PCM) under severe accumulative noise and can be integrated with M-estimation methods.

1. Introduction

In multi-robot simultaneous localization and mapping (SLAM) systems, each robot explores its local environment, creates a map from its observations, and estimates its own position within that map. Multi-Robot SLAM addresses the efficiency issue in scenes of a large area, e.g., underwater scenes [1]. However, the individual maps created by each robot may have inconsistencies due to different sensor measurements and environmental conditions. Thus, it is necessary to estimate the relative pose between the maps of multiple robots so that the individual maps can be merged into a consistent global map. This task is known as map fusion [2].

The simplest way to accomplish this task is to align each map to a same global reference, e.g., by using a global navigation satellite system (GNSS). However, in GNSS-denied areas, e.g., indoor environments and urban canyons, direct measurements of other robots’ poses or indirect measurements from similar landmarks must be utilized [2]. Similar to the single-robot map estimation problem, these measurements are frequently treated as loop closures and represented by factor nodes added to a global factor graph. Under the assumptions of independent measurement and Gaussian noise, the factor graph can be solved as a nonlinear least squares problem [3]. Thus, map fusion is realized. Map fusion is critical to the success of multi-robot SLAM, because it allows the robots to share their knowledge of the environment and create a more accurate and comprehensive map than any individual robot could create alone. The resulting global map can be used for a variety of applications, such as path planning, obstacle avoidance, and localization.

However, several issues must be considered in the map fusion problem. First, the prior of relative pose between robots may not exist [4] in multi-robot SLAM. This requires the map fusion algorithm to either function without the prior information or calculate a reliable prior by itself. Second, perceptual aliasing introduces a false-positive inter-robot loop closure to the system, which reduces the performance of the maximum likelihood estimator drastically [5]. Third, each robot’s trajectory estimate can have errors due to sensor noise and drift, and we found that poor single-robot odometry can result in poor map fusion performance, and to the best of our knowledge, this issue is rarely discussed in previous studies. In contrast, many existing map fusion methods assume high-quality single-robot trajectories [2,4]. As a result, these methods exhibit significant performance degradation as the accumulative noise in the odometry increases.

Thus, in this paper, we propose a robust consistency-based inter and intra-robot loop closure selection algorithm for multi-robot pose graph map fusion. Specifically, we create a novel measurement consistency score that integrates pairwise-loop consistency and loop-odometry consistency, which improves robustness against false-positive loop closures and accumulative noise in the odometry. Utilizing the pairwise-loop consistency from the consistency metric, an initial inter-robot loop closure can be selected with acceptable increases in computational costs. In addition, the initial loop closure allows us to update the pose estimates and to calculate the full consistency metric for both inter-robot and intra-robot loop closures. Finally, the loop closure selection problem is formulated and solved as a maximum edge weight clique problem.

Our primary contributions are summarized as follows:

1

We present a novel consistency score that considers both pairwise-loop and loop-odometry consistency for loop closure selection;

2

We propose an efficient consistency-based loop closure selection strategy to provide an initial relative pose estimate between robots;

3

To the best of our knowledge, this work is the first to conduct experiments to demonstrate the impact of accumulative noise on multi-robot map fusion problem, and our experimental result shows that the existing loop selection method relies on the quality of single-robot odometry and degenerates significantly under severe accumulative noise.

2. Related Work

2.1. Robust Optimization in SLAM

Pose graph optimization (PGO) is commonly used as the estimation backbone in state-of-the-art SLAM systems [6]. However, the standard least squares formulation of PGO is not robust against corrupted observations, which can cause the estimator to diverge from the actual value of the parameters to be solved. In practical SLAM applications, these corrupted observations arise from perceptual aliasing, which further leads to erroneous data associations and wrong measurement constraints.

To address this issue, robust optimization techniques are designed to handle outliers and noise in the SLAM problem. Robust SLAM estimation methods can be categorized into two classes [5]. The first class focuses on identifying the inlier measurement set by searching for the best subset that satisfies a mutual consistency metric. Ref. [7] used random sampling to iteratively decide inliers and outliers. Ref. [8] divided measurements into small groups and evaluated the consistency within and between the groups. Ref. [4] designed pairwise consistency measurement set maximization (PCM) and enforces pairwise internal consistency to inlier measurement sets.

The other class, which is referred to as M-estimation, replaces the likelihood function in the standard maximum likelihood estimation (MLE) with robust losses that are less sensitive to outliers. An early representative method is the popular Huber loss [9]. Ref. [10] discarded incorrect loop closure candidates using switchable variables and functions. Ref. [11] improved [10] by dynamically scaling the information matrix of each non-incremental edge. Ref. [12] used the Gaussian mixture model to represent the non-Gaussian behaviors of loop closures and employed max-mixture instead of sum-mixture to solve the problem efficiently. However, robust cost functions typically introduce non-convexities, which make the objective function more susceptible to converge to poor quality critical points [5]. Refs. [13,14] used convex relaxation to solve SLAM problems with robust cost functions. Ref. [15] further formulated the M-estimation problem and outlier procedure as a dual problem with Black-Rangarajan duality and solved the SLAM problem using graduated non-convexity optimization (GNC).

2.2. Robust Optimization in Multi-Robot SLAM

The majority of methods in Section 2.1 are designed for a single-robot pose graph and fails in multi-robot scenarios due to the lack of reliable initialization [10,11,12] or trusted odometry [8].

Previous studies [16,17] used expectation maximization to determine inliers and outliers from a set of measurements, which provides initial estimates for the relative poses between multiple robots. PCM [4] is widely adopted as an outlier rejection method in multi-robot systems [18,19,20,21]. Ref. [2] further added data similarity to the consistency weight and reformed PCM as a maximum edge weight clique problem (MEWCP) rather than MCP. The weight formulation improves PCM’s performance when perceptual aliasing occurs while the cardinality of the true measurement set is small. Ref. [2] is the most similar method to our proposed method. However, the method does not consider the consistency between loop closures and odometry, and it required similarity input from the front end, specifically, the number of features matched and the tuning parameter for loop closure. However, our method only depends on the pose graphs of multi-robots to function. On the other hand, we perform a consistency-based initialization and consider both inter-robot and intra-robot measurements in one framework.

Ref. [6] performed a distributed initialization by solving a robust pose averaging problem and then solved the distributed PGO with GNC (D-GNC). D-GNC showed better robustness against outliers and lower trajectory error; however, it depends heavily on the performance of initialization. In addition, the runtime of the GNC optimizer is much larger than that of PCM with heuristic solvers. Thus, an early stop must be employed in practical applications.

3. Preliminaries

3.1. Multiple Robot Pose Graph Optimization

Graph SLAM uses a graphical structure to represent the Bayesian SLAM [22]. In single-robot scenarios, the pose graph optimization (PGO) can be formulated as the following MLE problem:

$$\widehat{X}=\underset{X}{argmax}P(Z_o,Z_l|X)$$

(1)

where X is the set of all $SE\left(n\right)$ pose variables $x_t$, which denotes the robot trajectory, and $Z_o$ and $Z_l$ are two sets of measurements $z_{ij}$, representing the relative pose constraints between $x_i$ and $x_j$: $Z_o$ denotes the odometric measurement set where i and j are consecutive, and $Z_l$ denotes the loop closure measurement set. Note that $Z_o$ and $Z_l$ have empty intersection, and together they make up the full set of measurements.

The multi-robot pose graph optimization is an extension of the single-robot PGO problem, where both inter-robot and intra-robot loop closures can be obtained. Here, the relative pose between the local coordinate frames of multiple robots is unknown and must be estimated, as is suggested in [23]. As a result, the multi-robot map fusion problem (we consider two robots in this example) can be expressed as follows:

$$\widehat{X},{\widehat{T}}^g=\underset{X,T^g}{argmax}P(Z_o^a,Z_o^b,Z_l^a,Z_l^b,Z_l^{ab}|X,T^g)$$

(2)

where $T^g=\{T_g^a,T_g^b\}$, representing the relative transformation between the respective robot’s local frame $a,b$ and the global reference frame g. Here, $Z_o^a$, $Z_o^b$, $Z_l^a$, and $Z_l^b$ represent the intra-robot measurements sets, and $Z_l^{ab}$ is the inter-robot loop closure measurements set.

3.2. Loop Closure Selection Problem

Loop closure selection is a crucial step in SLAM algorithms that involves identifying and selecting the most significant loop closures in the map to refine the robot’s estimated trajectory and improve the accuracy of the map. The selection of loop closure is based on consistency because it is difficult to determine whether a measurement is an inlier or outlier [24]. We adopt the definition of pairwise internal consistency set [4] to make sure that every selected measurement be consistent with all the other selected measurements. Given a set of measurements $\tilde{Z}$, the condition of pairwise internal consistency can be formulated with a consistency metric C and the threshold $\gamma$ as follows:

$$C(z_i,z_j)\ge \gamma ,\forall z_i,z_j\in \tilde{Z}$$

(3)

where C is a function measuring the consistency of measurements $z_i$ and $z_j$.

Assumption 1. 

True loop closure measurements are more likely to have a higher degree of pairwise consistency metric scores than false positive loop closure measurements [2].

Assumption 1 indicates a larger metric magnitude from true loop closures and is robust against cases where the size of the true loop closure measurement set is smaller than that of the false positive loop closure measurement set.

Based on this assumption, the best loop closure measurement subset can be selected using the following equation:

$${\tilde{Z}}^{*}=\underset{\tilde{Z}\in Z_l}{argmax}\sum _{z_i,z_j\in \tilde{Z}}C(z_i,z_j)$$

(4)

where $Z_l=Z_l^a\cup Z_l^b\cup Z_l^{ab}$. ${\tilde{Z}}^{*}$ is the selected loop closure measurement subset.

4. Methodology

The proposed loop closure selection method relies on an initialization procedure, a novel pairwise consistency metric, and a graph-based solution. In this section, our novel pairwise consistency metric is first defined. Then, a single loop closure is selected with a simple strategy to provide an initial pose estimate between robots. Finally, the graph theory algorithm leveraged to solve the loop closure selection problem is introduced. A diagram of the proposed algorithm is shown in Figure 1, where the cyan blocks show our main contributions. In the following, we consider a case with two robots as an example.

4.1. Inter and Intra-Robot Consistency Metric

In the procedure of multi-robot SLAM, odometry of each robot, inter-robot loop closures and intra-robot loop closures are obtained. The pose graph formulation of the concerned measurements is shown in Figure 2. The odometry of each robot is represented by triangles of different colors.

Pairwise consistency metric measures the consistency between pairs of observations or measurements, and checks if the consistency between measurements is within certain thresholds. The proposed pairwise consistency metric is expressed as follows:

$$\begin{array}{c}\hfill C(z_{ik}^{pq},z_{jl}^{rs})=c_{ll}(z_{ik}^{pq},z_{jl}^{rs})c_{lo}(z_{ik}^{pq},{\widehat{x}}_{ik}^{pq})c_{lo}(z_{jl}^{rs},{\widehat{x}}_{jl}^{rs}),\\ \hfill p,q,r,s\in \{a,b\}\end{array}$$

(5)

where $z_{ik}^{pq}$ represents the loop closure measurement between the ith frame of robot p and the kth frame of robot q, and ${\widehat{x}}_{ik}^{pq}$ represents the relative pose estimation between the ith frame of robot p and the kth frame of robot q. Note that in the single-robot scenario, the pose estimate is usually obtained from odometry measurements. However, in the case of multiple robots ($p\ne q$ or $r\ne s$), relative pose estimates are not available directly because the relative pose between robots is initially unknown; this issue will be settled in Section 4.2. $c_{ll}$ represents the geometric consistency between two loop closures, $z_{ik}^{pq}$ and $z_{ik}^{rs}$. $c_{lo}$ represents the geometric consistency between one loop closure and the pose estimation of the corresponding odometry. Specifically, these three terms can be expressed as:

$$c_{ll}(z_{ik}^{pq},z_{jl}^{rs})=exp(-||(\ominus z_{ik}^{pq})\oplus {\widehat{x}}_{ij}^{pr}\oplus z_{jl}^{rs}\oplus {\widehat{x}}_{lk}^{sq}|{|}_{\Sigma })$$

(6)

$$c_{lo}(z_{ik}^{pq},{\widehat{x}}_{ik}^{pq})=exp(-||(\ominus z_{ik}^{pq})\oplus {\widehat{x}}_{ik}^{pq}|{|}_{\Sigma })$$

(7)

$$c_{lo}(z_{jl}^{rs},{\widehat{x}}_{jl}^{rs})=exp(-||(\ominus z_{jl}^{rs})\oplus {\widehat{x}}_{jl}^{rs}|{|}_{\Sigma })$$

(8)

where ⊕ represents pose composition and ⊖ represents pose inversion [25]. In addition, $\left|\right|·{\left|\right|}_{\Sigma }$ refers to the Mahalanobis distance.

$c_{ll}$ in Equation (6) (referred to as pairwise-loop consistency) was used as the consistency metric in previous studies [2,4], and its value is proportional to the probability density that the four rigid-body transformations actually form a cycle [26].

$c_{lo}$ in Equations (7) and (8) are referred to as the loop-odometry consistency, and their values are proportional to the probability density, causing the transformation of one loop closure and the corresponding odometry to actually form a cycle.

A simplified illustration of our proposed metric is shown in Figure 3.

Note that pairwise-loop consistency may degrade as the quality of the odometry decreases. When the current pose estimates ${\widehat{x}}_{ij}^{pq}$ and ${\widehat{x}}_{lk}^{sq}$ in Equation (6) are accurate, the pairwise-loop consistency score of two true loop closures is close to 1 and passes the threshold. In this case, the loop closure pair can be considered as inlier candidates. However, in practical applications, ${\widehat{x}}_{ij}^{pq}$ and ${\widehat{x}}_{lk}^{sq}$ can suffer from large error due to accumulative odometry noise, especially when i and j or l and k are distant in the robot trajectories. Large errors in pose estimates result in low pairwise-loop consistency scores and can potentially cause the rejection of true loop closures.

Thus, the loop-odometry consistency terms are integrated into an overall metric to reward the loop closure pairs, whose corresponding ${\widehat{x}}_{ij}^{pr}$ and ${\widehat{x}}_{lk}^{sq}$ are more likely to be accurate. Thus, we obtain the following:

$${\widehat{x}}_{ij}^{pr}={\widehat{x}}_{ik}^{pq}\oplus {\widehat{x}}_{kj}^{qr}$$

(9)

$${\widehat{x}}_{lk}^{sq}=\ominus ({\widehat{x}}_{kj}^{qr}\oplus {\widehat{x}}_{jl}^{rs})$$

(10)

By omitting ${\widehat{x}}_{kj}^{qr}$, we turn to a similar problem that determines whether ${\widehat{x}}_{ik}^{pq}$ and ${\widehat{x}}_{jl}^{rs}$ are likely to be accurate. Here, an approximate evaluation of the odometry’s quality can be realized by utilizing the loop-odometry consistency given in Equations (7) and (8).

4.2. Consistency-Based Initialization

The calculation of the proposed consistency metric requires an initial pose estimate between robots, which is unavailable at the beginning of the problem. Moreover, a reliable initial estimate between robots allows us to initialize robot trajectories in the global reference frame, which has a positive impact on the effect of PGO. Thus, an initialization strategy is introduced in the following.

Based on Assumption 1, an inter-robot loop closure that is likely to be consistent with other inter-robot loop closures can be found:

$$\underset{i}{argmax}\prod _{j=1}^n{c}_{ll}(z_i,z_j),z_i,z_j\in Z^{ab},j\ne i$$

(11)

where n is the size of inter-robot measurements set $Z^{ab}$. The selected inter-robot loop closure measurement $z_i$, which is denoted $z_{mn}^{ab}$, is used as the initial relative pose between robots. A simplified illustration of our proposed initialization is shown in Figure 4.

Constrained by the selected loop closure measurement, pose variables of robot trajectories can be updated as follows:

$$\widehat{X},{\widehat{T}}^g=\underset{X,T^g}{argmax}P(Z_o^a,Z_o^b,z_{mn}^{ab}|X,T^g)$$

(12)

Note that this update does not change the pairwise-loop consistency terms in Equation (11) because the “shape” of each robot’s initial trajectory is unchanged; however, the update improves the accuracy of the loop-odometry consistency terms.

The selected loop closure measurement serves as the initial pose estimate between robots; thus, the pose estimates between two arbitrary nodes from different robots, which are needed in the calculation of the proposed consistency metric, can be calculated as follows:

$${\widehat{x}}_{pq}^{ab}={\widehat{x}}_{pm}^{aa}\oplus z_{mn}^{ab}\oplus {\widehat{x}}_{nq}^{bb}$$

(13)

With Equation (13), the proposed consistency metric given in Equation (5) can be calculated.

4.3. Maximum Edge Weight Clique-Based Solution

The maximum edge weight clique problem is a graph optimization problem that involves finding the largest complete subgraph (clique) in a graph where the sum of the edge weights in the clique is maximized. Formally, the maximum edge weight clique problem can be defined as follows: Given an undirected weighted graph $G=(V,E)$, where V is the set of vertices and E is the set of edges, each edge is associated with a positive weight, where the goal is to find a subset C of V such that C forms a clique in G, and the sum of the weights of the edges in the induced subgraph of C is maximized.

Equation (4) can be formulated as a maximum edge weight clique problem (MEWCP). The consistency metric $C(z_i,z_j)$ of each loop closure pair is first calculated and stored in a symmetric matrix B, as shown in Figure 5. Each row or column of the matrix corresponds to one loop closure. Matrix elements that are less than the consistency metric threshold $\gamma$ are changed into zeroes to symbolize that the corresponding loop closure pair does not fit the consistency condition. The diagonal elements are also filled with zeroes because the consistency between one loop closure and itself is meaningless. Note that this matrix shares the same form as the weighted adjacent matrix of an undirected weighted graph, where each vertice of the graph represents a loop closure measurement. Whether two vertices are adjacent corresponds to whether the loop closure measurement pair fits the pairwise consistency metric condition, and the weight of the corresponding edge refers to the pairwise consistency metric score of the loop closure measurement pair. Thus, finding a measurement subset that fits the pairwise internally consistent condition in Equation (3) with the greatest overall consistency metric is equal to finding a clique with the greatest edge weight sum. Therefore, the solution to the robust loop closure selection problem can be transformed into the following expression:

$${\tilde{Z}}^{*}=MEWCP\left(B\right)$$

(14)

The MEWCP procedure is a classical combinatorial optimization problem that has been proven to be NP-hard. There are several approximation algorithms and heuristics that can provide good solutions in practice. Here, we utilize the ReConSLS solver [27] due to its simplicity, efficiency, and open-source implementation.

Finally, the multi-robot map fusion problem is accomplished by an optimization according to Equation (2).

5. Experimental Results

Experiments were conducted on multi-robot SLAM datasets with noisy odometry to demonstrate the performance of the proposed algorithm in batch mode. The proposed algorithm was compared against PCM [4], dynamic covariance scaling (DCS) [11], and distributed Graduated Nonconvexity (D-GNC) [6] under various settings.

5.1. MEWCP Cutoff Time Experiment

Note that ReConSLS [27] provides the option to stop the local search stage of the MEWCP solver with a certain cutoff time and return the current best solution. We used the ManhattanOlson3500 dataset to evaluate the impact of cutoff time on the MEWCP solver. This dataset comprises 3500 poses and 2099 correct loop constraints. In total, 2099 “random” outlier measurements are generated as described in the literature [10] for 10 runs. Our outlier rejection method was assessed with $\gamma =0.3$ and various cutoff time values.

Figure 6 shows the average precision and recall. Full precision means that the method only selects true loop closure measurements, and full recall means that the method selects all the true measurements. The MEWCP solver failed to obtain a solution at 0.75 s, and when the cutoff time is greater than 1 s, the solver obtained stable results. The recall ranged from 0.8141 to 0.8290, and the precision ranged from 0.9951 to 0.9972. We determined that the growth of precision and recall relative to cutoff time is insignificant. ManhattanOlson3500 dataset contains more loop closures than other datasets involved in our following experiments. Thus, we set the cutoff time to 1 s for our subsequent experiments.

5.2. Modified CSAIL Datasets

A previous study [4] provided a modified CSAIL dataset that contains two robot trajectories with 128 inlier loop closures and 250 outlier loop closures. In order to compare the robustness of the algorithms to varying odometry error, we added random noise to the orientation of each odometry pose, which affects the overall trajectories cumulatively. Ten noise levels were simulated from $2\sigma =1^{\circ }$ to ${10}^{\circ }$. For each noise level, the noisy trajectories were generated randomly for 100 runs. Figure 7 shows the noisy trajectories at noise levels of $1^{\circ }$, $5^{\circ }$, and ${10}^{\circ }$, respectively.

We used GTSAM [28] as the framework in our experiments. The baselines are summarized as follows: (1) PCM followed by Gauss–Newton (GN) optimizer with default settings; (2) our initialization technique with PCM followed by default GN optimizer; (3) our initialization technique with DCS solved by default GN optimizer; (4) our initialization technique with our loop closure selection followed by default GN optimizer; (5) D-GNC solved by Levenberg–Marquardt (LM) optimizer with default settings in [6]; (6) our initialization technique with GNC solved by LM optimizer; (7) our initialization technique with our loop closure selection followed by default GN optimizer, where the optimized variable values and the original full pose graph were further optimized by GNC (LM) with identical parameters in baseline (6).

Although PCM does not rely on initialization module to reject outlier measurements, better initial estimates of variable might improve the effect of optimization. Therefore, for comparison, we provide the result of PCM after our initialization as well. DCS turns off all the inter-map factors without initialization [4], and thus, we implement DCS after our initialization. Open source implementations of PCM [29], DCS [30], and D-GNC [29] based on GTSAM are all available online. Parameter sweep was conducted for the following threshold parameters: $\gamma$ for PCM, $\mathrm{\Phi }$ for DCS, the inlier threshold for robust distributed PGO in D-GNC, and $\gamma$ for the proposed loop closure selection method. The best parameters were determined according to the best average Absolute Trajectory Error (ATE) over 100 runs.

Robustness Against Odometry Noise. We compare the performance of concerned algorithms based on Absolute Trajectory Error. Figure 8 shows the distribution of ATE error across 100 runs for each noise level, and quantitative evaluation results for each method against the ground truth are shown in

Table A1. Average ATE and STD results of the robustly merging map on the multi-robot CSAIL dataset with 10 levels of odometry noise.

Noise Level (${}^{\circ }$)	PCM		OI + PCM		OI + DCS		Ours		D-GNC		OI + GNC		Ours + GNC
	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std
1	8.090	2.782	0.641	0.371	0.519	0.283	0.591	0.356	5.932	2.238	0.4235	0.2275	0.4218	0.2261
2	11.824	5.774	2.096	1.200	1.892	1.042	2.090	1.166	5.577	2.220	0.8429	0.4520	0.8428	0.4519
3	15.502	7.512	4.205	2.285	4.027	2.177	3.753	2.104	5.928	2.307	1.6510	0.9477	1.5759	0.9139
4	20.083	9.375	6.048	3.213	5.926	3.178	5.816	3.132	7.336	3.046	3.0158	1.7602	2.7889	1.6560
5	22.497	10.311	8.358	4.461	7.662	4.065	7.285	8.528	9.205	3.908	4.5247	2.6030	4.5200	2.6065
6	25.692	11.320	10.092	5.294	9.158	4.892	8.348	9.805	11.038	4.577	6.0605	3.4220	5.8896	3.3359
7	28.991	12.861	11.988	6.117	10.566	5.549	9.391	11.019	11.004	5.194	7.8708	4.4807	7.7653	4.3847
8	30.760	12.699	14.012	7.067	11.544	6.059	10.067	11.829	13.233	6.228	9.0729	4.9996	9.0393	4.9747
9	34.161	14.332	15.364	7.946	12.342	6.426	10.705	12.549	15.728	7.104	10.3245	5.6425	9.8986	5.4318
10	35.802	14.815	17.003	8.518	13.064	6.710	11.334	13.233	17.477	7.571	11.3157	6.1088	11.2058	6.0329

The best, second-best, and third-best results are highlighted. OI refers to our initialization.

.

As can be seen, PCM demonstrated great instability (many outliers shown in red plus signs) and the largest errors because of bad initial estimates on the trajectory. Utilizing the proposed initialization with PCM exhibited a positive impact because better initial value estimates improve the effect of the GN optimization. However, the final ATE of PCM, even with our initialization, increased drastically as the noise level increased. Both DCS with our initialization and our proposed method obtained lower ATE and better stability than PCM, and our proposed method performed better when the odometry noise level was greater than $3^{\circ }$. D-GNC demonstrated severe instability caused by inaccurate initialization results, and due to the instability, D-GNC gets higher average ATE than the proposed method. Replacing D-GNC’s initialization module with our initialization technique significantly improved the performance and stability of D-GNC, exhibiting the second-best ATE results. Finally, our proposed method followed by GNC achieved stability and the best overall accuracy.

Figure 9 illustrates the trajectory result of different methods on the 39th randomly generated dataset at noise level 8. The obtained trajectory of PCM, PCM with our initialization, DCS with our initialization, and D-GNC exhibit larger drift than our proposed method in most parts of the trajectory. Our proposed method, our initialization with GNC, and our proposed method with GNC show significantly better trajectory results than the four methods mentioned above, especially in the center part of the trajectory. The main difference between these three trajectories is located at the upper-right part of the map, where our proposed method with GNC shows the most similar trajectory to the ground truth, and our initialization with GNC shows the least similar trajectory to the ground truth.

Runtime Comparison.

Table 1. Average runtime (ms) of all modules on multi-robot CSAIL dataset.

Noise Level (${}^{\circ }$)	O-ini	D-GNC-ini	PCM	DCS	O-lcs	GNC
1	231	81	1328	146	2178	10,897
2	232	101	1326	103	2255	17,794
3	233	100	1348	70	2224	18,191
4	230	101	1313	57	2245	21,279
5	232	115	1299	56	2234	26,863
6	230	109	1294	64	2202	25,744
7	232	117	1286	61	2241	31,033
8	231	117	1376	53	2216	30,913
9	232	111	1405	46	2271	27,632
10	232	107	1412	40	2230	30,652

O-ini refers to our initialization. O-lcs refers to our loop closure selection.

shows the average runtimes of the compared methods.

Note that for PCM and our loop closure selection method (O-lcs), the runtime includes the time required to calculate the pairwise consistency metric, MCP or MEWCP, and the GN optimization. As can be seen, DCS outperformed the other methods in terms of runtime efficiency. Our outlier rejection module consumed more time than PCM. The difference is primarily caused by our MEWCP solving time, which is 858 ms on average, compared to PCM’s MCP solving time, which is 175 ms. GNC demonstrated the longest calculation time among all compared methods, and the runtime increases with the noise level. Although D-GNC’s initialization were about one-half that of our initialization technique, this consumed a very small proportion of the procedure’s total runtime. Our initialization technique consumed about 230 to 232 ms, of which 182 milliseconds are used for traversing all the inter-robot loop closure pairs and calculating the $c_{ll}(z_i,z_j)$ (referred to as pairwise-loop consistency) in Equation (11). The rest are used for updating the pose variables of robot trajectories in Equation (12).

Performance of the Proposed Initialization Technique.

Table 2. Initialization performance on multi-robot CSAIL dataset.

Noise Level (${}^{\circ }$)	D-GNC		Ours
	ATE (m)	Std	ATE (m)	Std	Precision
1	12.420	4.125	1.501	0.796	1.00
2	10.708	3.772	3.634	1.858	1.00
3	11.160	4.153	5.387	2.726	1.00
4	12.447	4.796	7.036	3.591	1.00
5	13.686	5.525	8.556	4.358	1.00
6	15.155	6.025	9.822	5.042	0.98
7	15.022	6.382	11.008	5.654	0.96
8	16.631	7.159	11.838	6.128	0.95
9	18.919	7.915	12.604	6.491	0.93
10	20.422	8.181	13.267	6.763	0.93

The best results are highlighted.

shows the average Absolute Trajectory Errors with different initialization methods.

Here, precision represents the rate at which our proposed initialization method selected a true loop closure as the initial between-robot pose estimate. D-GNC’s initialization obtained much higher ATE than the proposed initialization than ours for two reasons. First, D-GNC’s initialization itself requires a good initial estimate of the relative pose between multiple robots to perform well. Specifically, the robust pose averaging problem in D-GNC’s initialization [6] is formulated as a GNC optimization solved by LM. During this optimization, the pose to be solved is initialized at identity pose, and it has been shown that naive initialization affects the performance of GNC optimizer [6]. In contrast, our initialization does not require this initial estimate. Second, the GNC optimization in D-GNC’s initialization process needs an outlier threshold parameter, which requires an additional parameter tuning to gain robustness against incorrect inter-robot loop closures in actual cases. In contrast, our initialization does not require this additional parameter tuning process.

5.3. Modified Bicocca Dataset

To show the performance of the proposed method on diverse datasets. We also evaluated the performance of our algorithm on a modified Bicocca dataset. The sequence 25b of Bicocca dataset is divided into two robot trajectories. The dataset has 350 loop closures from a simple place recognition algorithm (BoW) with the minimum confidence parameter (${\alpha }^{-}$) set to 0.15. Among them, 261 loop closures are inliers and the other 89 loop closures are ourliers. Another 172 outlier loop closures were randomly generated as described in the literature [10], in order to increase the outlier rate to 50%. Similarly, we added random noise to the orientation of each odometry pose for ten noise levels from $2\sigma =1^{\circ }$ to ${10}^{\circ }$, in order to simulate scenarios with different odometry errors. For each noise level, 100 noisy trajectories were generated. The implementations and parameter tuning strategies of seven algorithms are the same as those adopted in the modified multi-robot CSAIL dataset. Although modified Bicocca dataset and the modified CSAIL dataset share the similar orientation noise per frame, the trajectory of the Bicocca dataset is much more distorted than that of the CSAIL dataset. Figure 10 shows the noisy trajectories at noise levels of $1^{\circ }$, $5^{\circ }$, and ${10}^{\circ }$, respectively.

Figure 11 shows the distribution of ATE error across 100 runs for each noise level, and quantitative results for each method against the ground truth is shown in

Table A2. Average ATE and STD results of the robustly merging map on the multi-robot Bicocca dataset with 10 levels of odometry noise.

Noise Level (${}^{\circ }$)	PCM		OI + PCM		OI + DCS		Ours		D-GNC		OI + GNC		Ours + GNC
	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std	ATE (m)	Std
1	113.951	53.989	17.698	8.253	12.365	5.153	13.492	6.090	43.335	13.896	4.0235	1.8584	4.2150	1.8826
2	111.403	54.106	39.964	16.652	24.664	11.707	25.571	11.958	44.859	16.985	18.9330	9.3146	18.9429	9.3044
3	108.038	39.653	51.492	24.937	34.980	16.510	34.297	16.151	45.277	19.417	30.3708	14.7687	30.3025	14.7380
4	98.795	38.421	62.715	30.596	40.315	18.607	38.682	17.873	46.192	20.283	36.5794	17.4698	36.5397	17.1571
5	98.909	39.586	67.526	31.298	43.420	20.053	42.361	19.1428	48.213	21.789	41.0118	19.350	41.0852	19.1168
6	97.372	41.464	73.040	34.739	43.631	20.194	43.231	19.5631	49.363	22.467	42.3226	19.8184	42.4672	19.924
7	88.277	38.572	72.526	35.259	45.189	20.606	44.914	20.1161	48.016	21.909	44.4454	20.303	44.0455	20.2566
8	87.086	38.354	69.034	33.882	44.554	19.711	44.291	19.262	48.614	21.774	43.6520	19.1942	43.3634	18.9289
9	77.532	34.223	68.225	32.246	44.800	19.341	44.443	18.7434	47.241	21.616	43.7138	18.7792	44.0341	18.999
10	73.651	32.999	66.824	33.040	45.681	19.860	45.093	19.475	46.440	20.922	44.4772	19.0131	44.4518	19.1225

The best, second-best, and third-best results are highlighted. OI refers to our initialization.

.

Similar to the CSAIL experiments, the proposed method exhibited greater accuracy and stability over PCM, PCM with our initialization, and D-GNC. Either our initialization or our full method significantly improved the performance and stability of D-GNC, exhibiting the best ATE results.

Figure 12 illustrates the trajectory result of different methods on the 28th randomly generated dataset at noise level 3. Similarly, our proposed method, our initialization with GNC, and our proposed method with GNC shows significantly better trajectory results than PCM, PCM with our initialization, DCS with our initialization, and D-GNC. However, the odometry noise exhibits greater impact on Bicocca dataset than CSAIL Dataset. When the odometry noise is larger than 7, the final trajectories provided by all seven methods are hardly applicable. Thus, we try to evaluate the performance of the proposed method from other aspects.

Performance of the Proposed Consistency Metric. Two other important metrics to look at in the context of robust back-ends are precision and recall, especially for outlier rejection methods. An algorithm that provides higher precision with a considerable recall shows better performance. Figure 13 shows the average precision–recall curves of solvers on multi-robot Bicocca dataset under 10 levels of odometry noise.

As can be seen in Figure 13, under low odometry noise, both PCM and our method provide high precision. Under the same recall, PCM gets higher precision only when the recall is lower than 0.2. When recall is higher than 0.2, our proposed method shows higher precision in all of the 10 noise levels. This proves that the proposed method not only provides better final trajectory, but also selects inlier measurements more accuracy than PCM under different odometry noise.

6. Discussion

Evaluation of the Proposed Methods. Our experimental results have proven that the proposed method significantly improves the performance of PCM by a better consistency metric and en extra initialization procedure. Under low odometry error, the proposed method showed slightly better performance than PCM with our initialization, DCS with our initialization, and D-GNC. Under high odometry error, however, the proposed method exhibited significantly better trajectory estimations and stability than PCM with our initialization, DCS with our initialization, and D-GNC. Although GNC with our initialization shows slightly better performance than the proposed method, the runtime of GNC optimization is at least five times longer than our loop closure selection procedure.

Relationship between PCM-like Methods and M-estimation-based Methods. Our experimental results have proven that the proposed method can further improve the performance of GNC [15] by refining pose variable estimates. According to the literature [6], applying GNC after PCM always leads to suboptimal performance due to the low recall of PCM. This low recall is theoretically caused by the formulation of cliques, which finds the best internally consistent measurement rather than all the possible inliers. In other words, the function of PCM-like methods is trusted inlier selection rather than outlier rejection, especially when strict thresholds are applied. Thus, we recommend the usage of PCM-like methods under loose thresholds for updating the pose variable values, followed by M-estimation-based methods, because M-estimation-based methods exhibited better performance when variable values were refined.

7. Conclusions

We proposed a consistency-based inter and intra-robot loop closure selection method for multi-robot map fusion. The proposed method includes an effective initialization strategy that selects a single reliable loop closure as the initial between-robot pose and a modified pairwise consistency metric that serves as weights when we select loop closures with the MEWCP. We compared the proposed method with state-of-the-art robust back-end methods for multi-robot applications. The proposed method shows competitive performance under regular odometry noise, and significantly outperformed the state-of-the-arts methods under severe odometry noise. The proposed consistency metric outperformed PCM under different odometry noise. The proposed initialization method outperformed D-GNC with outliers and noisy odometry, and our loop closure selection method further improved the performance of GNC by providing more accurate variable estimates.