# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## 3 Localization and Mapping
### Global Alignment
#### 3.1.2 Optimization
Since our initial estimate of the state vector might be far from a minimum solution, we use a dampened version of the GN algorithm called the Levenberg-Marquardt (LM) algorithm [57; 58]. The linear system that is solved during each LM iteration takes the form  
\[(\mathbf{H}+\lambda\mathbf{I})\Delta\mathbf{s}=-\mathbf{b}\,, \tag{36}\]  
where \(\Delta\mathbf{s}\) is the solution to the linear system that provides incremental improvements to the state vector in the non-linear solution space by \(\bar{\mathbf{s}}+\bar{\mathbf{s}}+\Delta\mathbf{s}\), where \(\bar{\mathbf{s}}\) is the current estimate of the state configuration. The variable \(\lambda\) is a non-negative damping factor that, when large, forces the update to behave as the steepest descent and, when small, brings the algorithm closer to GN. We initialize \(\lambda=\text{trace}(\mathbf{H})\) and divide by two if the objective value decreases and multiply by two if the objective value increases. To construct the Hessian \(\mathbf{H}\) and the gradient vector \(\mathbf{b}\), we first take the derivatives of the error functions with respect to the state parameters evaluated at the current estimated of the state. We initialize the poses in the state vector according to Equations (28a) and (28b), and set the global orientation parameter to \(\phi=0\). The partial derivatives of the odometry error function can be written as  
\[\mathbf{A}_{ij} =\left.\frac{\partial\mathbf{r}_{ij}}{\partial\mathbf{v}_{i}} \right|_{\mathbf{s}=\bar{\mathbf{s}}}=\left( \begin{array}{cc}-\mathbf{R}( \theta_{ij})^{\mathsf{T}}\mathbf{R}(\theta_{i})^{\mathsf{T}}&\mathbf{R}( \theta_{ij})^{\mathsf{T}}\frac{\partial\mathbf{R}(\theta_{i})^{\mathsf{T}}}{ \partial\theta_{i}}(\mathbf{t}_{j}-\mathbf{t}_{i})\\ \mathbf{0}^{\mathsf{T}}&-1\end{array} \right)\,, \tag{37a}\] \[\mathbf{B}_{ij} =\left.\frac{\partial\mathbf{r}_{ij}}{\partial\mathbf{v}_{j}} \right|_{\mathbf{s}=\bar{\mathbf{s}}}=\left( \begin{array}{cc}\mathbf{R}( \theta_{ij})^{\mathsf{T}}\mathbf{R}(\theta_{i})^{\mathsf{T}}&\mathbf{0}\\ \mathbf{0}^{\mathsf{T}}&1\end{array} \right), \tag{37b}\]  
where \(\
icefrac{{\partial\mathbf{R}(\theta_{i})^{\mathsf{T}}}}{{\partial\theta_{i}}}\) in \(\mathbf{A}_{ij}\) is given by  
\[\frac{\partial\mathbf{R}(\theta)^{\mathsf{T}}}{\partial\theta}=\left( \begin{array} []{cc}-\sin\theta&-\cos\theta\\ \cos\theta&-\sin\theta\end{array} \right). \tag{38}\]The derivatives for the second error function \(\mathbf{r}_{ik}(\mathbf{v}_{i},\phi)\) are defined as  
\[\mathbf{C}_{ik} =\left.\frac{\partial\mathbf{r}_{ik}}{\partial\phi}\right|_{ \mathbf{s}=\mathbf{s}}=\frac{\partial\mathbf{R}(\phi)^{\mathsf{T}}}{\partial \phi}\mathbf{t}_{i}\,, \tag{39a}\] \[\mathbf{D}_{ik} =\left.\frac{\partial\mathbf{r}_{ik}}{\partial\mathbf{v}_{i}} \right|_{\mathbf{s}=\mathbf{s}}=\left( \begin{array}{cc}\mathbf{R}(\phi)& \mathbf{0}\end{array} \right)\,. \tag{39b}\]  
The partial derivate of the rotation matrix w.r.t. the rotation angle in \(\mathbf{C}_{ik}\) takes the same form as presented in Equation (38). Note that we have not taken any derivatives w.r.t. the GPS coordinates, as we do not wish to reconfigure them, and thus they do not appear in the state vector. For clarity, we specify the dimensions of these matrices: both \(\mathbf{A}\) and \(\mathbf{B}\) are \(3\times 3\) matrices, \(\mathbf{C}\) is a \(2\times 1\) matrix, and \(\mathbf{D}\) is a \(2\times 3\) matrix. These derivatives lead to sparse Jacobian matrices for each of the error functions,  
\[\mathbf{J}_{ij} =\left( \begin{array}{cccc}\ldots&\ldots&\mathbf{A}_{ij}& \mathbf{B}_{ij}&\ldots\end{array} \right)_{3\times 1+3n}\,, \tag{40a}\] \[\mathbf{J}_{ik} =\left( \begin{array}{cccc}\mathbf{C}_{ik}&\ldots&\mathbf{D}_{ ik}&\ldots&\ldots\end{array} \right)_{2\times 1+3n}\,. \tag{40b}\]  
We use ellipses to indicate that unspecified values in the matrix are zeros.