# Accurate Collaborative Globally-Referenced Digital Mapping with Standard GNSS
## 4 Globally-Referenced Electro-Optical SLAM (GEOSLAM)
### Visual SLAM
After tracking the stereo image pair as described above, GEOSLAM decides whether or not the candidate keyframe \(\mathcal{I}_{i}^{lr}\) must be selected as a keyframe. This decision is made based on the number of map points that were matched to the image features, and the distance traveled by the platform since the last keyframe was chosen. New keyframes are not spawned if the platform is nearly stationary. If the platform is in motion, and the number of feature matches to the map drops below a threshold, then the candidate keyframe \(\mathcal{I}_{i}^{lr}\) is chosen as keyframe \(K_{i}\), windowed BA is performed over \(N\) levels of covisibility, and the unmatched stereo features in \(K_{i}\) are spawned as new map points. Additionally, if \(\mathcal{I}_{i}^{lr}\) is selected to be \(K_{i}\), then the set of measurements from the features matched to the map, denoted \(\mathbf{z}_{i}^{lr}\triangleq\Big{\{}\mathbf{z}_{im}^{lr}\ :\ m\in\mathcal{M}_{i}\Big{\}}\), along with their SIFT descriptors, are passed on to the map module for storage, future feature matching, and processing in the windowed BA routine. In a visual-only SLAM system, the state vector \(\mathbf{X}_{i}\) is optimized with respect to the measurement vector \(\mathbf{Z}_{i}\), defined as  
\[\mathbf{Z}_{i}\triangleq\Big{[}\Big{\{}\mathbf{z}_{n}^{lr}\ :\ n\in\text{cov}(i,N)\Big{\}} \Big{]}^{T}.\]  
The windowed BA routine in GEOSLAM minimizes the 3D-to-2D reprojection error. The error term \(\mathbf{e}_{nm}\) for observation of map point \(p_{m}\) in the stereo keyframe \(K_{n}\) is given as  
\[\mathbf{e}_{nm}=\mathbf{z}_{nm}^{lr}-\Pi\Big{(}\mathbf{x}_{p_{m}}^{\mathcal{S}},\mathbf{x}_{C_ {n}}^{\mathcal{S}},\mathbf{\theta}_{C_{n}}^{\mathcal{S}}\Big{)},\]  
where \(\Pi\) is the projection function for an undistorted and rectified stereo camera  
\[\Pi\Big{(}\mathbf{x}_{p=n}^{\mathcal{S}},\mathbf{x}_{C_{n}}^{\mathcal{S} },\mathbf{\theta}_{C_{n}}^{\mathcal{S}}\Big{)} = \begin{bmatrix}f\frac{x_{nm}}{z_{nm}}+c_{u}\\ f\frac{y_{nm}}{z_{nm}}+c_{v}\\ f\frac{x_{nm}}{z_{nm}}+c_{u}-bf\end{bmatrix} ,\] \[=R\Big{(}\mathbf{\theta}_{C_{n}}^{\mathcal{S}}\Big{)}^{T}\Big{(}\mathbf{ x}_{p_{m}}^{\mathcal{S}}-\mathbf{x}_{C_{n}}^{\mathcal{S}}\Big{)},\]  
in which \(R(\cdot)\) denotes the rotation matrix corresponding to the argument angle-axis vector, and \(f\), \((c_{u},c_{v})\), and \(b\) are the focal length, the principal point, and the baseline distance between the left and right cameras of the rectified stereo camera model, respectively. The cost function to be minimized for visual SLAM is given as  
\[C_{i}=\sum_{n\in\text{cov}(i,N)}\sum_{m\in\mathcal{M}_{n}}\rho\Big{(}\mathbf{e}_{ nm}^{T}\Omega_{nm}^{-1}\mathbf{e}_{nm}\Big{)},\]  
where \(\rho\) may be the standard least squares cost function \(\rho(\cdot)=(\cdot)\) or a more robust cost function such as the Huber or Tukey cost functions, and where \(\Omega_{nm}=\sigma_{nm}^{2}I_{3\times 3}\) is the covariance of the feature measurements. GEOSLAM performs BA minimization via Google's ceres-solver. The automatic differentiation feature of ceres-solver is used to compute the Jacobian for the measurement model. An important feature of the GEOSLAM visual pipeline is the ability to merge maps from multiple mapping sessions. This is embodied in an algorithm similar to the loop closure technique from the visual SLAM literature [60]. Map merging is described in detail in Section 4.3.2.