# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## Appendix A  
In this appendix, we provide an overview of the mathematical foundations for special orthogonal and special Euclidean Lie groups. We discuss the closed-form exponential maps and logarithm maps for these groups, as well as their corresponding Lie algebras. This section aims to offer a convenient reference for the reader, summarizing essential concepts and formulas related to these topics, even though these derivations might exist in textbooks. The special orthogonal Lie groups \(\mathtt{SO}(2)\) and \(\mathtt{SO}(3)\) represent rotations in 2 and 3 dimensions, respectively. In \(\mathtt{SO}(2)\), we represent a transformation by a \(2\times 2\) matrix,\(\mathbf{R}\in\mathtt{SO}(2)\), and its Lie algebra by \(\theta\in\mathfrak{so}(2)\). The closed-form exponential map that converts the Lie algebra, \(\mathfrak{so}(2)\), to the Lie group, \(\mathtt{SO}(2)\), is given by  
\[\mathbf{R}=\exp(\theta_{\times})=\left( \begin{array}{cc}\cos\theta&-\sin \theta\\ \sin\theta&\cos\theta\end{array} \right)\in\mathtt{SO}(2)\,, \tag{10}\]  
where \(\theta_{\times}\) denotes a skew symmetric matrix generated from \(\theta\). The inverse of the exponential map is given by the logarithm map that brings the transformation matrix back to the algebra,  
\[\theta=\ln(\mathbf{R})=\tan^{-1}\frac{r_{21}}{r_{11}}\in\mathfrak{so}(2)\,, \tag{11}\]  
where \(r_{21}\) and \(r_{11}\) are entries in \(\mathbf{R}\). In practice the \(\tan^{-1}\) function is replaced by the two-argument inverse tangent function \(\arctan 2(y,x)\) for appropriate quadrant checking. In the case of 3-dimensional rotations, we represent the transformation as a \(3\times 3\) matrix \(\mathbf{R}\in\mathtt{SO}(3)\), and its algebra as \(\mathbf{\omega}\in\mathfrak{so}(3)\), where \(\mathbf{\omega}=(\alpha,\beta,\gamma)^{\mathsf{T}}\) is a vector encoding the rotation angles about the \(x\), \(y\) and \(z\)-axes. The closed-form exponential is given by  
\[\mathbf{R}=\exp(\mathbf{\omega}_{\times})=\mathbf{I}+\left(\frac{\sin\theta}{ \vartheta}\right)\mathbf{\omega}_{\times}+\left(\frac{1-\cos\vartheta}{\vartheta^ {2}}\right)\mathbf{\omega}_{\times}^{2}\in\mathtt{SO}(3)\,, \tag{12}\]  
where \(\vartheta=\sqrt{\mathbf{\omega}^{\mathsf{T}}\mathbf{\omega}}\) and \(\mathbf{\omega}_{\times}\) is defined as  
\[\mathbf{\omega}_{\times}=\left( \begin{array}{ccc}0&-\gamma&\beta\\ \gamma&0&-\alpha\\ -\beta&\alpha&0\end{array} \right)\,. \tag{13}\]  
Equation (12) is Rodrigues' formula for rotating a vector in space, given an axis and a rotation angle. The logarithm map that brings the transformation matrix back to the algebra is given by  
\[\vartheta =\cos^{-1}\frac{\operatorname{Tr}(\mathbf{R})-1}{2}\,, \tag{14a}\] \[\mathbf{\omega}_{\times} =\ln(\mathbf{R})=\frac{\vartheta}{2\sin\vartheta}\Big{(}\mathbf{ R}-\mathbf{R}^{\mathsf{T}}\Big{)}\in\mathfrak{so}(3)\,. \tag{14b}\]  
The algebra is taken from the off-diagonal components of \(\mathbf{\omega}_{\times}\), i.e., skew symmetric matrix to vector. The notation \(\operatorname{Tr}(\cdot)\) is the trace function of a square matrix, which returns the sum of the elements along the diagonal. In both the 2D and 3D cases, the column vectors of the matrices representing the transformations are orthogonal. Thus, the inverse is equivalent to the matrix transpose,  
\[\mathbf{R}^{-1} =\mathbf{R}^{\mathsf{T}}\in\mathtt{SO}(2)\,, \tag{15a}\] \[\mathbf{R}^{-1} =\mathbf{R}^{\mathsf{T}}\in\mathtt{SO}(3)\,. \tag{15b}\]  
Rigid transformations in 2 and 3 dimensions are represented by the special Euclidean Lie groups \(\mathtt{SE}(2)\) and \(\mathtt{SE}(3)\). By definition, these groups include rotations from the special orthogonal group described in the previous section,  
\[\mathtt{SE}(2) \stackrel{{\mathsf{def}}}{{=}}\mathtt{SO}(2)\times \mathbb{R}^{2}\,, \tag{16a}\] \[\mathtt{SE}(3) \stackrel{{\mathsf{def}}}{{=}}\mathtt{SO}(3)\times \mathbb{R}^{3}\,. \tag{16b}\]  
A rigid transformation in 2D takes the form  
\[\mathbf{T}=\left(