Stereo Vision-Based Simultaneous Localization and Mapping with Ranging Aid
Young-Hee Lee, Technical University Munich, Germany; Chen Zhu, Technical University Munich, Germany; Gabriele Giorgi, German Aerospace Center, Germany; Christoph Guenther, Technical University Munich and German Aerospace Center, Germany
Date/Time: Tuesday, Apr. 24, 1:50 p.m.
Stereo visual odometry is a viable tool for exploring unknown and GNSS-denied environments by unmanned ground vehicles (rovers). Unlike monocular vision, it is possible to determine rover poses and build a map database without scale ambiguity. However, incremental errors are unavoidable since stereo visual odometry is a dead-reckoning process in which current state is propagated from previous poses.
Loop closing is a generally applied technique to mitigate accumulated estimation errors and achieve global estimation accuracy. In this technique, matches of the feature points observed on the current scene are continuously searched in a global map database (loop detection) and the states in the loop are refined whenever matches are detected (loop correction). By applying this method, accumulated estimation errors in visual odometry can be effectively compensated. However, it is challenging to accurately detect and correct loops without introducing significant computational complexity to the system. Furthermore, a rover has to revisit previously observed places to operate any loop closing technique. This is an intrinsic problem in loop closure which results in constraining the mission planning.
We propose a different approach to resolve the drift problem: a fusion of keyframe- and feature-based stereo visual odometry and ranging measurements. We refer to the state-of-art algorithm Oriented Fast and Rotated BRIEF-SLAM (ORB-SLAM) to implement basic processes in visual odometry; poses are incrementally recovered by tracking the feature observations on the current frame from a local map (Tracking), in parallel, the local map is refined in another processing thread (Local mapping). All local keyframes, map points, and visual constraints (feature observations) are not discarded but saved in a graphically formulated global map database, as well as the distance measurements (ranging constraints) between the keyframes and a static base station. At the end of the sequence, we formulate a least squares problem with a cost function given by the summation of re-projection and distance squared errors over all the constraints in the global graph with an appropriate relative weighting between vision and range. Furthermore, the Huber kernel is applied to robustify the cost function. The defined least squares problem is solved by the sparse Levenberg-Marquardt algorithm. Ranging measurements are independent of the previously determined states, thus enabling compensation for bias in visual odometry and a more accurate estimation of the rover's location.
We evaluate the proposed algorithm on a public real stereo vision dataset. Distance measurements are synthetically generated with the ground true positions provided by the dataset with superimposed Gaussian white noise. First, we compare the trajectory and the map point estimates of the proposed algorithm to the ones obtained with stereo visual odometry. Then, we compare the local accuracy and global consistency of the proposed algorithm, stereo visual odometry, and stereo vision-only SLAM with loop closures in terms of the Relative Pose Error (RPE) and Absolute Trajectory Error (ATE). The evaluation results show both qualitatively and quantitatively that the accumulating drifts inherent in the dead reckoning visual odometry are efficiently reduced by ranging fusion. Moreover, the performance of the proposed algorithm is comparable to the one of stereo vision-only SLAM with closing loops.