# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## Appendix A
\begin{array}{cc}\mathbf{R}\in\mathtt{SO}(2)&\mathbf{t}\in \mathbb{R}^{2}\\ \mathbf{0}^{\mathsf{T}}&1\end{array} \right)\in\mathtt{SE}(2)\,. \tag{17}\]This matrix is homogeneous for compatibility with inversion and composition operations. We represent the parameter space, i.e., the Lie algebra, as \(\boldsymbol{\xi}=\left(\theta,x,y\right)^{\mathsf{T}}=\left(\theta,\mathbf{u} \right)^{\mathsf{T}}\in\mathfrak{se}(2)\). The exponential map is given in closed form as  
\[\mathbf{T}=\exp(\boldsymbol{\hat{\xi}})=\left( \begin{array}{cc}\exp(\theta_{ \times})&\mathbf{V}\mathbf{u}\\ \mathbf{0}^{\mathsf{T}}&1\end{array} \right)=\left( \begin{array}{cc}\mathbf{ R}&\mathbf{t}\\ \mathbf{0}^{\mathsf{T}}&1\end{array} \right)\in\mathfrak{SE}(2) \tag{10}\]  
where \(\exp(\theta_{\times})\) is defined in Equation (10) and  
\[\mathbf{V}=\frac{1}{\theta}\left( \begin{array}{cc}\sin\theta&-(1-\cos\theta) \\ 1-\cos\theta&\sin\theta\end{array} \right). \tag{11}\]  
The logarithm map that brings the transformation matrix back to the algebra is given by  
\[\boldsymbol{\xi}=\left( \begin{array}{c}\theta\\ \mathbf{V}^{-1}\mathbf{t}\end{array} \right)\in\mathfrak{se}(2)\;, \tag{12}\]  
where \(\theta=\ln(\mathbf{R})\) is defined in Equation (11) and \(\mathbf{V}^{-1}\) is defined as  
\[\vartheta =\frac{\sin\theta}{\theta}\;, \tag{13a}\] \[\phi =\frac{1-\cos\theta}{\theta}\;,\] (13b) \[\mathbf{V}^{-1} =\frac{1}{\theta^{2}+\phi^{2}}\left( \begin{array}{cc}\vartheta &\phi\\ -\phi&\phi\end{array} \right). \tag{13c}\]  
Rigid transformations in 3-dimensional space are written similarly to our definition for \(\mathfrak{SE}(2)\) but with 3D rotation matrices and 3-vector translations,  
\[\mathbf{T}=\left( \begin{array}{cc}\mathbf{R}\in\mathfrak{SO}(3)&\mathbf{t} \in\mathbb{R}^{3}\\ \mathbf{0}^{\mathsf{T}}&1\end{array} \right)\in\mathfrak{SE}(3)\;. \tag{14}\]  
We use the variable \(\boldsymbol{\xi}\) to represent the algebra, \(\boldsymbol{\xi}=\left(\boldsymbol{\omega},\mathbf{u}\right)^{\mathsf{T}}\in \mathfrak{se}(3)\), where \(\boldsymbol{\omega}=\left(\alpha,\beta,\gamma\right)^{\mathsf{T}}\) and \(\mathbf{u}=\left(x,y,z\right)^{\mathsf{T}}\). The exponential map again can be computed in closed form,  
\[\exp(\boldsymbol{\hat{\xi}})=\exp( \begin{array}{c}\boldsymbol{\omega}_{ \times}\\ \mathbf{u}\end{array} )=\left( \begin{array}{cc}\mathbf{R}&\mathbf{V}\mathbf{u }\\ \mathbf{0}^{\mathsf{T}}&1\end{array} \right)\in\mathfrak{SE}(3)\;, \tag{15}\]  
where \(\mathbf{R}\), defined in Equation (12) is restated below for convenience, and \(\mathbf{V}\) are given by  
\[\theta =\sqrt{\boldsymbol{\omega}^{\mathsf{T}}\boldsymbol{\omega}}\,, \tag{16a}\] \[\vartheta =\frac{\sin\theta}{\theta}\;,\] (16b) \[\phi =\frac{1-\cos\theta}{\theta^{2}}\,,\] (16c) \[\psi =\frac{1-\vartheta}{\theta^{2}}\,,\] (16d) \[\mathbf{R} =\mathbf{I}+\vartheta\boldsymbol{\omega}_{\times}+\vartheta \boldsymbol{\omega}_{\times}^{2}\,,\] (16e) \[\mathbf{V} =\mathbf{I}+\phi\boldsymbol{\omega}_{\times}+\psi\boldsymbol{ \omega}_{\times}^{2}\,. \tag{16f}\]  
Finally, the logarithm map is written as  
\[\boldsymbol{\xi}=\ln(\mathbf{T})=\left( \begin{array}{c}\ln(\mathbf{R})\\ \mathbf{V}^{-1}\mathbf{t}\end{array} \right)\in\mathfrak{se}(3)\,. \tag{16}\]The logarithm map of the rotation part is given by Equation (A5), and the inverse of \(\mathbf{V}\) is defined as  
\[\mathbf{V}^{-1}=\mathbf{I}+\frac{1}{2}\mathbf{\omega}_{\times}+\frac{1}{\theta^{2}} \bigg{(}1-\frac{\theta}{2\phi}\bigg{)}\mathbf{\omega}_{\times}^{2}\,.\] (A17)  
Composition of two transformation is equivalent for \(\mathtt{SE}(2)\) and \(\mathtt{SE}(3)\),  
\[\mathbf{T}_{1}\circ\mathbf{T}_{2}=\left( \begin{array}{cc}\mathbf{R}_{1} \mathbf{R}_{2}&\mathbf{R}_{1}\mathbf{t}_{2}+\mathbf{t}_{1}\\ \mathbf{0}^{\mathsf{T}}&1\end{array} \right)\,,\] (A18)  
and inverting a transformation is given by  
\[\mathbf{T}^{-1}=\left( \begin{array}{cc}\mathbf{R}^{\mathsf{T}}&-\mathbf{R}^{ \mathsf{T}}\mathbf{t}\\ \mathbf{0}^{\mathsf{T}}&1\end{array}