# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## 3 Localization and Mapping
### Global Alignment
#### 3.1.2 Optimization  
Given the graph structure outline above, we seek to find the optimal configuration of the state vector,  
\[\mathbf{s}=\left(\phi,\mathbf{v}_{1}^{\mathsf{T}},\mathbf{v}_{2}^{ \mathsf{T}},\ldots,\mathbf{v}_{n}^{\mathsf{T}}\right)^{\mathsf{T}}, \tag{30}\]  
that minimizes the sum of squared errors. The state is simply a vector consisting of the global orientation parameter followed by the camera poses, where each pose is parameterized as \(\mathbf{v}_{i}=(x,y,\theta)^{\mathsf{T}}\). Thus, the size of this vector is \(1+dn\), where \(d\) is the number of dimensions used to describe a camera pose, in this case 3, and \(n\) is the number of camera poses. The errors associated with the state configuration are given by two functions: one corresponding to the relative motion constraints given by the odometry observations and one corresponding to the constraints imposed by the observations from the GPS. The odometry error function, which is equivalent to the error function used in [49], is given by  
\[\mathbf{r}_{ij}(\mathbf{v}_{i},\mathbf{v}_{j}) =\mathsf{T}(\boldsymbol{\xi}_{ij})^{-1}\Big{(}\mathbf{T}(\mathbf{ v}_{i})^{-1}\mathbf{T}(\mathbf{v}_{j})\Big{)}\;,\] \[=\left( \begin{array}{c}\mathbf{R}(\theta_{ij})^{\mathsf{T}} \Big{(}\mathbf{R}(\theta_{i})^{\mathsf{T}}(\mathbf{t}_{j}-\mathbf{t}_{i})- \mathbf{t}_{ij}\Big{)}\\ \theta_{j}-\theta_{i}-\theta_{ij}\end{array} \right). \tag{31}\]  
where the notation \(\mathbf{R}(\theta)\) represents a 2D rotation matrix of the form  
\[\mathbf{R}(\theta)=\left( \begin{array}{cc}\cos\theta&-\sin\theta\\ \sin\theta&\cos\theta\end{array} \right), \tag{32}\]  
and \(\theta_{i},\theta_{j}\) and \(\theta_{ij}\) are the rotation angles corresponding to \(\mathbf{v}_{i},\mathbf{v}_{j}\) and \(\boldsymbol{\xi}_{ij}\), respectively. The notations \(\mathbf{t}_{i},\mathbf{t}_{j}\) and \(\mathbf{t}_{ij}\) represent the translation vectors from \(\mathbf{v}_{i},\mathbf{v}_{j}\) and \(\boldsymbol{\xi}_{ij}\), respectively. The  
Figure 3: Graphical model of global alignment. Circles denote nodes (vertices) and lines denotes edges (constraints). error function defined in Equation (31) gives the translational and rotational errors by first transforming pose \(\mathbf{v}_{j}\) into the coordinate frame of \(\mathbf{v}_{i}\), then computing the differences according to the odometry observation. Thus, this function returns zero when the state vector is configured according to Equations (28a) and (28b). The second error function calculates the position difference between a pose and its corresponding GPS coordinate according to the global orientation parameter,  
\[\mathbf{r}_{ik}(\mathbf{v}_{i},\phi)=\mathbf{R}(\phi)\mathbf{t}_{i}-\mathbf{g }_{k}\,. \tag{33}\]  
This function rotates the translation vector of pose \(\mathbf{v}_{i}\) about the origin of the coordinate frame according to a rotation matrix constructed from the global orientation parameter \(\phi\) and returns the offset to the corresponding GPS coordinate. Given the two error functions, we can write the sum of squared errors of the state configuration \(\mathbf{s}\) as  
\[\epsilon(\mathbf{s})=\sum_{ij}\mathbf{r}_{ij}^{\mathsf{T}}\mathbf{Q}_{ij} \mathbf{r}_{ij}+\sum_{(i,k)\in C}\mathbf{r}_{ik}^{\mathsf{T}}\mathbf{Q}_{k} \mathbf{r}_{ik}\,. \tag{34}\]  
where the error vectors \(\mathbf{r}_{ij}\) and \(\mathbf{r}_{ik}\) are weighted by the information matrices to incorporate the degree of belief given to the observations. This leads to the following objective function:  
\[\mathbf{s}^{\star}=\operatorname*{argmin}_{\mathbf{s}}\,\epsilon(\mathbf{s})\,. \tag{35}\]  
This is a non-linear least-squares optimization problem that can be solved with the GN algorithm. Specifically, the error function is linearized about a current estimate of the state configuration and we iteratively solve the linear system and incrementally update the state vector.