# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## 2 Direct Visual Odometry
### Optimization
Denoting the \(i\)th row in the Jacobian as \(\
icefrac{{\partial\mathcal{I}(\mathbf{p})}}{{\partial\xi}}/{\partial\xi}\), corresponding to some pixel \(\mathbf{p}\in\Phi\) and applying the chain rule, we obtain  
\[\frac{\partial\mathcal{I}(\mathbf{p})}{\partial\xi}^{\mathsf{T}}=\frac{ \partial\mathcal{I}(\mathbf{p})}{\partial\mathbf{p}}\frac{\partial\mathbf{p}} {\partial\mathbf{x}}\frac{\partial\mathbf{x}}{\partial\xi}. \tag{19}\]The partial derivative of the reference image w.r.t. some pixel position \(\mathbf{p}\) is simply the gradient vector of the image along the \(u\) and \(v\) axes,  
\[\frac{\partial\mathcal{I}(\mathbf{p})}{\partial\mathbf{p}}=\
abla\mathcal{I}( \mathbf{p})=\left(\mathcal{I}_{u}(\mathbf{p}),\mathcal{I}_{v}(\mathbf{p}) \right). \tag{20}\]  
We purposefully denoted the gradient vector as a row vector. Taking \(\mathbf{p}\) to be equal to a reduced form of the forward projection function \(\pi(\cdot)\) such that \(\mathbf{p}=\left(\
icefrac{{r}}{{2}}+c_{u},\
icefrac{{fy}}{{2}}+c_{v}\right)^ {\mathsf{T}}\), and \(\mathbf{x}=\left(x,y,z\right)^{\mathsf{T}}\) to be a back projection of the pixel, the partial derivative can be written as  
\[\frac{\partial\mathbf{p}}{\partial\mathbf{x}}=\left( \begin{array}{ccc}\frac {f}{z}&0&-f\frac{x}{z^{2}}\\ 0&\frac{f}{z}&-f\frac{y}{z^{2}}\end{array} \right). \tag{21}\]  
Finally, the partial derivative of the back-projected pixel \(\mathbf{x}\) w.r.t. the warping parameters evaluated at the identity warp \(\boldsymbol{\xi}=\boldsymbol{0}\) takes the form,  
\[\left.\frac{\partial\mathbf{x}}{\partial\boldsymbol{\xi}}\right|_{\boldsymbol {\xi}=\boldsymbol{0}}=\left(\left[\mathbf{x}\right]_{\times}\mathbf{I}_{3} \right)=\left( \begin{array}{cccccc}0&-z&y&1&0&0\\ z&0&-x&0&1&0\\ -y&x&0&0&0&1\end{array} \right). \tag{22}\]  
This result follows from the skew-symmetric matrix operator used when computing the \(\mathtt{SE}(3)\) exponential map of the warp parameters. Multiplying out the last two partials gives  
\[\frac{\partial\mathcal{I}(\mathbf{p})}{\partial\boldsymbol{\xi}} ^{\mathsf{T}} =\
abla\mathcal{I}(\mathbf{p})\frac{\partial\mathbf{p}}{\partial \mathbf{x}}\frac{\partial\mathbf{x}}{\partial\boldsymbol{\xi}}\] \[=\left(\mathcal{I}_{u}(\mathbf{p}),\mathcal{I}_{v}(\mathbf{p}) \right)\left( \begin{array}{cccccc}f\frac{xy}{z^{2}}&f\frac{x^{2}-z^{2}}{z^{2 }}&\frac{xy}{z}&\frac{f}{z}&0&-f\frac{x}{z^{2}}\\ f\frac{y^{2}+z^{2}}{z^{2}}&-f\frac{xy}{z^{2}}&-f\frac{x}{z}&0&\frac{f}{z}&-f \frac{y}{z^{2}}\end{array} \right). \tag{23}\]  
For convenience, we also show a row in the Jacobian corresponding to some pixel \(\mathbf{p}\) written out explicitly,  
\[\frac{\partial\mathcal{I}(\mathbf{p})}{\partial\boldsymbol{\xi}} =\left( \begin{array}{c}\frac{\mathcal{I}_{u}(\mathbf{p})/xy+ \mathcal{I}_{v}(\mathbf{p})(\mathbf{p}^{2}+fz^{2})}{z^{2}}\\ \frac{\mathcal{I}_{u}(\mathbf{p})(-(\mathbf{r}x^{2}-fz^{2})-\mathcal{I}_{v}( \mathbf{p})/xy}{z^{2}}\\ \frac{\mathcal{I}_{u}(\mathbf{p})f-\mathcal{I}_{v}(\mathbf{p})/x}{z}\\ \frac{\mathcal{I}_{v}(\mathbf{p})f}{z}\\ \frac{\mathcal{I}_{v}(\mathbf{p})f}{z}\\ \frac{-\mathcal{I}_{u}(\mathbf{p})f-\mathcal{I}_{v}(\mathbf{p})fy}{z^{2}} \end{array} \right)^{\mathsf{T}}\in\mathbb{R}^{6}\,. \tag{24}\]  
The row vector stated above is computed for each pixel \(\mathbf{p}\in\Phi\) and stacked into the \(m\times 6\) Jacobian matrix, again where \(m\) is the number of pixels in \(\Phi\). The columns of the Jacobian can be visualized as the steepest descent images shown in Figure 1.  
Since the linearization is performed on the coordinate frame of the reference image, the Jacobian \(\mathbf{J}\) and the GN approximation of the Hessian \(\mathbf{H}=\mathbf{J}^{\mathsf{T}}\mathbf{J}\), as well as its inverse \(\mathbf{H}^{-1}\) only need to be computed once. These are the computational savings of the IC formulation [50] over the original Lucas-Kanade algorithm [45].