# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## 2 Direct Visual Odometry
### Image Warping  
The egomotion of a camera with no assumed holonomic constraints has 6 degrees of freedom; rotation about the \(x,y\) and \(z\) axes and translation along the \(x,y\) and \(z\) axes. The motion according to these degrees of freedom is represented by a transformation matrix in the special Euclidean Lie group:  
\[\mathbf{T}=\left( \begin{array}{cc}\mathbf{R}&\mathbf{t}\\ \mathbf{0}^{\mathbf{T}}&1\end{array} \right)\in\mathtt{SE}(3)\,, \tag{5}\]  
where \(\mathbf{R}\in\mathtt{SO}(3)\) is a 3D rotation matrix and \(\mathbf{t}\in\mathbb{R}^{3}\) is a 3D displacement or translation vector. This is a rigid-body transformation that encodes the change in rotation and translation of a non-deformable object in motion between two discrete points in time. Although the matrix \(\mathbf{T}\) has 6 degrees of freedom, there are 12 values that make up the non-homogeneous portion of the matrix: 9 values in the rotation matrix and 3 values for translation. This imposes unnecessary computational demands in an optimization setting. For this reason, we use the Lie algebra \(\mathfrak{se}(3)\) to parameterize the transformation. We denote the transformation matrix as a function of the parameters \(\xi\in\mathfrak{se}(3)\),  
\[\mathbf{T}(\xi)=\exp(\xi)\in\mathtt{SE}(3)\,. \tag{6}\]  
The exponential map of \(\mathfrak{se}(3)\) can be computed in closed form, as can the logarithm map that takes \(\mathtt{SE}(3)\) back to the algebra \(\mathfrak{se}(3)\),  
\[\xi=\ln(\mathbf{T}(\xi))\in\mathfrak{se}(3)\,. \tag{7}\]  
Using the Lie algebra parameterization of the transformation, we define a warping function \(\mathcal{W}:(\mathbb{R}^{3}\times\mathbb{R}^{6})\to\mathbb{R}^{2}\) that takes a homogeneous pixel coordinate as an input, back projectsit to \(\mathbb{R}^{3}\), transforms the back projection according to the warping parameters, and then projects it back to \(\mathbb{R}^{2}\),  
\[\mathcal{W}(\widetilde{\mathbf{p}};\boldsymbol{\zeta})=\pi\Big{(} \mathsf{T}(\boldsymbol{\zeta})\pi^{-1}(\widetilde{\mathbf{p}},z)\Big{)}\,, \tag{8}\]  
where the notation \(\widetilde{\mathbf{p}}=(u,v,1)^{\mathsf{T}}\in\mathbb{P}^{2}\) denotes a pixel position vector in homogeneous coordinates. The function \(\pi^{-1}(\cdot)\) performs the back-projection and is defined as  
\[\pi^{-1}(\widetilde{\mathbf{p}},z)=\left(\Big{(}z\mathbf{K}^{-1 }\widetilde{\mathbf{p}}\Big{)}^{\mathsf{T}},1\right)^{\mathsf{T}}\in\mathbb{ R}^{4}\,, \tag{9}\]  
where \(z\) is a depth measurement for the pixel and \(\mathbf{K}^{-1}\) is the inverse of the projection matrix. Note that we homogenized, i.e., appended a fictitious coordinate, to the back-projected vector so that it is compatible with the transformation matrix. The transformation matrix is not homogeneous, i.e., the last row of the matrix shown in Equation (5) is omitted, so the resulting vector following the transformation has three dimensions. The function that performs the forward projection is defined as  
\[\pi(\mathbf{x})=\widetilde{\mathbf{n}}(\mathbf{K}\mathbf{x})\in \mathbb{R}^{2}\,, \tag{10}\]  
where \(\mathbf{x}=(x,y,z)^{\mathsf{T}}\in\mathbb{R}^{3}\), \(\mathbf{K}\) is the camera projection matrix, and \(\widetilde{\mathbf{n}}:\mathbb{R}^{3}\to\mathbb{R}^{2}\) normalizes the homogeneous coordinates, i.e., \(\widetilde{\mathbf{n}}\Big{(}(x,y,w)^{\mathsf{T}}\Big{)}=(x/w,y/w)^{\mathsf{T}}\). Finally, the projection matrix and its inverse are defined as  
\[\mathbf{K}=\left( \begin{array}{ccc}f&0&c_{u}\\ 0&f&c_{v}\\ 0&0&1\end{array} \right)\,,\quad\mathbf{K}^{-1}=\left( \begin{array}{ccc} \frac{1}{f}&0&-\frac{c_{u}}{f}\\ 0&\frac{1}{f}&-\frac{c_{v}}{f}\\ 0&0&1\end{array} \right), \tag{11}\]  
where \(f\) is the focal length, assuming unit aspect ratio pixels, and \((c_{u},c_{v})\) is the principal point. The only variable that still needs attention is the depth measurement \(z\) used in the back-projection function. We estimate the depth of each pixel by first computing the stereo correspondence via semi-global matching [51]. We use a real-time GPU implementation presented in [52].