System and method for mapping, localization and pose correction of a vehicle based on images
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;
querying a survey landmark database to identify a subset of surveyed landmarks relative to the current position of the vehicle, the survey landmark database comprising 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 including determining one or more two-dimensional transform estimates at one or more transform scales between the set of currently observable landmarks and the subset of surveyed landmarks in a discrete bounded space, wherein the one or more two-dimensional transform estimates are determined for each depth level N, wherein N is a maximum number of depth levels;
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, wherein identifying the best transform estimate includes determining a transform cost of the one or more two-dimensional transform estimates at each depth level N, generating a sorted listed of best transform estimates at each depth level N based on the transform cost of the one or more two-dimensional transform estimates at each depth level N, and identifying the best transform estimate by comparing each best transform estimate in the sorted list to an adjacent best transform estimate in the sorted list; and
correcting a pose of the vehicle based on the best transform estimate.
2. The computer-implemented method of claim 1 , 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.
3. The computer-implemented method of claim 2 , wherein determining one or more two-dimensional transform estimates comprises projecting the centroid of each landmark of the subset of surveyed landmarks and the centroid of each landmark of the set of currently observable landmarks into two-dimensions.
4. The computer-implemented method of claim 3 , wherein the best transform estimate minimizes corresponding centroid distances between the set of currently observable landmarks and the subset of surveyed landmarks.
5. The computer-implemented method of claim 1 , wherein the one or more transform scales progress from coarse transform scales to fine transform scales.
6. The computer-implemented method of claim 1 , wherein identifying the best transform estimate by comparing each best transform estimate in the sorted list to an adjacent best transform estimate in the sorted list includes determining one or more clusters of best transform estimates based on comparing each best transform estimate in the sorted list to the adjacent best transform estimate in the sorted list.
7. The computer-implemented method of claim 6 , including determining a best cluster transform estimate based on the best transform estimates of each of the one or more clusters and wherein correcting a pose of the vehicle is based on the best cluster transform estimate.
8. 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 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 communicatively coupled to the processor;
the localization module causes the processor to query a survey landmark database communicatively coupled to the processor to retrieve a subset of surveyed landmarks relative to the current position of the vehicle, the survey landmark database including a set of surveyed landmarks previously stored; and
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 that minimizes distances between the set of currently observable landmarks and the subset of surveyed landmarks, wherein identifying the best transform estimate includes the pose correction module causing the processor to generate a sorted list of best transform estimates based on a transform cost of each estimate and determine one or more clusters of the best transform estimates based on comparing each best transform estimate in the sorted list to an adjacent best transform estimate in the sorted list, and wherein the pose correction module causes the processor to correct a pose of the vehicle based on the best transform estimate.
9. The system of claim 8 , wherein the localization module determines 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.
10. The system of claim 9 , wherein the pose correction module identifies the best transform estimate as a transform estimate that minimizes corresponding centroid distances between the set of currently observable landmarks and the subset of surveyed landmarks.
11. The system of claim 8 , wherein the pose correction module determines a set of transform estimates between the set of currently observable landmarks and the subset of surveyed landmarks based on a two-dimensional scale transform at one or more transform scales, wherein the one or more transform scales progress from coarse transform scales to fine transform scales.
12. The system of claim 8 , wherein the pose correction module determines a best cluster transform estimate based on the best transform estimates of each of the one or more clusters and corrects a pose of the vehicle is based on the best cluster transform estimate.