Systems and methods for VSLAM optimization
The invention is related to methods and apparatus that use a visual sensor and dead reckoning sensors to process Simultaneous Localization and Mapping (SLAM). These techniques can be used in robot navigation. Advantageously, such visual techniques can be used to autonomously generate and update a map. Unlike with laser rangefinders, the visual techniques are economically practical in a wide range of applications and can be used in relatively dynamic environments, such as environments in which people move. Certain embodiments contemplate improvements to the front-end processing in a SLAM-based system. Particularly, certain of these embodiments contemplate a novel landmark matching process. Certain of these embodiments also contemplate a novel landmark creation process. Certain embodiments contemplate improvements to the back-end processing in a SLAM-based system. Particularly, certain of these embodiments contemplate algorithms for modifying the SLAM graph in real-time to achieve a more efficient structure.
1. A method for navigating a mobile system, the method implemented on one or more computer systems, comprising:
matching landmarks for a first query image associated with a mobile device by:
retrieving features from a global database for the first query image, wherein the global database comprises a plurality of landmarks and each landmark corresponds to a collection of 3-D features and corresponding 2-D features from which the 3-D features are computed;
ranking landmarks of the plurality of landmarks by visual similarity to features of the first query image;
selecting, from the global database, candidate landmarks that potentially match the features of the first query image;
for each of the candidate landmarks selected from the global database:
retrieving features in a local database for each of the candidate landmarks selected from the global database for the first query image, wherein the local database comprises a collection of 3-D features and corresponding 2-D features from which the 3-D features are computed for a specific landmark from the plurality of landmarks in the global database;
performing robust pose estimation;
performing bundle adjustment;
determining an observation pose and covariance; and
selecting one of the candidate landmarks as a matching landmark for the first query image; and
responsive to selecting the matching landmark for the first query image, retrieving features from the global database for a next query image, selecting next candidate landmarks therefrom, and retrieving features in a local database for the next query image.
2. The method of claim 1 , wherein the mobile system comprises a camera system that provides a stereoscopic view from which 3-D features of a landmark can be identified.
3. The method of claim 1 , the method further comprising the steps of:
estimating course and distance traveled from a prior pose; and
using change in pose information to update one or more poses and maps maintained by a SLAM module within the mobile device.
4. The method of claim 3 , wherein estimating course and distance traveled from a prior pose further comprises computing a relative pose of the mobile device with respect to one or more identified landmarks.
5. The method of claim 4 , wherein computing a relative pose of the mobile device with respect to the one or more identified landmarks further comprises finding a relative pose of the mobile device that minimizes projection error from 3-D features onto 2-D coordinates of a visible feature of the one or more identified landmarks.
6. The method of claim 1 , wherein the method further comprises the steps of:
observing a new physical landmark; and
determining displacements from the mobile device to features of the new physical landmark.
7. The method of claim 6 , wherein determining displacements from the mobile device to the features of the new physical landmark further comprises using a current position of a mobile device reference frame as an initial estimate of a landmark reference frame.
8. The method of claim 6 , wherein the method further comprises the step of storing a set of 3-D features and corresponding 2-D features that visually identify the new physical landmark in the global database.
9. The method of claim 8 , wherein the 2-D features are selected from the group consisting of SIFT features, SURF features, and BRIEF features.
10. The method of claim 8 , wherein storing the set of 3-D features and corresponding 2-D features that visually identify the new physical landmark in the global database further comprises:
retrieving a group of at least two images for analysis, where the at least two images are selected based upon a baseline between the at least two images;
analyzing the group of at least two images to identify 2-D features that are common to the at least two images in the group;
determining that there are enough features detected to reliably identify the new physical landmark; and
computing 3-D local reference frame positions to the common 2-D features based upon observed disparity and the baseline between the at least two images.
11. The method of claim 10 , wherein retrieving the group of at least two images for analysis, where the at least two images are selected based upon the baseline between the at least two images comprises determining the baseline between the at least two images using dead reckoning.
12. The method of claim 10 , wherein determining that there are enough features detected to reliably identify the new physical landmark comprises detecting 10 or more features.
13. The method of claim 10 , wherein computing the 3-D local reference frame positions to the common 2-D features based upon observed disparity and the baseline between the at least two images further comprises solving a structure and motion problem using a trifocal tensor method.
14. The method of claim 10 , wherein storing the set of 3-D features and corresponding 2-D features that visually identify the new physical landmark in the global database further comprises identifying each feature of the new physical landmark within the global database using a Landmark ID and a Feature ID.