SYSTEM AND METHOD FOR MAPPING, LOCALIZATION AND POSE CORRECTION

• US 20150228077A1
• Filed: 08/13/2014
• Published: 08/13/2015
• Est. Priority Date: 02/08/2014
• Status: Active Grant

• Current Assignee: Honda Motor Co., Ltd. (Honda Motor Company)
• Original Assignee: Honda Motor Co., Ltd. (Honda Motor Company)
• Inventors: Murarka, Aniket, Ranganathan, Ananth, Menashe, Jacob
• Granted Patent: US 9,342,888 B2
• Time in Patent Office: Days
• US Class Current: 1/1
• CPC Class Codes: G01S 5/163 Determination of attitude using inertial means G01C9/00; control of attitude G05D1/49
• CPC Class Codes: G06F 18/22 Matching criteria, e.g. proximity measures
• CPC Class Codes: G06F 18/23 Clustering techniques
• CPC Class Codes: G06T 2200/04 involving 3D image data
• CPC Class Codes: G06T 2207/10012 Stereo images
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 2207/30248 Vehicle exterior or interior
• CPC Class Codes: G06T 2207/30252 Vehicle exterior; Vicinity of vehicle
• CPC Class Codes: G06T 3/20 Linear translation of whole images or parts thereof, e.g. panning
• CPC Class Codes: G06T 5/80 Geometric correction
• CPC Class Codes: G06T 7/593 from stereo images
• CPC Class Codes: G06T 7/60 Analysis of geometric attributes
• CPC Class Codes: G06T 7/73 using feature-based methods
• CPC Class Codes: G06T 7/74 involving reference images or patches
• CPC Class Codes: G06T 7/97 Determining parameters from multiple pictures depth or shape recovery from multiple images G06T7/55; stereo camera calibration G06T7/85
• CPC Class Codes: G06V 20/10 Terrestrial scenes scenes under surveillance with static cameras G06V20/52; scenes perceived from the exterior of a vehicle G06V20/56; scenes perceived from the interior of a vehicle G06V20/59
• CPC Class Codes: G06V 20/56 exterior to a vehicle by using sensors mounted on the vehicle
• CPC Class Codes: G06V 30/2504 Coarse or fine approaches, e.g. resolution of ambiguities or multiscale approaches

Abstract

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.

Claims

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;

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.

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 determining one or more two-dimensional transform estimates includes 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 for a depth level N.

6. The computer-implemented method of claim 5, wherein the one or more transform scales progress from coarse transform scales to fine transform scales.

7. The computer-implemented method of claim 5, including generating a sorted list of best transform estimates at each depth level N based on a transform cost of the one or more two-dimensional transform estimates at each depth level N.

8. The computer-implemented method of claim 7, including determining one or more clusters of best transform estimates based on comparing each best transform estimate in the sorted list to an adjacent best transform estimate in the sorted list.

9. The computer-implemented method of claim 8, 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.

10. The computer implemented method of claim 1, comprising 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, determining one or more clusters based on a transform cost of each of the one or more two-dimensional transform estimates and identifying a best transform estimate from the one or more clusters.

11. A system for mapping, localization and pose correction, comprising:

a processor;

a visual odometry module that 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;

a localization module that causes the processor to query a survey landmark database communicatively coupled to the process 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

a pose correction module that 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 and correct a pose of the vehicle based on the best transform estimate.

12. The system of claim 11, 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.

13. The system of claim 12, 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.

14. The system of claim 11, 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.

15. The system of claim 14, wherein the pose correction module generates a sorted list of best transform estimates the set of transform estimates based on a transform cost of each estimate and determining one or more clusters of best transform estimates based on comparing each best transform estimate in the sorted list to an adjacent best transform estimate in the sorted list.

16. The system of claim 15, 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.

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 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;

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.