# Accurate Collaborative Globally-Referenced Digital Mapping with Standard GNSS
## 4 Globally-Referenced Electro-Optical SLAM (GEOSLAM)
### Multi-Session Mapping
#### 4.3.2 Map Merging
The pose-graph optimization minimizes the following cost function with respect to \(\left(\mathbf{x}_{\mathcal{C}_{n}}^{\mathcal{S}},\mathbf{\theta}_{\mathcal{C}_{n}}^{ \mathcal{S}}\right)\forall n\in\text{cov}(i,N_{m})\):  
\[C=\sum_{n\in\text{cov}(i,N_{m})}\sum_{k\in\text{cov}(n,1)}\left[\left\|\left( \mathbf{x}_{\mathcal{C}_{n}}^{\mathcal{S}}-\mathbf{x}_{\mathcal{C}_{k}}^{\mathcal{S} }\right)-\delta\mathbf{x}_{nk}^{\mathcal{S}}\right\|_{P_{\delta\pi}^{-1}}^{2}+ \left\|\theta\bigg{(}R\left(\mathbf{\theta}_{\mathcal{C}_{n}}^{\mathcal{S}}\right) R\left(\mathbf{\theta}_{\mathcal{C}_{n\pm}}^{\mathcal{S}}\right)^{T}\bigg{)}- \delta\mathbf{\theta}_{n-1}^{\mathcal{S}}\right\|_{P_{\delta\theta}^{-1}}^{2} \right],\]  
where \(\|\mathbf{q}\|_{P}^{2}=\mathbf{q}^{T}P\mathbf{q}\), with the following constraints:  
\[\left(\mathbf{x}_{\mathcal{C}_{i}}^{\mathcal{S}},\mathbf{\theta}_{\mathcal{C}_{i}}^{ \mathcal{S}}\right) =\left(\tilde{\mathbf{x}}_{\mathcal{C}_{i}}^{\mathcal{S}},\tilde{\mathbf{ \theta}}_{\mathcal{C}_{i}}^{\mathcal{S}}\right),\] \[\left(\mathbf{x}_{\mathcal{C}_{n}}^{\mathcal{S}},\mathbf{\theta}_{\mathcal{ C}_{n}}^{\mathcal{S}}\right) =\left(\tilde{\mathbf{x}}_{\mathcal{C}_{n}}^{\mathcal{S}},\tilde{\mathbf{ \theta}}_{\mathcal{C}_{n}}^{\mathcal{S}}\right)\forall n\in\mathcal{K}_{0}.\]Figure 8: GEOSLAM trajectories at the instant when a merge has been detected and verified. The cameras colored black are keyframes from a prior map, and those colored red are from the current session. (**a**) Top view of the trajectories. (**b**) View from \(5^{\circ}\) elevation showing a discontinuity in the vertical component. Figure 9: GEOSLAM trajectories post-pose-graph optimization (in blue), overlaid on the corresponding (black and red) trajectories from Figure 8. All keyframes are colored blue at this stage since prior and current keyframes are now connected. (**a**) Top view of the trajectories. Note that the discontinuity at the merge location is smoothly distributed across \(N_{m}\) levels of covisibility in the current session, and that the keyframe poses from the prior map are unchanged at this stage. (**b**) View from \(5^{\circ}\) elevation. Keyframes from the current trajectory have been adjusted to remove the discontinuity, blue and black keyframes exactly overlap. Not shown: the corresponding map points in the current session are also adjusted to match the updated keyframe poses. As a result of this pose-graph optimization, any discontinuity at the merge location between the prior and current keyframes is smoothed out, as shown in Figure 9. Subsequently, the map points as seen in the current keyframes are also adjusted in accordance to the pose-graph optimization. Finally, the duplicated map points near the merge location are fused together. As a result of shared feature matches between the current and prior keyframes, the updated covisibility window for \(K_{i}\) includes keyframes from prior session(s). The merged map is then optimized in a windowed BA, this time with feature point coordinates and GNSS positions as measurements. Note that this is a joint windowed BA with both current and prior keyframes and map points. As a result, both the current and prior states are appropriately adjusted based on the number and covariance of the feature point and GNSS measurements. The result of the map merging routine is shown in Figure 10.