System and methods for feature selection and matching

A method for matching features, the method comprising:

• extracting a first plurality of features from current image data acquired from at least one sensor;
• extracting a second plurality of features from a prior map, wherein the prior map represents an environment containing the navigation system independently of data currently acquired by the at least one sensor;
• identifying at least one first feature in the first plurality of features and at least one second feature in the second plurality of features that have associated two-dimensional representations;
• and
• identifying at least one corresponding pair of features by comparing a three-dimensional representations of the at least one first feature to a three-dimensional representation of the at least one second feature.

A program product for identifying corresponding features, the program product comprising a plurality of instructions tangibly stored on a non-transitory storage medium, the instructions operable, when executed, to cause a processing unit to:

• extract a first plurality of features from current image data acquired from at least one sensor configured to acquire a three-dimensional representation of an environment containing the at least one sensor;
• extract a second plurality of features from a prior map of the environment, wherein the prior map represents the environment independently of data currently acquired by the at least one sensor;
• identify at least one first feature in the first plurality of features and at least one second feature in the second plurality of features that have associated two-dimensional representations;
• and
• identify at least one corresponding pair of features by comparing a three-dimensional histogram of the at least one first feature to a three-dimensional histogram of the at least one second feature.