# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## 3 Localization and Mapping
### Global Alignment
#### 3.1.2 Optimization
Since the odometry part of the pose graph only has constraints between consecutive nodes, the Jacobian \(\mathbf{J}_{ij}\) will always have a contiguous \(3\times 6\) block of non-zero values corresponding to the odometry constraints between nodes \(i\) and \(j\). Furthermore, the Jacobian \(\mathbf{J}_{ik}\) will always have non-zero values in the first \(2\times 1\) block corresponding to the global orientation parameter and a \(2\times 3\) non-zero block at the \(i\)th node representing the constraint between the GPS-odometry correspondences. Now that the Jacobians are specified, we obtain the Gauss-Netwon approximation to the Hessian matrices by  
\[\mathbf{H}_{ij} =\mathbf{J}_{ij}^{\mathsf{T}}\mathbf{Q}_{ij}\mathbf{J}_{ij}\,, \tag{41a}\] \[\mathbf{H}_{ik} =\mathbf{J}_{ik}^{\mathsf{T}}\mathbf{Q}_{ik}\mathbf{J}_{ik}\,, \tag{41b}\]  
and the gradient vectors by  
\[\mathbf{b}_{ij} =\mathbf{J}_{ij}^{\mathsf{T}}\mathbf{Q}_{ij}\mathbf{r}_{ij}\,, \tag{42a}\] \[\mathbf{b}_{ik} =\mathbf{J}_{ik}^{\mathsf{T}}\mathbf{Q}_{ik}\mathbf{r}_{ik}\,. \tag{42b}\]  
From an implementation standpoint, it is easier to construct the Hessian directly using our definitions for the non-zeros blocks in the Jacobians. The Hessian matrix for the \(i\)th pose in graph, assuming there is a corresponding GPS coordinate with the node, takes the form  
\[\mathbf{H}_{ij}+\mathbf{H}_{ik}=\left( \begin{array}{cccc}\mathbf{C}_{ik}^{ \mathsf{T}}\mathbf{Q}_{ik}\mathbf{C}_{ik}&\ldots&\mathbf{C}_{ik}^{\mathsf{T}} \mathbf{Q}_{ik}\mathbf{D}_{ik}&\ldots&\\ \vdots&\ddots&\vdots&\\ \mathbf{D}_{ik}^{\mathsf{T}}\mathbf{Q}_{ik}\mathbf{C}_{ik}&\ldots&\mathbf{A} _{ij}^{\mathsf{T}}\mathbf{Q}_{ij}\mathbf{A}_{ij}+\mathbf{D}_{ik}^{\mathsf{T}} \mathbf{Q}_{kj}\mathbf{D}_{ik}&\mathbf{A}_{ij}^{\mathsf{T}}\mathbf{Q}_{ij} \mathbf{B}_{ij}&\ldots\\ \vdots&&\mathbf{B}^{\mathsf{T}}\mathbf{Q}_{ij}\mathbf{A}_{ij}&\mathbf{B}_{ij}^ {\mathsf{T}}\mathbf{Q}_{ij}\mathbf{B}_{ij}&\\ &&\vdots&&\ddots\end{array} \right)\,. \tag{43}\]  
Since the Jacobians are sparse, the resulting Hessian matrix is also sparse. Therefore, in practice, it is advantageous to use a memory-efficient sparse storage scheme for these matrices, e.g., compressed sparse column or row matrices. The gradient vector for the \(i\)th pose can be constructed directly with  
\[\mathbf{b}_{ij}+\mathbf{b}_{ik}=\left( \begin{array}{c}\mathbf{C}_{ik}^{\mathsf{ T}}\mathbf{Q}_{k}\mathbf{r}_{ik}\\ \vdots\\ \mathbf{A}_{ij}^{\mathsf{T}}\mathbf{Q}_{ij}\mathbf{r}_{ij}+\mathbf{D}_{ik}^{ \mathsf{T}}\mathbf{Q}_{k}\mathbf{r}_{ik}\\ \mathbf{B}_{ij}^{\mathsf{T}}\mathbf{Q}_{ij}\mathbf{r}_{ij}\\ \vdots\end{array} \right). \tag{44}\]  
The final Hessian matrix and gradient vector for the linear system are obtained by summing over all the constraint-wise Hessians and descent vectors,  
\[\mathbf{H}=\sum_{ij}\mathbf{H}_{ij}+\sum_{(i,k)\in\mathcal{C}} \mathbf{H}_{ik}\,, \tag{45a}\] \[\mathbf{b}=\sum_{ij}\mathbf{b}_{ij}+\sum_{(i,k)\in\mathcal{C}} \mathbf{b}_{ik}\,. \tag{45b}\]  
This linearization is performed during each iteration of the LM algorithm. We take advantage of the sparse structure of the Hessian and solve the system using sparse Cholesky factorization. We terminate the algorithm when the Euclidean norm of the linear increment to the state vector \(\|\Delta\mathbf{s}\|\) is less than some small threshold, e.g., 0.001, and take the optimal configuration as \(\mathbf{s}^{*}=\mathbf{s}+\Delta\mathbf{s}\). Finally, we rotate each pose in the optimal state vector according the global orientation parameter and translate back to the UTM coordinate system and update the rotation component of each pose,  
\[\hat{\mathbf{v}}_{i}=\left( \begin{array}{c}\mathbf{R}(\phi^{*})\mathbf{t}_{i }^{*}+\mathbf{g}_{0}\\ \theta_{i}^{*}+\phi^{*}\end{array} \right),\quad\forall\{i\}_{1}^{n}\,.