# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## 3 Localization and Mapping
### Local Refinement
#### 3.2.2 Refinement  
In order to find an optimal configuration of the new state vector, we must define an error function corresponding to the expected tree stem location that we initialized by computing the cluster centers. The error is simply the difference between the expected position of the stem \(\mathbf{m}_{\ell}\) and the observation \(\hat{\mathbf{z}}_{iq}\). Since the observations in the global coordinate frame are subject to change during optimization, we will define the error functions on the observations from the coordinate frame of the corresponding camera, i.e., \(\mathbf{z}_{iq}\), as we did with the error functions defined in the previous section. Thus, the error function is defined as  
\[\mathbf{r}_{i\ell}(\hat{\mathbf{v}}_{i},\mathbf{m}_{\ell})=\mathbf{R}(\hat{ \theta}_{i})^{\mathsf{T}}(\mathbf{m}_{\ell}-\hat{\mathbf{t}}_{i})-\mathbf{z}_{ iq}\,, \tag{51}\]  
which leads to an additional term in the objective function,  
\[e(\mathbf{s})=\sum_{ij}\mathbf{r}_{ij}^{\mathsf{T}}\mathbf{Q}_{ij}\mathbf{r}_ {ij}+\sum_{(i\in\mathcal{E})\in\mathcal{C}}\mathbf{r}_{ii}^{\mathsf{T}} \mathbf{Q}_{ik}\mathbf{r}_{ik}+\sum_{((i\
eq j),\ell)\in\mathcal{M}}\mathbf{r} _{i\ell}^{\mathsf{T}}\mathbf{Q}_{iq}\mathbf{r}_{i\ell}\,. \tag{52}\]  
The matrix \(\mathbf{Q}_{iq}\) is the information matrix for the \(q^{\text{th}}\) observation in camera \(i\). Since the information matrix in the inverse of the covariance matrix, we can define \(\mathbf{Q}_{iq}\) in terms of the expected measurement noise of the sensor. As we back projected the detected stem to the camera coordinate frame using parameters from the stereo camera, the information matrix will, in general, have relatively small values for stems detected far from the camera rig. We proceed in the same manner as we did in the previous section by linearizing the error function and constructing the Jacobian to compute the Hessian matrix and gradient vector. The partial derivatives of the new error function can be written as  
\[\mathbf{E}_{i\ell} =\left.\frac{\partial\mathbf{r}_{i\ell}}{\partial\hat{\mathbf{v} }_{i}}\right|_{\mathbf{s}=\mathbf{s}}=\left( \begin{array}{cc}-\mathbf{R}( \hat{\theta}_{i})&\frac{\partial\mathbf{R}(\hat{\theta}_{i})^{\mathsf{T}}}{ \partial\hat{\theta}_{i}}(\mathbf{m}_{\ell}-\hat{\mathbf{t}}_{i})\end{array}  \right)\,, \tag{53a}\] \[\mathbf{F}_{i\ell} =\left.\frac{\partial\mathbf{r}_{i\ell}}{\partial\mathbf{m}_{\ell }}\right|_{\mathbf{s}=\mathbf{s}}=\mathbf{R}(\hat{\theta}_{i})\,. \tag{53b}\]  
\(\mathbf{E}_{i\ell}\) is a \(2\times 3\) matrix and \(\mathbf{F}_{i\ell}\) is a \(2\times 2\) matrix. These definitions can be used directly to compute the blocks with the sparse Hessian matrix,  
\[\mathbf{H}_{i\ell}=\left( \begin{array}{ccccc}\ddots&&&\\ &\mathbf{E}_{i\ell}^{\mathsf{T}}\mathbf{Q}_{iq}\mathbf{E}_{i\ell}&\ldots& \mathbf{E}_{i\ell}^{\mathsf{T}}\mathbf{Q}_{iq}\mathbf{F}_{i\ell}&\\ &\vdots&\ddots&\vdots&\\ &&&\mathbf{F}_{i\ell}^{\mathsf{T}}\mathbf{Q}_{iq}\mathbf{F}_{i\ell}&\\ &&&\ddots\end{array} \right), \tag{54}\]  
Figure 4: Graphical model of local refinement. Circles denote nodes (vertices) and lines denote edges (constraints). Odometry nodes are represented as \(\mathbf{v}\), GPS nodes as \(\mathbf{g}\), and tree stem positions as \(\mathbf{m}\). where the Hessian matrix is now a \((1+3n+2|\mathcal{L}|)\times(1+3n+2|\mathcal{L}|)\) matrix. Similarly, the entries to the gradient vector are  
\[\mathbf{b}_{i\ell}=\left( \begin{array}{c}\vdots\\ \mathbf{E}_{i\ell}^{\intercal}\mathbf{Q}_{i\ell}\mathbf{r}_{i\ell}\\ \vdots\\ \mathbf{F}_{i\ell}^{\intercal}\mathbf{Q}_{i\ell}\mathbf{r}_{i\ell}\\ \vdots\end{array} \right) \tag{55}\]  
The complete linear system is constructed by summing the Hessian matrices and gradient vectors for all the tree stem observations and adding them to the other Hessians and gradient vectors defined in Equation (45). Again, the first row and column of the Hessian, as well as the first row in the gradient vector, are removed to account for the omission of the global orientation parameter.