## 4 Visual Odometry
### Feature Based Keyframe Refinement  
Feature based methods of camera pose estimation can obtain good accuracy, but it is difficult to achieve high computational efficiency. Feature based methods need to extract features and match them for every single image, but there is much redundancy in the content of adjacent images, so this method was very inefficient. However, the hybrid method, which combined direct and indirect methods, used direct image alignment to obtain the pose of the camera in a non-keyframe, and when the distance between the last keyframe and the current frame was large enough, the keyframe was added to update the pose of the current keyframe by using the feature matching method, which significantly improved the computational efficiency.  
After direct image alignment, the algorithm determined whether a new keyframe needed to be added. When the new keyframe was created, the FAST keypoints were extracted using the method described in Section 4.2, and then, the ORB descriptors were computed to match the features of the last keyframe. In our algorithm, because the current keyframe camera pose, which was close to the real pose, was obtained by direct image alignment, the matching process could be accelerated by the initial new keyframe camera pose. First, all the points visible in the last keyframe were projected in the new keyframe according to the initial pose of the new keyframe. Then, keypoints that were near the projection keypoint in the new keyframe were selected as the candidate points to match the projection point. The keypoint with the highest similarity was the matching point. Thus, the number of candidate keypoints was greatly reduced, so the computational cost remained small.  
After using the ORB descriptor to match keypoints between the last left keyframe image and the new left keyframe image, the right keyframe image information was needed to obtain the depth of the keypoints in the new keyframe. For keypoints that were matched with the last keyframe, the initial depth could be obtained by projecting the points. Therefore, when searching for depth information, we could make full use of these initial depth values to accelerate the computation. According to the initial depth of the keypoint, the searching range of the corresponding position of the keypoint in the right image could be determined. Then, the sum of absolute difference (SAD) method was used to find the corresponding position of this keypoint in the right image. However, for a new keypoint extracted from the left image that did not match, SAD was used to find the corresponding match across the epipolar line in the right image.  
Once the matching relationship of keypoints between the new keyframe and the last keyframe was obtained, the pose of the new keyframe could be optimized by minimizing the re-projection error of the keypoints from 3D to 2D:  
\[\mathbf{\xi}^{*}=\arg\min_{\xi}\frac{1}{2}\sum_{i=1}^{n}\parallel\mathbf{p}_{i}- \pi(\exp(\mathbf{\xi}^{\wedge})\mathbf{P}_{i})\parallel_{2}^{2}, \tag{20}\]  
where \(\mathbf{P}_{i}\) is the 3D point that can be seen in the last keyframe and the new keyframe at the same time and \(\pi(\exp(\mathbf{\xi}^{\wedge})\mathbf{P}_{i})\) is the projection function of \(\mathbf{P}_{i}\) in the new current keyframe pose \(\exp(\mathbf{\xi}^{\wedge})\) and \(\parallel\cdot\parallel_{2}\) is the Euclidean norm.