How do robots know where to go?
In this project, we use a feature-based monocular SLAM system called ORB-SLAM2 on the two provided datasets KITTI 07 and KITTI 10 [1], and evaluate its performance. ORB-SLAM2 is an open-source visual SLAM implementation released in 2017 and based on its 2015 monocular version.
The main goal of visual SLAM is to estimate the trajectory of the platform as it moves through an environment that we attempt to reconstruct. Assuming that the physical world is mostly rigid and fixed, illuminated and contrasted, we can use visual odometry techniques to estimate a camera’s trajectory from a set of images.
In ORB-SLAM2, this is done using the data from selected keyframes to ensure more accuracy than with filter based methods [2]. ORB features are used for matching and guarantee real-time performance. ORB-SLAM2 implements tracking, mapping, relocalization and loop closing. It maintains a map that grows when the surroundings of the camera are affected by a change – which may happen through motion [3].
Figure 1: Architecture of the 3 parallel components of the open-source monocular ORB-SLAM2 system described in [4], updated from [3]
The visual SLAM system we are using is thus made of three parallel threads. For tracking, local mapping and loop closing for monocular input (Fig.1). In the first thread (tracking), the camera becomes localized using the feature matching to the local map at every frame. In the local mapping component, the local map becomes optimised and local BA (Bundle Adjustment) becomes executed.
Finally, the loop closing thread fixes the scale drift accumulation by constructing the Essential Graph, a sparse version of the potentially very dense co-visibility graph and performing a pose graph optimization as proposed in [5]. A full BA thread becomes then triggered after loop closing and the map is updated. We describe the tracking component in the next section.
In the final part of the report, we evaluate the results of ORB-SLAM2 on different settings by comparing the estimated platform motion with the ground-truth trajectory on each run. Since we were provided with the ground-truth poses, we used the Absolute Trajectory Error (ATE) or Absolute Pose Error (APE) to evaluate the results. APE works by aligning the true trajectory and the estimated trajectory before computing the absolute difference between them [6]. This is a useful metric to assess the overall robustness of our system as opposed to focusing on a single point of the path which is less representative of the system’s reliability [7].
2 Tracking in ORB SLAM
ORB-SLAM2 can process each frame it receives through the tracking component and decide when to insert a new keyframe. It also also responsible for the estimation of the camera’s location at every step. This is because we have knowledge about a static world in which only the camera is assumed to be moving. We will describe each step one by one as they appear in Fig. 1 and in Mur-Artal et al’s paper. paper [3], that is:
- 1. Pre-processing and ORB extraction
- 2. Initial Pose Estimation with the previous frame
- 3. Initial Pose Estimation with Relocalization
- 4. Local map tracking
- 5. New keyframe decision
Note that the same detected ORB features are used in every component tasks of the system.
2.1 ORB extraction
The first step is to pre-process the input data and extract the salient features. For this, we use the rotation invariant and noise resistant ORB (Oriented FAST and Rotated BRIEF) binary descriptor [8]. As it name suggests, ORB is based on two state-of-the-art algorithms: the FAST (Features from Accelerated Segment Test) key-points detector [9] and the BRIEF (Binary Robust Independent Elementary Features) descriptor [10]. ORB features are fast to compute and robust. The general process idea is to identify which pixels are significantly different from the neighbouring pixels. If so, then they can be considered as corners. This is measured on the intensity of the pixels.
The frame is analysed on 8 different scale levels with a 1.2 scale factor and is divided into a grid. Using the FAST detector, a chosen number of corners are found across the frame. The system attempts to find at minimum of 5 corners in every defined cell of the grid to make sure the distribution of key-points is homogeneous [3]. However, it may be the case that a cell does not contain enough corners due to low contrast or uniform patterns.
In the monocular version of the general ORB-SLAM2, the key-points are defined as two coordinates xm = (uL, vL) on the image, where L refers to left image – in a monocular setting, this is irrelevant [4]. Once key features have been detected on each scaled version of each frame, we process them through the ORB descriptor to establish correspondences and find key-point matches between the keyframes.
2.2 Camera Calibration and the Pinhole Model
The pinhole camera model is used in order to define an intrinsic camera matrix which is based on the parameters determined by the camera the images were taken from.
For a given rotation matrix R which is defined as a 3 by 3 rotation matrix. This is as described by [12]. This set of equations can then be used to transform the set of 3D points to the set of 2D in image shape from world space and vise versa after transforming the equations.
2.3 Initial Pose Estimation
In the second step, we check whether tracking was successful in the previous frame and estimate the pose. This means that we are trying to predict two values given that the intrinsic parameters are known in monocular mode:
• The translation vector t : this corresponds to the position of the camera in the given keyframe.
• The rotation matrix R : this is a 3×3 matrix that defines the orientation of the camera. That is, where the camera is point to.
In other words, we attempt to estimate a rigid transformation that includes both the camera position and the rotation from a frame Kk compared to the previous frame Kk−1.
The information taken from the initialization will be used to calculate the pose between two frames and use triangulation to define an initial set of points. In Mur-Artal et al.’s paper, they define a method that does not depend on the model of the scene [4]. For that reason, after finding ORB features at the finest level and matching them on the reference frame, two geometrical models are computed in parallel: a homography for a planar scene and a fundamental matrix for a non-planar scene. The most appropriate model is then selected with the heuristic defined below:
RH =SH
SH + SF(1)
If RH is over 0.45, the homography is used, otherwise the fundamental matrix for parallax cases. The homography Hcr and fundamental matrix Fcr are calculated by solving the following equations with the normalized DLT and 8-points algorithm respectively:
xc = Hcrxr xTc Fcrxr = 0 (2)
A score is calculated for moth models at each iteration:
SM =X
ρMd2cr xic, xir, M + ρMd2rc xic, xir, M
i
(3)
With :
- ρM : Spearman’s score.
- ρMd2 = Γ − d2if d2 < TM 0 if d2 ≥ TM
- d2cr and d2rc : symmetric transfer errors between frames.
- TM : threshold for outlier rejection.
- Γ : gamma function for homography threshold.
After this process, the homography matrix and fundamental matrix with highest scores will be kept. 3
2.3.1 Previous Frame Motion Model
If the tracking is still consistent, we can optimize the camera pose with a type of Structure from Motion mechanism: montion-only bundle adjustement (BA). Like Least-Squares, the problem is non-linear and is an alternative to factor graph methods, so BA provides more reliable results by optimizing the concatenation of some sequential relation motion estimations over the keyframes. In this version of BA, a velocity model is used when it comes to outlier rejection and it is assumed to be constant.
The parameters estimates are computed by minimizing a cost function that represents the fitting errors in the image re-projection – that is, the “distance” (represented by a cost) to key 3D points in the vector X when re-projecting. This is shown in equation 4. In ORB-SLAM2, Σ is the covariance matrix for the key-point scale.
{R, t} = argmin R,t
X i∈X
ρ
xi − πRXi + t 2Σ (4)
π is the projection function (monocular) that relates the focal length and the point when preforming a re-projection.
where the parameters (fx, fy) (focal length), (cx, cy) (principal point) and b (baseline) have been computed during the calibration phase..
The ρ corresponds to the Huber cost function and is responsible for the application of a penalty for costlier estimations (less correct) [13]. The Huber function is defined below in equation 6.
(1
2a2for |a| ≤ δ
δ ·|a| − 12δ , otherwise(6)
2.3.2 Global Relocalization
ρδ(a) =
Sometimes, visual SLAM may lose track of the features due to a number of possible reasons such as outliers or the accidental inclusion of moving component despite the assumption of a stationary world [14].
In the situation where tracking fails, ORB-SLAM2 uses a relocalization mechanism in real time to recover. As mentioned in the paper, an crucial aspect of relocalization is its strong illumination and viewpoint invariance [3]. This is all implemented in the recognition module [4].
The first step when relocalizing is to compute the bag of words vector and select a subset of keyframes called ’candidates’ that will be used in the process. This is achieved with the ’DetectRelocalizationCandidates’ function in the KeyFrameDatabase.cc source file, where frames that have common words with the current frame are given a similarity score based on their visual word conversion (DBoW2 library). All frames with a score above 0.75*bestScore are returned.
Next, each of these keyframe will undergo ORB matching with the current map points. Once the correspondences are established, we can decide the next course of action: if enough matches were made (the code indicates 15 or more), then a Perspective-n-Points (shortened PnP) solver will be ran in alternation with some iterations of RANSAC. The goal is to find a reliable camera pose. This is decided by computing the number of inliers.
The PnP algorithm attempts to estimate a pose with a 3D-to-2D correspondence and a simple example is illustrated below in Fig. 3.
Finally, we run pose optimization and discard outliers.
Figure 3: PnP problem illustrated. Using the camera intrinsic parameters and the perspective projection model, the pose from is estimated on the 2D plane from real world 3D points. From OpenCV Documentation.
2.4 Track Local Map
At this stage we should have an estimation of the camera pose. As explained, each keyframe stores information about the pose, the camera intrinsic parameters and the extracted keypoints from the current frame. The next step is to track the local map by analysing the incoming keyframes and establishing map-to-keyframes correspondences. The reason for using a local map is to reduce the complexity of matching through the whole map.
In the paper, they describe two sets of keyframes K1 and K2 that contain the map points matched with the current frame and the neighbours of each keyframe K1[i] in the covisibility graph [3]. We use the keyframe from the first set K1 that contains the most common points with the current frame as a reference keyframe and name it Kref . To track the local map, we proceed with an exigent algorithm that iterates through each map point piin K1 and K2 as described [3]:
1. piis found in the current frame and projected. If it is located outside the bounds of the image, it is incorrect and discarded.
2. We calculate the angle between pi’s direction n and the camera view direction v. If the point lies over 60◦away from the camera view, we discard it.
3. Like the angle, we now calculate the distance d between pi and the camera. To avoid scale drift and scaling uncertainty, we remove it if it does not belong to the evaluated scale invariance region.
4. We compute the scale with the distance d over the the scale invariance region minimum bound dmin : d/dmin.
5. Finally, the ORB features in the current frame that have not found a match are compared to the descriptor of the map point pi to find an association.
After this process, we run camera pose optimization on the keypoints found in the frame.
2.5 New Keyframe Decision
Finally in the last step of the tracking thread, a decision is taken on whether the current frame is considered as a keyframe. A set of conditions have been stated to ensure the keyframe is relevant:
• Minimum time since latest global relocalization: over 20 frames.
• If the status of the local mapping is idle. Otherwise, the minimum time since latest keyframe insertion is considered: over 20 frames. In that case, we insert a keyframe despite the local mapping being busy. To prevent delays in the system, local BA is stopped and the new keyframe is added right away. The point of the keyframes is to reduce the set of frames for BA to run faster, and it is also unnecessary to have very similar images that might show very little different in the keypoints detection.
• Minimum number of points tracked in current frame: 50 points, we want to make sure that the frame is informative enough. It also allows us to keep a threshold for all keyframes and thus make the matching meaningful.
• Maximum number of points tracked compared to Kref : under 90%. This is referred to as a minimum visual change in the paper [4].
If all these conditions are true in the system, then the frame is added as a keyframe. The database of visual words can be queried fast and efficiently, which makes it easier to erase certain redundant keyframes (e.g. too much similarity) in a culling mechanism.
The keyframes will be used in the local mapping thread as a mean to reduce the computational power needed. Optimization algorithms will run on this set of keyframes rather than on ever single frame.
3 Evaluation
In this section, we evaluate the performance of ORB-SLAM2 using four different sets of parameters. We will start by running the algorithm once with its initial settings and then proceed to analyse its behaviour when some of the underlying algorithms are modified.
3.1 Off-the-shelf
Figure 4: Off-the-shelf results of an initial ORB-SLAM2 run with KITTI07
a) Evolution of the APE through time
b) Error mapped onto the trajectory
Figure 5: Off-the-shelf results of an initial ORB-SLAM2 run with KITTI10
a) Evolution of the APE through time
b) Error mapped onto the trajectory
The default parameters for KITTI07 were set so that the number of features was 2000 and the scale factor between levels in the scale pyramid was 1.2 with the levels being 8. An fps of 10 was used. We see on the two figures above (Fig. 4 and 5) that the initial setup performs well on the 2 given scenarios successfully mapping accurately to the path of the systems agent. It should be noted that the maps are the path that the vehicle or agent is going where the doted line is the ground truth and the solid line predicted. The raw graphs or error graphs show the value of different error metrics over time and we can see that the error can peek in certain periods but overall remains consistently low.
In the following sections, we will analyse how modifying certain of these default parameters may impact the efficiency and reliability of ORB-SLAM2. We will notably look at how the tracking process behaves when the number of ORB features is reduced, the detected outliers are not removed and finally when the loop closure is disabled.
3.2 Reduced ORB features
For this section, we select three different number of ORB features to evaluate. The lowest amount achievable is 1256. Under that, the system fails to initialise as it does not detect enough inlier points to proceed with the re-projections. We thus chose the following values for nFeature :
• 1256 ORB features, 1500 ORB features and 1700 ORB features
3.2.1 nFeature = 1265
Figure 6: APE error of an ORB-SLAM2 run with 1256 ORB features with KITTI07 a) Evolution of the APE through time
b) Error mapped onto the trajectory
Figure 7: APE error of an ORB-SLAM2 run with 1256 ORB features with KITTI10 a) Evolution of the APE through time
b) Error mapped onto the trajectory
3.2.2 nFeature = 1500
Figure 8: APE error of an ORB-SLAM2 run with 1500 ORB features with KITTI07 a) Evolution of the APE through time
b) Error mapped onto the trajectory
Figure 9: APE error of an ORB-SLAM2 run with 1500 ORB features with KITTI10 a) Evolution of the APE through time
b) Error mapped onto the trajectory
3.2.3 nFeature = 1700
(a) (b)
Figure 10: APE error of an ORB-SLAM2 run with 1700 ORB features with KITTI07 a) Evolution of the APE through time
b) Error mapped onto the trajectory
(a) (b)
Figure 11: APE error of an ORB-SLAM2 run with 1700 ORB features with KITTI10 a) Evolution of the APE through time
b) Error mapped onto the trajectory
The graphs (Figures 6, 7, 8, 9, 10 and 11) show that as the number of ORB features reduces, the tracking gets observably worse on the second task. The system failed to reach completion when using the lowest number of features (1256). In KITTI07, we also observed that the error mapped onto the trajectory is more considerable than with default parameters, and gets worse as the number of features reduces: it does not overlay properly, there are gaps between the estimated trajectory and the reference trajectory and the error is higher (more yellow) on many parts of the map. This is true for both datasets.
As for KITTI10 we see that for later tests in which the system does not fail, the mean error is lower and in-fact all error metrics are significantly lower. In the first run with 1256 features, the system fails to construct the first part of the map. The APE error is indeed incredibly high towards time t = 0 as it reaches 16. The estimated trajectory is also very imprecise and inaccurate on most of the path. We can therefore conclude that reducing the number of features used by the algorithm increases the error of the system. On the contrary, it still runs pretty well with 1700 ORB features compared to the original default run.
The reason for this behaviour can be explained by the lack of information given in the frames. This results in more dependence on reliable matching, but as we always expect a number of outliers to be discarded, there may be too little successful correspondences. Note that ORB-SLAM2 has several threshold checks (in tracking thread and optimizing thread) to verify that enough matches have been found. We can assume that this condition was not mean in the first run at 1256 features, hence the map is incomplete. Even if there are enough correspondences, they might not constitute good matches and thus influence the estimated pose negatively as fewer correspondences infers more weight on each match (that is, wrong matches have more power when there are less features).
We can conclude that a system with more features is more reliable. However, given that 1700 ORB fea tures produce a decent result, it also reduces the computation time and power needed. Under this value, the performance of ORB-SLAM2 was strongly decreased.
3.3 Removed outlier rejection
In the code the following changes were made in the pose estimation function:
• in Optimizer.cc line 387, the pFrame->mvbOutlier[idx] = true; is replaced by false; • The same change is also done in line 411.
In this section of the evaluation, we removed the outlier detection and rejection part of the source code. (a) (b)
Figure 12: APE error of an ORB-SLAM2 run with no outliers rejection with KITTI07 a) Evolution of the APE through time
b) Error mapped onto the trajectory
(a) (b)
Figure 13: APE error of an ORB-SLAM2 run with no outliers rejection with KITTI10 a) Evolution of the APE through time
b) Error mapped onto the trajectory
Outliers constitute false information and create noise in the tracking and reconstruction process. When features are incorrectly matched, the map points are not updated reliably despite the optimization processes. In ORB-SLAM2 and in many other visual SLAM systems, we keep iterate over the mapped points and discard the outliers one by one. Fig. 13 shows the effect of outliers in the VSLAM on KITTI10. The estimated path is not smooth compared to the reference as shown on (b). Moreover, the ORB-SLAM2 system loses tracks earlier, which results in a mismatch between the generated route and the actual route as well as an incomplete path. It is also the reason why the average APE errors remain low: the errors at the early runs are comparatively low.
Similarly, after removing the outliers detection and rejection in KITTI07 (Figure 12), the SLAM system loses track at the end of the route. The generated route deviates a lot more from the ground truth compared to the off-the-shelf version. For APE errors, the highest error is 28.690, which is higher than 7.071 (Figure 4), and the average error is a lot higher than off-the-shelf version. The error is especially big towards the extremities. Outliers are responsible for incorrect correspondences which explain the unreliable results. They induce tracking errors and the system gets lost as the post estimations are wrong and do not match from keyframe to keyframe.
3.4 Removed loop closure
In this section, we disable the loop closure algorithm by removing all references to loop closure in tracking.cc, system.cc and localMapping.cc.
(a) (b)
Figure 14: APE error of an ORB-SLAM2 run with no loop closure with KITTI07
a) Evolution of the APE through time
b) Error mapped onto the trajectory
In KITTI07, removing the loop closure thread causes the reconstruction to be inaccurate. We can see that the paths are not connected and does not form a continuous line like in the previous evaluations (Fig. 14 (b) ). This is because a SLAM system without loop closure is unable to detect when the camera comes back to an area that has already been mapped.
Key features detected at the road connection get duplicated rather than linked to where they were previously visited. Hence, the system cannot correct the accumulated drift error which is solved using a pose-graph optimization in ORB-SLAM2 [4]. This directly affects the robustness of the system. Without loop closure, errors will accumulate on the long term and produce a wrong, unreliable reconstruction of the world [15].
The APE error reflects this very well. It is at its highest towards the beginning of the path where it reaches a score above 45. The reason for this is possibly the missing ’4th thread’. In ORB-SLAM2, once the loop closure thread has been ran, it triggers a full BA thread that can appropriately update the map based on the new information it receives [4]. When this optimization is done, the update set of keyframes and points are merged into the previous ones, thus guaranteeing a reliable reconstruction of the world. In KITTI10, the BA optimization does not happen and so the error is not corrected.
We can also observe that the proportions of the map appeared to be more incorrect than on our previous runs: its dimension on the z axis stays goes from -100 to around 90 meters while the reference path goes from around -90 to 130 meters. A similar behaviour shows on the x axis. This is due to the fact that the paths evaluated on the map are not able to merge and get condensed. It is a very clear result of how the scale drift accumulation remains uncorrected.
(a) (b)
Figure 15: APE error of an ORB-SLAM2 run with no loop closure with KITTI10
a) Evolution of the APE through time
b) Error mapped onto the trajectory
The structure of the path in KITTI10 does not need the loop closure thread as the robot’s route does not link back to its starting point. Hence, the APE error is much lower as seen in Fig. 15 (a). The error is more important before the U turn and at the end of the route where it reaches its peak (24.726) as shown Fig. 15 (b), similarly to previously obtained results. The structure of the path is also the same and we can thus conclude that loop closure is not necessary on the KITTI10 dataset.
4 Conclusion
In this report, we have described the underlying architecture and algorithms from the tracking thread of the monocular ORB-SLAM2 system. After modifying the values of the parameters and running the system on two datasets (KITTI07 and KITTI10), we have analysed how these specific features influence the reliability of the results. Overall, we have shown that these optimization algorithms are responsible for the efficiency and reliability of ORB-SLAM2.
Starting backwards with the conclusion, we have found that loop closure allows us to form complex world reconstruction where the camera detects already found features and matches them accurately. However, this algorithm is unnecessary in shorter settings, for example when the robot travels on a straight line.
Detecting and rejecting outliers removes the noise and uncertainty of failed correspondences. When this algorithm becomes not enabled, we may face errors as the matching is not consistent. Moreover, the resulting map becomes incorrect as the pose becomes badly estimated.
In conclusion, reducing the number of ORB features influences the reliability of the result negatively. Lastly, the system becomes more dependent on the few correspondences it can make, and thus introduces uncertainty and error into the estimations. Moreover, it may fail to find any inliers at all.
References
[1] Andreas Geiger, Philip Lenz, and Raquel Urtasun. “Are we ready for Autonomous Driving? The KITTI Vision Benchmark Suite”. In: Conference on Computer Vision and Pattern Recognition (CVPR). 2012.
[2] Hauke Malte Strasdat, J. M. M. Montiel, and Andrew J. Davison. “Visual SLAM: Why filter?” In: Image Vis. Comput. 30 (2012), pp. 65–77.
[3] Raul Mur-Artal, J. Montiel, and Juan Tardos. “ORB-SLAM: a versatile and accurate monocular SLAM system”. In: IEEE Transactions on Robotics 31 (Oct. 2015), pp. 1147–1163. doi: 10 .1109/TRO .2015 .2463671.
[4] Raul Mur-Artal and Juan D. Tardós. “ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras”. In: CoRR abs/1610.06475 (2016). arXiv: 1610.06475. url: http://arxiv.org/abs/1610.06475.
[5] Hauke Strasdat, J. Montiel, and Andrew Davison. “Scale drift-aware large scale monocular SLAM”. In: vol. 2. June 2010. doi: 10.15607/RSS.2010.VI.010.
[6] Jürgen Sturm et al. “A benchmark for the evaluation of RGB-D SLAM systems”. In: 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. 2012, pp. 573–580. doi: 10.1109/IROS.2012 .6385773.
[7] Edwin Olson and Michael Kaess. “Evaluating the Performance of Map Optimization Algorithms”. In: RSS Workshop on Good Experimental Methodology in Robotics. June 2009.
[8] Ethan Rublee et al. “ORB: An efficient alternative to SIFT or SURF”. In: 2011 International Conference on Computer Vision. 2011, pp. 2564–2571. doi: 10.1109/ICCV.2011.6126544.
[9] Edward Rosten and Tom Drummond. “Machine Learning for High-Speed Corner Detection”. In: vol. 3951. July 2006. isbn: 978-3-540-33832-1. doi: 10.1007/11744023_34.
[10] Michael Calonder et al. “BRIEF: Binary Robust Independent Elementary Features”. In: vol. 6314. Sept. 2010, pp. 778–792. isbn: 978-3-642-15560-4. doi: 10.1007/978-3-642-15561-1_56.
[11] Chuan Luo et al. “Overview of Image Matching Based on ORB Algorithm”. In: Journal of Physics: Conference Series 1237 (June 2019), p. 032020. doi: 10.1088/1742-6596/1237/3/032020.
[12] Lourdes Agapito. Robotic Vision and Navigation Part III Visual SLAM. University Lecture. 2021-2022.
[13] Peter J. Huber. “Robust Estimation of a Location Parameter”. In: The Annals of Mathematical Statistics 35.1 (1964), pp. 73–101. doi: 10 .1214/aoms/1177703732. url: https://doi.org/10 .1214/aoms/ 1177703732.
[14] S. Wangsiripitak and D.W. Murray. “Avoiding moving outliers in visual SLAM by tracking moving ob jects”. In: June 2009, pp. 375–380. doi: 10.1109/ROBOT.2009.5152290.
[15] P. Newman and Kin Ho. “SLAM-Loop Closing with Visually Salient Features”. In: vol. 2005. May 2005, pp. 635–642. isbn: 0-7803-8914-X. doi: 10.1109/ROBOT.2005.1570189.
Written by James Harding, Yicheng Zhan, Jeanne Beyazian
How do robots know where to go?
