# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## 3 Localization and Mapping
### Global Alignment
#### 3.1.1 Graph Construction  
A pose graph is used to represent the camera poses and the motion constraints between the poses. A node, or vertex, in the graph denotes a pose, i.e., a position and orientation, and an edge denotes the relative motion constraint given by the odometry observation. We use \(\mathbf{v}_{i}\) to denote the \(i\)th node in the graph and \(\Delta\xi_{i}\) to denote the relative motion between \(\mathbf{v}_{i}\) and \(\mathbf{v}_{i+1}\). We construct the initial pose graph by sequentially transforming poses with the odometry observations. First, we fix the first node in the graph to the zero vector, then each subsequent pose is computed by right multiplying the previous pose with a homogeneous transformation matrix \(\mathbf{T}(\Delta\xi)\in\mathfrak{SE}(2)\), representing the exponential map of the odometry observation,  
\[\mathbf{v}_{1} =(0,0,0)^{\mathsf{T}}\,, \tag{28a}\] \[\mathbf{v}_{i+1} =\mathbf{T}(\xi_{i})\mathbf{v}_{i}\,,\quad\forall i=\{1,2,\ldots,n\}\,. \tag{28b}\]  
To simplify the notation, we use \(\boldsymbol{\xi}_{ij}\) to denote the motion constraint between the poses \(\mathbf{v}_{i}\) and \(\mathbf{v}_{j}\) where \(\mathbf{v}_{j}\stackrel{{\text{\tiny{\bf def}}}}{{=}}\mathbf{v}_ {i+1}\). We also take \(n\) to be equal to the number of poses, which is the number of odometry constraints plus one, i.e., \(n\stackrel{{\text{\tiny{\bf def}}}}{{=}}m+1\). Associated with each motion constraint is a \(3\times 3\) covariance matrix \(\boldsymbol{\Sigma}_{ij}\) that represents the uncertainty of the motion. As we describe in the optimization section that follows, we use the information matrix \(\mathbf{Q}_{ij}=\boldsymbol{\Sigma}_{ij}^{-1}\) to represent the strength of the edge, or constraint, in the graph.  
Let \(\mathcal{C}\) be an ordered set of 2-tuples representing the correspondences between poses and GPS readings. Thus, the tuple \((i,k)\in\mathcal{C}\) specifies that pose \(\mathbf{v}_{i}\) corresponds to the GPS reading \(\mathbf{g}_{k}\). We insert GPS coordinates as nodes in the graph and add an edge to the corresponding camera pose. We also translate all GPS coordinates according to the first correspondence in \(\mathcal{C}\). We do this by storing the translation, \(\mathbf{g}_{0}\leftarrow\mathbf{g}_{k\in\mathcal{C}_{1}}\), where \(\mathcal{C}_{1}\) is the first GPS-odometry correspondence, and subtracting \(\mathbf{g}_{0}\) from all coordinates in the track,  
\[\mathbf{g}_{k}\leftarrow\mathbf{g}_{k}-\mathbf{g}_{0}\quad\forall k=\{1,2, \ldots,|\mathcal{C}|\}\,. \tag{29}\]We store the translation so that we can invert the pose graph back to the original UTM coordinates after optimization. Associated with each GPS coordinate is a \(2\times 2\) covariance matrix representing the expected accuracy of the receiver. We invert to covariance matrix, as we did with the odometry covariance, to obtain an information matrix \(\mathbf{Q}_{k}\). The values in this matrix depend on the expected accuracy of the GPS receiver and the environment in which the receiver is operating. For example, a consumer-grade GPS device operating under a dense forest canopy will have a relatively large uncertainty and thus small values in the information matrix.  
Although we anchor the GPS track to a camera pose in the graph, their global orientation will likely differ. Thus, we introduce a global orientation parameter \(\phi\) as another node in the graph that imposes a constraint on each node representing a camera pose that has a corresponding GPS reading. See Figure 3 for an illustration of the graph.