# Fast Loop Closure Selection Method with Spatiotemporal Consistency for Multi-Robot Map Fusion
## 4 Proposed Method
### Topologically Related Loop Closures
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._  
Figure 1: (**a**) Clusters consist of topologically related measurements and are labeled by different colors. (**b**) Two sets of robot trajectories, the blue square represents the end position of trajectory 1, which is also the start position of trajectory 2. 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}}{\arg\max}\sum_{\mathbf{z}_{jl}^{ab},\bm {z}_{ik}^{ab}\in\mathbf{Z}^{ab}}h\Big{(}\mathbf{z}_{jl}^{ab},\mathbf{z}_{ik}^{ab}\Big{)} \oplus c\left(\mathbf{z}_{jl}^{ab},\mathbf{z}_{ik}^{ab}\right) \tag{7}\]  
where \(h\Big{(}\mathbf{z}_{jl}^{ab},\mathbf{z}_{ik}^{ab}\Big{)}\) 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.