Patent Document ID: 9342888
Application ID: 14458286
Patent Status: 1

Claim One:
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.