Systems and methods for landmark generation for visual simultaneous localization and mapping
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. One embodiment further advantageously uses multiple particles to maintain multiple hypotheses with respect to localization and mapping. Further advantageously, one embodiment maintains the particles in a relatively computationally-efficient manner, thereby permitting the SLAM processes to be performed in software using relatively inexpensive microprocessor-based computer systems.
1. A method of creating a landmark for navigation, the method comprising:
acquiring a plurality of images with a visual sensor of a vehicle;
retrieving dead reckoning data corresponding to the retrieved plurality of images;
using the dead reckoning data to select at least 2 images from the plurality of images, wherein the selected images are taken spaced apart;
identifying visual features common to at least 2 of the selected images;
determining 3-dimensional coordinates of the identified visual features using the selected images;
generating a landmark comprising the one or more identified visual features and their corresponding 3-dimensional coordinates;
storing the generated landmark in a database comprising one or more existing landmarks; and
updating an estimate of the vehicle location and the locations of the existing landmarks and the generated landmark while navigating the same vehicle with the visual sensor based on the updated estimate of the vehicle location, said updating occurring when a landmark stored in the database is visually recognized;
wherein navigating and updating are performed under the control of electronic hardware.
2. The method as defined in claim 1 , wherein the vehicle comprises a robot and navigating based on the existing landmarks and the generated landmark further comprises using the landmark for robot navigation.
3. The method as defined in claim 1 , wherein the visual sensor comprises only a single camera.
4. The method as defined in claim 1 , wherein the dead reckoning data corresponds to data from at least one of an odometer or a pedometer.
5. The method as defined in claim 1 , wherein determining 3-dimensional coordinates further comprises simultaneously solving for the 3-D coordinates and relative poses of the camera or cameras for each image.
6. The method as defined in claim 1 , wherein the at least 2 images comprise at least 3 images spaced apart, and wherein determining 3-dimensional coordinates further comprises:
using distances between the at least 3 images computed from dead reckoning data;
using 2-dimensional image coordinates for selected features of the images; and
calculating the 3-dimensional coordinates by simultaneously calculating 3-dimensional coordinates and relative poses from the distances and the 2-dimensional image coordinates.
7. The method as defined in claim 6 , wherein calculating the 3-dimensional coordinates comprises using the trifocal tensor.
8. The method as defined in claim 1 , wherein the at least 2 images are spaced apart by at least a predetermined nonzero baseline that permits determining the 3-dimensional coordinates of the identified visual features.
9. The method as defined in claim 1 , wherein the identified visual features correspond to scale-invariant features (SIFT).
10. The method as defined in claim 1 , further comprising providing an indication of a new landmark to a mapping process.
11. The method as defined in claim 10 , further comprising relating the new landmark to a corresponding timestamp, and providing the timestamp with the indication of the new landmark as inputs to the mapping process.
12. A computer program embodied in a tangible non-transitory computer readable medium for creating a landmark for navigation, the computer program comprising:
a module with instructions configured to acquire a plurality of images with a visual sensor of a vehicle;
a module with instructions configured to retrieve dead reckoning data corresponding to the retrieved plurality of images;
a module with instructions configured to use the dead reckoning data to select at least 2 images from the plurality of images, wherein the selected images are spaced apart;
a module with instructions configured to identify visual features common to at least 2 of the selected images;
a module with instructions configured to determine 3-dimensional coordinates of the identified visual features using the selected images;
a module with instructions configured to generate a landmark comprising the one or more identified visual features and their corresponding 3-dimensional coordinates;
a module with instructions configured to store the generated landmark in a database comprising one or more existing landmarks; and
a module with instructions configured to update an estimate of the vehicle location and the locations of the existing landmarks and the generated landmark while navigating and to navigate the same vehicle with the visual sensor based on the updated estimate of the vehicle location, said update occurring when a landmark stored in the database is visually observed.
13. The computer program as defined in claim 12 , wherein the vehicle comprises a robot and the module with instructions configured to navigate based on the existing landmarks and the generated landmark further comprises instructions configured to use the landmark for robot navigation.
14. The computer program as defined in claim 12 , wherein the dead reckoning data corresponds to data from at least one of an odometer or a pedometer.
15. The computer program as defined in claim 12 , wherein the module with instructions configured to determine 3-dimensional coordinates further comprises instructions configured to simultaneously solve for the 3-D coordinates and relative poses of the camera or cameras for each image.
16. The computer program as defined in claim 12 , wherein the identified visual features correspond to scale-invariant features (SIFT).
17. A method of creating a landmark for navigation, the method comprising:
acquiring a plurality of images with a visual sensor of a vehicle;
selecting at least 2 images from a plurality of images, wherein the at least 2 images are spaced apart;
identifying visual features common to at least 2 of the selected images;
determining 3-dimensional coordinates of the identified visual features using the at least 2 images;
generating a landmark comprising the one or more identified visual features and their corresponding 3-dimensional coordinates;
storing the generated landmark in a database comprising one or more existing landmarks; and
updating an estimate of the vehicle location and the locations of the existing landmarks and the generated landmark while navigating the same vehicle with the visual sensor based on the updated estimate of the vehicle location, said updating occurring when a landmark stored in the database is visually recognized, wherein navigating is performed under the control of electronic hardware.
18. The method as defined in claim 17 , wherein the vehicle comprises a robot and navigating based on the existing landmarks and the generated landmark further comprises using the landmark for robot navigation.
19. The method as defined in claim 17 , further comprising selecting a reference frame corresponding to one of the at least two images.
20. The method as defined in claim 17 , wherein the identifiably stored information related to the identified visual features corresponds to at least one of 3-D coordinates and feature descriptors.
21. The method as defined in claim 17 , wherein the visual sensor comprises only a single camera.
22. The method as defined in claim 17 , wherein the visual sensor comprises a plurality of cameras that are spaced apart.
23. The method as defined in claim 17 , wherein determining 3-dimensional coordinates further comprises simultaneously solving for the 3-D coordinates and relative poses of the visual sensor for each of the selected images.
24. The method as defined in claim 17 , wherein the at least 2 images are spaced apart by at least a predetermined nonzero baseline that permits determining the 3-dimensional coordinates of the visual features.
25. The method as defined in claim 17 , wherein the identified visual features correspond to scale-invariant features (SIFT).
26. The method as defined in claim 17 , further comprising providing an indication of a new landmark to a mapping system.
27. A circuit for creating a landmark for navigation, the circuit comprising:
a circuit configured to acquire a plurality of images with a visual sensor coupled to a vehicle;
a circuit configured to select at least 2 images from a plurality of images, where wherein the at least 2 images are spaced apart;
a circuit configured to identify visual features common to at least 2 of the selected images;
a circuit configured to determine 3-dimensional coordinates of the identified visual features using the at least 2 images;
a circuit configured to generate a landmark comprising the one or more identified visual features and their corresponding 3-dimensional coordinates;
a circuit configured to store the generated landmark in a database comprising one or more existing landmarks; and
a circuit configured to update an estimate of the vehicle location and the locations of the existing landmarks and the generated landmark while navigating and to navigate the same vehicle with the visual sensor based on the updated estimate of the vehicle location, said update occurring when a landmark stored in the database is visually observed.
28. The circuit as defined in claim 27 , wherein the vehicle comprises a robot and at least a portion of the circuit is configured to navigate the robot.
29. The circuit as defined in claim 27 , further comprising a circuit configured to select a reference frame corresponding to one of the at least two images.
30. A computer program embodied in a tangible non-transitory computer readable medium for creating a landmark for navigation, the computer program comprising:
a module with instructions configured to acquire a plurality of images with a visual sensor coupled to a vehicle;
a module with instructions configured to select at least 2 images from a plurality of images, wherein the at least 2 images are spaced apart;
a module with instructions configured to identify visual features common to at least 2 of the selected images;
a module with instructions configured to determine 3-dimensional coordinates of the identified visual features using the at least 2 images;
a module with instructions configured to generate a landmark comprising the one or more identified visual features and their corresponding 3-dimensional coordinates;
a module with instructions configured to store the generated landmark in a database comprising one or more existing landmarks; and
a module with instructions configured to update an estimate of the vehicle location and the locations of the existing landmarks and the generated landmark while navigating and to navigate the same vehicle with the visual sensor based on the updated estimate of the vehicle location, said update occurring when a landmark stored in the database is visually observed.
31. The computer program as defined in claim 30 , wherein the vehicle comprises a robot and the module with instructions configured to navigate based on the existing landmarks and the generated landmark further comprises instructions configured to use the landmark for robot navigation.
32. The computer program as defined in claim 30 , further comprising a module with instructions configured to select a reference frame corresponding to one of the at least two images.