# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## 3 Localization and Mapping
### Local Refinement
#### 3.2.1 Graph Augmentation  
Tree stems are detected in each frame using the convolutional neural network (CNN) object detector outlined in [59]. The input image to the network is resized to a resolution of 128 columns and 352 rows for real-time performance. We also detect the ground plane and breast height using the RANSAC-based algorithm presented in [60]. For each bounding box predicted by the CNN object detector, we extract the image coordinate, where the center-line of the bounding box along the \(u\)-axis of the image intersects the ground plane positioned at breast height. A disparity value is assigned to this image coordinate using Equation (5) in [59]. Using the inverse projection matrix and the disparity assignment, we project the image coordinate to \(\mathbb{R}^{3}\). We represent a tree stem observation in the camera coordinate frame of the \(i\)th camera pose as \(\mathbf{z}_{iq}=\left(x,y\right)^{\mathsf{T}}\), where \(x\) and \(y\) correspond to the back-projected image coordinate along the \(x\) and \(z\) axes of the camera coordinate frame. The index \(q\in\mathcal{Z}_{i}\) where \(\mathcal{Z}_{i}\) is a set of indices denoting the observations of stems from the \(i\)th camera. Thus, \(|\mathcal{Z}_{i}|\) is the number of tree stem observations in camera \(i\) and \(|\mathcal{Z}|\) is the number of camera poses in the graph. We use the notation \((i,q)\in\mathcal{Z}\) to index the \(q^{\text{th}}\) observation in camera \(i\). Recalling that a camera pose in the globally aligned state vector is represented as \(\hat{\mathbf{v}}_{i}=\left(\hat{x},\hat{y},\hat{\theta}\right)^{\mathsf{T}}\), we can transform the observations from the camera coordinate frame to the world frame with  
\[\hat{\mathbf{z}}_{iq}=\mathbf{R}(\hat{\theta}_{i})\mathbf{z}_{iq}+\hat{ \mathbf{t}}_{i}\,,\quad\forall(i,q)\in\mathcal{Z}\,. \tag{48}\]  
Given all tree stem observations in the global coordinate frame, we perform data association by clustering spatially similar observations. We use the density-based spatial clustering of applications with the noise (DBSCAN) algorithm [61] to cluster the observations. DBSCAN takes two parameters and a distance function. For the distance function, we simply use the Euclidean distance. The two parameters correspond to the search radius and the minimum number of points required for a cluster. We use a search radius of 1 m and 10 as the minimum number of points in a cluster. The algorithm yields a label for each observation that specifies to which cluster observation \(\hat{\mathbf{z}}_{iq}\) belongs. A label equal to zero denotes an outlier, i.e., an observation that does not belong to any cluster. We denote the set of unique labels as \(\mathcal{L}\) and use the correspondence set \(\mathcal{M}\) to specify that observation \(\hat{\mathbf{z}}_{iq}\) is assigned to label \(\ell\in\mathcal{L}\). The notation \(((i,q),\ell)\in\mathcal{M}\) is used to denote that observation \(\hat{\mathbf{z}}_{iq}\) is assigned to cluster \(\ell\). For each cluster of tree stem observations, we compute the center of the clusters by  
\[\mathbf{m}_{\ell}=\frac{1}{\sum\mathbf{1}_{\ell}}\sum_{(i,q)\in\mathcal{Z}} \mathbf{1}_{i}\hat{\mathbf{z}}_{iq}\,,\quad\forall\ell\in\mathcal{L}\,, \tag{49}\]  
where \(\mathbf{m}_{\ell}=\left(x,y\right)^{\mathsf{T}}\) is the center of the cluster in the global coordinate frame, and the notation \(\mathbf{1}_{\ell}\) takes 1 when the observation \(\hat{\mathbf{z}}_{iq}\) is assigned to cluster \(\ell\) and zero otherwise. Given the cluster centers we extend our state vector as  
\[\hat{\mathbf{s}}=\left(\hat{\mathbf{v}}_{i}^{\mathsf{T}},\hat{\mathbf{v}}_{2}^ {\mathsf{T}},\ldots,\hat{\mathbf{v}}_{n}^{\mathsf{T}},\mathbf{m}_{1}^{\mathsf{ T}},\mathbf{m}_{2}^{\mathsf{T}},\ldots,\mathbf{m}_{\ell}^{\mathsf{T}}\right). \tag{50}\]  
Note that we omitted the global orientation parameter.