## 4 Visual Odometry
### Direct Image Alignment  
After the pose of the forklift was estimated using the motion model, the algorithm grabbed a new stereo-pair frame, and only the left image was used in this step. First, the relative transformation of the current new frame against the last frame was computed. Further, because the latest keyframe may be updated, the relative transformation of the current new frame against the latest keyframe was then calculated to improve the accuracy of odometry. \(\mathbf{I}_{j}\) represents the current frame; reference frame \(\mathbf{I}_{i}\) represents the last frame or last keyframe; and the transformation between them is expressed as \(\mathbf{\Upsilon}_{ji}\), which is denoted by \(\mathbf{\xi}_{ji}\) in Lie algebra, given by:  
\[\exp\left(\mathbf{\xi}_{ji}^{\wedge}\right)=\mathbf{\Upsilon}_{ji}= \begin{bmatrix}\mathbf{ \mathbf{R}}_{ji}&\mathbf{\mathbf{t}}_{ji}\\ \mathbf{0}&1\end{bmatrix} =\mathbf{\Upsilon}_{j}\mathbf{\Upsilon}_{i}^{-1} \tag{15}\]  
where \(\mathbf{\Upsilon}_{j}\) is the current frame pose estimated by the motion model and \(\mathbf{\Upsilon}_{i}\) is the reference frame pose. For one keypoint \(\mathbf{\mathbf{p}}\) with valid depth information \(d_{\mathbf{\mathbf{p}}}\) in reference frame \(\mathbf{I}_{i}\) observed in the target current frame \(\mathbf{I}_{j}\), given an initial relative transformation, \(\mathbf{\xi}_{ji}\) was estimated by the motion model. According to the projection model of the camera, the keypoint observed \(\mathbf{\mathbf{p}}^{\prime}\) in the current frame is:  
\[\mathbf{\mathbf{p}}^{\prime}=\pi_{c}\left(\exp\left(\mathbf{\xi}_{ji}^{\wedge}\right) \pi_{c}^{-1}(\mathbf{\mathbf{p}},d_{\mathbf{\mathbf{p}}})\right) \tag{16}\]  
Once \(\mathbf{\mathbf{p}}^{\prime}\) is observed in the current frame, the photometric error of \(\mathbf{\mathbf{p}}\) in reference \(\mathbf{I}_{i}\) and \(\mathbf{\mathbf{p}}^{\prime}\) in current frame \(\mathbf{I}_{j}\) can be defined as:  
\[e_{\mathbf{\mathbf{p}}}=\sum_{\mathbf{\mathbf{p}}\in\mathcal{N}_{\mathbf{\mathbf{p}}}} \omega_{\mathbf{\mathbf{p}}}\parallel\mathbf{I}_{j}(\mathbf{\mathbf{p}}^{\prime})-\mathbf{I}_{ i}(\mathbf{\mathbf{p}})\parallel_{\gamma} \tag{17}\]  
where \(\mathcal{N}_{\mathbf{\mathbf{p}}}\) denotes a small neighborhood of pixels around keypoint \(\mathbf{\mathbf{p}}\). \(\parallel\cdot\parallel_{\gamma}\) is the Huber norm. In particular, the slightly spread pattern proposed by DSO was used to achieve a trade-off between computational effort and accuracy. \(\omega_{\mathbf{p}}\) is the weight function based on the pixel gradient that is inversely proportional to it. \[\omega_{\mathbf{p}}=\frac{c^{2}}{c^{2}+\|\Delta\mathbf{I}_{i}(\mathbf{p})\|_{2}^{2}} \tag{18}\]  
where cexpresses the constant gradient penalty factor. \(\|\cdot\|_{2}\) is the Euclidean norm. Thus, the optimized relative transformation \(\mathbf{\xi}_{ji}^{*}\) can be computed by minimizing the full photometric error over the reference frame and the current frame [35]. \[\mathbf{\xi}_{ji}^{*}=\arg\min_{\mathbf{\xi}}\sum_{\mathbf{p}\in K_{i}}e_{\mathbf{p}} \tag{19}\]  
where \(K_{i}\) is all keypoints \(\mathbf{p}\) in the reference frame and \(e_{\mathbf{p}}\) denotes the photometric error mentioned earlier. The above optimization process was carried out twice. The first step was to align the new frame and the previous frame to determine the current camera pose; the second step was to align the new frame and the last keyframe with the initial pose of the first step's result and determine the final pose of the camera. Therefore, the reference frame could be set as keyframe or non-keyframe; the depth \(d_{\mathbf{p}_{k}}\) of the keypoint \(\mathbf{p}_{k}\) in the keyframe came from the sum of absolute difference (SAD) in the stereo pair in Section 4.4.