# Fast Loop Closure Selection Method with Spatiotemporal Consistency for Multi-Robot Map Fusion
## 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\
eq 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:  
\[\mathbf{X}^{*}=\underset{\mathbf{x}}{\arg\max}P(\mathbf{X}\mid\mathbf{Z})= \underset{\mathbf{x}}{\arg\min}\underbrace{\left\{\sum_{i}\lVert\mathbf{x}_{i +1}-f\big{(}\mathbf{x}_{i},\mathbf{z}_{ij}\big{)}\rVert_{\Sigma_{i}}^{2}+ \underbrace{\sum_{ij}\lVert\mathbf{x}_{j}-f\big{(}\mathbf{x}_{i},\mathbf{z}_{ ij}\big{)}\rVert_{\Lambda_{ij}}^{2}\}\right\}}_{\text{Odometry Constraints}} \tag{1}\]  
where \(\mathbf{X}=\big{(}\mathbf{x}_{0},\mathbf{x}_{1},\ldots\mathbf{x}_{i},\ldots \mathbf{x}_{j},\ldots\big{)}^{T}\) presents the robot trajectory that consists of all pose nodes \(\mathbf{x}_{i}\in\text{SE}(3)\) or \(\mathbf{x}_{i}\in\text{SE}(2)\) at the time \(i,f(\cdot)\) is the nonlinear motion model, \(\sum_{i}\) and \(\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}^{S^{*}}=\underset{\mathbf{X},\mathbf{T}^{S}}{\arg \max}P\Big{(}\mathbf{X},\mathbf{T}^{S}\mid\mathbf{Z},\mathbf{Z}^{ab}\Big{)}. \tag{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\text{SE }(2)\text{ or }\operatorname{SE}(3)\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}^{S}=\left\{\mathbf{T}_{a}^{S},\mathbf{T}_{b}^{S}\mid\mathbf{T}_{a }^{S},\mathbf{T}_{b}^{S}\in\text{SE}(2)\text{ or }\operatorname{SE}(3)\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\big{(}\mathbf{z}_{ik}^{ab},\mathbf{z}_{jl}^{ab}\big{)}=\Big{\lVert}\left( \boxdot{\mathbf{z}}_{ik}^{ab}\right)\boxdot\mathbf{\dot{x}}_{ij}^{a}\boxdot \mathbf{z}_{jl}^{ab}\boxdot\mathbf{\dot{x}}_{lk}^{b}\Big{\rVert}_{\Sigma}. \tag{3}\]  
where the notations \(\boxdot\) and \(\boxdot\) are used for pose composition and inversion [36], respectively, and \(\mathbf{\dot{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.