# Vision-Aided Localization and Mapping in Forested Environments Using Stereo Images
## 2 Direct Visual Odometry
### Optimization  
We will now extend our discussion regarding the minimization of photometric error. As noted earlier, minimizing the expression defined in Equation (2) is a non-linear optimization task. This formulation requires a linearization step during each GN iteration. Namely, the Jacobian of the warp and the Hessian need to be computed during each iteration, which can lead to significant computational demands depending on the size of the Jacobian. Following from [PERSON] and [PERSON] (2016), we redefine the objective under the inverse compositional (IC) formulation by interchanging the roles of the reference and input image and solving for incremental warp parameters instead of additive updates as in the Lucas-Kanade formulation. Given some initial guess of the parameters, \(\xi\), the objective is to minimize the following expression w.r.t the incremental warp parameters,  
\[\Delta\xi^{\star}=\operatorname*{argmin}_{\Delta\xi}\sum_{\mathbf{p}\in \Phi}\left\|\mathcal{I}\left(\mathcal{W}(\widetilde{\mathbf{p}};\Delta\xi) \right)-\mathcal{I}^{\prime}(\mathcal{W}(\widetilde{\mathbf{p}};\xi))\right\|^ {2}. \tag{14}\]  
The parameters are updated by inverting the incremental warp parameters and composing with the current estimate, \(\xi\leftarrow\xi\circ\Delta\xi^{-1}\), where the notation \(\circ\) denotes composition. The incremental warp parameters need to be inverted at each GN iteration as the linearization, which we will discuss next, is performed on the reference image. The update rule can be explicitly written as  
\[\xi_{\kappa+1} =\xi_{\kappa}\circ\Delta\xi^{-1}\] \[=\ln\left(\exp(\xi_{\kappa})\exp(-\Delta\xi)\right)\] \[=\ln\left( \begin{array}{cc}\mathbf{R}_{\kappa}\Delta\mathbf{R} ^{\mathsf{T}}&\mathbf{R}_{\kappa}(-\Delta\mathbf{R}^{\mathsf{T}}\Delta t)+ \mathbf{t}_{\kappa}\\ \mathbf{0}^{\mathsf{T}}&1\end{array} \right). \tag{15}\]  
According to the GN algorithm, the incremental warp parameters \(\Delta\xi\) are given by the normal equations,  
\[\mathbf{J}^{\mathsf{T}}\mathbf{J}\Delta\xi=-\mathbf{J}^{\mathsf{T}}\mathbf{r} \implies\Delta\xi=-\left(\mathbf{J}^{\mathsf{T}}\mathbf{J}\right)^{-1} \mathbf{J}^{\mathsf{T}}\mathbf{r}, \tag{16}\]  
where \(\mathbf{J}\) is a \(m\times 6\) Jacobian matrix, \(\mathbf{J}^{\mathsf{T}}\mathbf{J}\) is the Gauss-Newton approximation of the Hessian matrix, and \(\mathbf{r}\) is the vector of residuals given by  
\[\mathbf{r}=\mathcal{I}^{\prime}\left(\mathcal{W}(\widetilde{\mathbf{p}};\xi) \right)-\mathcal{I}(\mathbf{p})\,. \tag{17}\]  
The Jacobian encodes the partial derivatives of the reference image at each pixel \(\mathbf{p}_{\{i\}_{1}^{\mathsf{T}}}\) with respect to the six warping parameters,  
\[\mathbf{J}=\left( \begin{array}{cccc}\frac{\partial\mathcal{I}(\mathbf{p}_{ 1})^{\mathsf{T}}}{\partial\xi}^{\mathsf{T}}\\ \frac{\partial\mathcal{I}(\mathbf{p}_{2})}{\partial\xi}^{\mathsf{T}}\\ \vdots\\ \frac{\mathcal{I}(\mathbf{p}_{m})}{\partial\xi}^{\mathsf{T}}\end{array} \right) =\left( \begin{array}{cccc}\frac{\partial\mathcal{I}(\mathbf{p}_{1})}{ \partial\xi_{1}}&\frac{\partial\mathcal{I}(\mathbf{p}_{1})}{\partial\xi_{2}}& \ldots&\frac{\partial\mathcal{I}(\mathbf{p}_{1})}{\partial\xi_{6}}\\ \frac{\partial\mathcal{I}(\mathbf{p}_{2})}{\partial\xi_{1}}&\frac{\partial \mathcal{I}(\mathbf{p}_{2})}{\partial\xi_{2}}&\ldots&\frac{\partial \mathcal{I}(\mathbf{p}_{2})}{\partial\xi_{6}}\\ \vdots&\vdots&\ddots&\vdots\\ \frac{\partial\mathcal{I}(\mathbf{p}_{m})}{\partial\xi_{1}}&\frac{\partial \mathcal{I}(\mathbf{p}_{m})}{\partial\xi_{2}}&\ldots&\frac{\partial \mathcal{I}(\mathbf{p}_{m})}{\partial\xi_{6}}\end{array} \right). \tag{18}\]  
The linearization of Equation (14) can be achieved by performing a first-order Taylor expansion about the current estimate of the parameters.