## 4 Visual Odometry
### Feature Detection  
In the existing SLAM framework, the selection of points to track is related to the accuracy of camera pose estimation. In particular, in order to increase the accuracy of pose estimation, features that need to be extracted should be distributed as evenly as possible in the image. However, the location of feature point extraction depends heavily on the image itself, so it is difficult to make the features evenly distributed to the greatest extent possible. For this reason, a two step feature extractor with a dynamic threshold was proposed, which could significantly improve the extraction efficiency. The detailed steps are shown in Algorithm 1. ```
Input:\(I_{c}\): current camera image; \(t_{0}\): initialization threshold; \(t_{m}\): minimum threshold; \(N\): expected number of features; \(l\): Image pyramid levels; \(s\): scale of image pyramid; Output: Keypoints \(\mathbf{p}_{n}\);
1 Compute image pyramid \(\mathbf{I}_{l}\), the number of features expected for each layer of the image pyramid \(N_{l}\);
2 for\(l_{i}\in l\)do
3 repeat
4 Extract features \(\mathbf{p}_{l_{i}}\) in the current layer image \(\mathbf{I}_{l_{i}}\) using threshold \(t_{0}\);
5\(N_{l_{i}}\gets Size(\mathbf{p}_{l_{i}})\);
6 if\(N_{l_{i}}<N_{l}\)then\(t_{0}\gets t_{0}-2\) ;
7 elseif\(N_{l_{i}}>2N_{l}\)then\(t_{0}\gets t_{0}+2\) ;
8 until\((N_{l_{i}}>N_{l}\) or \(t_{0}<=t_{m})\);
9 Divide \(\mathbf{I}_{l_{i}}\) into \(m\times n\) cells \(\mathbf{I}_{l_{i}}^{mn}\), and calculate the desired features \(N_{l_{i}}^{c}\) in each cell;
10 Count the number of features \(\mathbf{p}_{l_{i}}^{mn}\) in each cell;
11 for\(\mathbf{I}_{l_{i}}^{ab}\in\mathbf{I}_{l_{i}}^{mn}\)do
12\(t_{i}^{ab}\gets t_{0}\);
13\(N_{l_{i}}^{ab}\gets Size(\mathbf{p}_{l_{i}}^{ab})\);
14 if\(N_{l_{i}}^{ab}<0.5N_{l_{i}}^{c}\)then
15 repeat
16\(t_{l_{i}}^{ab}\gets 0.9t_{l_{i}}^{ab}\);
17 Extract features \({}^{*}\mathbf{p}_{l_{i}}^{ab}\) in \(\mathbf{I}_{l_{i}}^{ab}\);
18\(N_{l_{i}}^{ab}\gets Size(\mathbf{p}_{l_{i}}^{ab})\);
19 until\((N_{l_{i}}^{ab}>0.8N_{l_{i}}^{c}\) or \(t_{l_{i}}^{ab}<=t_{m})\);
20 Replace \(\mathbf{p}_{l_{i}}^{ab}\) with \({}^{*}\mathbf{p}_{l_{i}}^{ab}\);
21 Select the best \(N_{l_{i}}^{c}\) features with quadtree in \(\mathbf{p}_{l_{i}}\);
22 Put the features of each layer \(\mathbf{p}_{l_{i}}\) into \(\mathbf{p}_{n}\). ```  
**Algorithm 1**Two step dynamic threshold feature extraction. In the first step, the initial threshold was used to extract the features from the whole image, and then, the threshold was modified according to the number of features extracted. If the number of features was lower than expected, then the texture of the image was not rich, so it was necessary to reduce the threshold and re-extract more features. When the threshold was reduced to a pre-set minimum value, the number of feature points to be extracted was insufficient, which indicated that this image was textureless and could only use the features already extracted. In our algorithm, the expected number of features was set to 1000, and the minimum threshold was set to seven. Feature points that were extracted by using an unrealistically small threshold were very unstable and had no application value. On the other hand, if the number of features was more than twice the expected value, then the image contained features in abundance, so we needed to extract stronger features as the object of tracking. However, because the appearance of the adjacent image was not very different, the system overhead could be reduced since the re-extraction of features of the current image was not necessary. Feature extraction of the subsequent image then used a modified threshold. Moreover, after completing the overall extraction on the current image, the image was divided into \(m\times n\) cells, and then, the number of features in each cell was counted. If the number of features in a cell was lower than expected, then the threshold was reduced to re-extract the current cell until the number of features reached the target number or the threshold was reduced to the minimum.