System and method for image based mapping, localization, and pose correction of a vehicle with landmark transform estimation
A system and method for mapping, localization and pose correction including, determining a current position of a vehicle along a travel route and a set of currently observable landmarks along the travel route relative to the current position, the set of currently observable landmarks extracted from one or more stereo images obtained from an imaging device, and querying a survey landmark database to identify a subset of surveyed landmarks relative to the current position of the vehicle. The method including determining one or more two-dimensional transform estimates between the set of currently observable landmarks and the subset of surveyed landmarks and identifying a best transform estimate from the one or more two-dimensional transform estimates that minimizes distances between the set of currently observable landmarks and the subset of surveyed landmarks. The method including correcting a pose of the vehicle based on the best transform estimate.
1. A computer-implemented method for mapping, localization, and pose correction, comprising:
determining a current position of a vehicle along a travel route and a set of currently observable landmarks along the travel route relative to the current position, the set of currently observable landmarks extracted from one or more stereo images obtained from an imaging device;
receiving a subset of surveyed landmarks from a survey landmark database based on a query identifying the current position, the query executed at the survey landmark database, the survey landmark database including a set of surveyed landmarks previously stored;
determining one or more two-dimensional transform estimates between the set of currently observable landmarks and the subset of surveyed landmarks for one or more depth levels;
identifying a best transform estimate from the one or more two-dimensional transform estimates, wherein the best transform estimate minimizes distances between the set of currently observable landmarks and the subset of surveyed landmarks; and
correcting a pose of the vehicle based on the best transform estimate.
2. The computer-implemented method of claim 1 , including determining a centroid of each landmark of the set of currently observable landmarks.
3. The computer-implemented method of claim 2 , wherein the subset of surveyed landmarks stores a centroid for each landmark in the subset of surveyed landmarks.
4. The computer-implemented method of claim 3 , wherein identifying the best transform estimate includes determining from the one or more two-dimensional transform estimates corresponding centroid distances between the set of currently observable landmarks and the subset of surveyed landmarks, and determining the best transform estimate as a two-dimensional transform estimate of the one or more two-dimensional transform estimates having the smallest centroid distances between the set of currently observable landmarks and the subset of surveyed landmarks.
5. The computer-implemented method of claim 3 , wherein determining one or more two-dimensional transform estimates between the set of currently observable landmarks and the subset of surveyed landmarks includes determining one or more two-dimensional transform estimates at one or more transform scales for one or more depth levels.
6. The computer-implemented method of claim 1 , including determining a transform cost of each of the one or more two-dimensional transform estimates, wherein the transform cost is a difference between corresponding centroids of the set of currently observable landmarks and the subset of surveyed landmarks.
7. The computer-implemented method of claim 6 , determining a best transform estimate is based on the transform cost of each of the one or more two-dimensional transform estimates.
8. The computer-implemented method of claim 1 , wherein identifying a best transform estimate from the one or more two-dimensional transform estimates includes identifying a best transform estimate from the one or more two-dimensional transform estimates for each of the one or more depth levels, wherein the one or more two-dimensional transform estimates are determined at one or more transform scales for the one or more depth levels.
9. The computer-implemented method of claim 8 , including determining one or more clusters of best transform estimates based on a difference between each best transform estimate for each of the one or more depth levels.
10. A system for mapping, localization and pose correction, comprising:
a processor; and
a memory operatively connected for computer communication to the processor, the memory storing a visual odometry module, a localization module, and a pose correction module with instructions for execution by the processor, wherein:
the visual odometry module causes the processor to determine a current position of a vehicle along a travel route, wherein the visual odometry module causes the processor to determine a set of currently observable landmarks along the travel route relative to the current position, wherein the set of currently observable landmarks are extracted from one or more stereo images received from an imaging device;
the localization module causes the processor to execute a query including the current position at a survey landmark database, the survey landmark database including a set of surveyed landmarks previously stored, wherein the localization module receives a subset of surveyed landmarks from the survey landmark database as a result of the query;
the pose correction module causes the processor to determine one or more two-dimensional transform estimates between the set of currently observable landmarks and the subset of surveyed landmarks, identify a best transform estimate from the one or more two-dimensional transform estimates, wherein the best transform estimate is one of the one or more two-dimensional transform that minimizes distances between the set of currently observable landmarks and the subset of surveyed landmarks, and correct a pose of the vehicle based on the best transform estimate.
11. The system of claim 10 , wherein the localization module causes the processor to determine a centroid of each landmark of the set of currently observable landmarks.
12. The system of claim 11 , wherein the pose correction module causes the processor to identify the best transform estimate as a two-dimensional transform estimate of the one or more two-dimensional transform estimates that minimizes corresponding distances between the centroid of each landmark of the set of currently observable landmarks and a centroid of each landmark of the subset of surveyed landmarks.
13. The system of claim 10 , wherein the visual odometry module causes the processor to determine the one or more two-dimensional transform estimates at one or more transform scales for one or more depth levels.
14. The system of claim 13 , wherein the pose correction module causes the processor to identify the best transform estimate based on a comparison between a best transform estimate at each of the one or more depth levels.
15. The system of claim 10 , wherein the pose correction module causes the processor to determine a transform cost for each of the two-dimensional transform estimates.
16. The system of claim 15 , wherein the pose correction module causes the processor to identify the best transform estimate as one of the two-dimensional transform estimates having a lowest transform cost.
17. A non-transitory computer readable medium comprising instructions that when executed by a processor perform a method for mapping, localization and pose correction, comprising:
determining a current position of a vehicle along a travel route and a set of currently observable landmarks along the travel route relative to the current position, the set of currently observable landmarks extracted from one or more stereo images obtained from an imaging device;
receiving a subset of surveyed landmarks from a survey landmark database based on a query identifying the current position, the query executed at the survey landmark database, the survey landmark database includes a set of surveyed landmarks previously stored;
determining one or more two-dimensional transform estimates between the set of currently observable landmarks and the subset of surveyed landmarks;
identifying a best transform estimate from the one or more two-dimensional transform estimates that minimizes distances between the set of currently observable landmarks and the subset of surveyed landmarks; and
correcting a pose of the vehicle based on the best transform estimate.
18. The non-transitory computer readable medium of claim 17 , comprising determining a centroid of each landmark of the set of currently observable landmarks and wherein the subset of surveyed landmarks comprises a centroid for each landmark in the subset of surveyed landmarks.
19. The non-transitory computer readable medium of claim 18 , wherein the best transform estimate minimizes corresponding centroid distances between the set of currently observable landmarks and the subset of surveyed landmarks.
20. The non-transitory computer readable medium of claim 17 , wherein determining one or more two-dimensional transform estimates between the set of currently observable landmarks and the subset of surveyed landmarks comprises determining one or more two-dimensional transform estimates at one or more transform scales for one or more depth levels and determining a best transform estimate based on a transform cost of each of the one or more transform estimates.