Feature matching and corresponding refinement and 3D submap position refinement system and method for centimeter precision localization using camera-based submap and LiDAR-based global map

1. A system for localization, the system comprising:

a memory;

one or more processing units; and

one or more programs stored in the memory, the one or more programs configured to cause the one or more processing units to perform at least:

extracting first features from a 3D submap, wherein the 3D submap is generated using images from a camera;

extracting second features from a global map, wherein the global map is generated from a data from a LiDAR; and

generating matching scores by comparing the first features to the second features, wherein each matching score represents a correspondence between one of the first features and one of the second features, and wherein each matching score comprises a distance between the one of the first features and the one of the second features.

2. The system according to claim 1 , further comprising:

constructing the 3D submap; and

constructing the global map.

3. The system according to claim 2 , wherein the constructing the 3D submap comprises:

obtaining the images from the camera; and

constructing the 3D submap based on the images using visual simultaneous localization and mapping (SLAM).

4. The system according to claim 2 , wherein the constructing the global map comprises:

obtaining the data from the LiDAR; and

constructing a city-scale 3D map based on the data from the LiDAR using LiDAR mapping.

5. The system according to claim 1 , wherein the extracting the first features from the 3D submap and the extracting the second features from the global map comprises:

extracting structured features and unstructured features from the 3D submap and the global map.

6. The system according to claim 5 , wherein the structured features comprise at least one of planes, straight lines, and curved lines, and the unstructured features comprise sparse 3D points.

7. The system according to claim 1 , wherein the extracting the first features from the 3D submap and the extracting the second features from global map comprises:

voxelizing the 3D submap and the global map into voxels; and

estimating a distribution of 3D points within the voxels using a probabilistic model.

8. The system according to claim 1 , wherein the extracting the first features from the 3D submap and the extracting the second features from the global map comprises:

classifying the extracted features into classes.

9. The system according to claim 1 , further comprising:

removing one or more correspondences between the first features and the second features for one or more matching scores that are larger than a threshold value.

10. The system according to claim 1 , wherein the global map is further generated from a vehicle pose information from an inertial navigation module, wherein the vehicle pose information includes vehicle position and orientation information.

11. The system according to claim 1 , further comprising:

determining, based on information from an inertial navigation module, a location for the 3D submap.

12. The system according to claim 1 , further comprising:

aligning the 3D submap with the global map.

13. A non-transitory computer-readable medium storing a program causing a computer to execute a process, the process comprising:

extracting first features from a 3D submap, wherein the 3D submap is generated using images from a camera;

extracting second features from a global map, wherein the global map is generated from the data from a LiDAR; and

generating matching scores from comparing the first features to the second features, wherein each matching score represents a correspondence between one of the first features and one of the second features, and wherein each matching score comprises a distance between the one of the first features and the one of the second features.

14. The non-transitory computer-readable medium of claim 13 , wherein the distance between corresponding features is determined by a trained classifier.

15. The non-transitory computer-readable medium of claim 13 further comprising:

refining a location of the 3D submap, wherein the refining the location of the 3D submap comprises performing an iterative estimation of the location of the 3D submap until a first distance between corresponding features reaches a predetermined value or less.

16. A non-transitory computer-readable medium storing a program causing a computer to execute a process, the process comprising:

computing, in response to features from a 3D submap and features from a global map, matching scores between corresponding features of a same class within the 3D submap and the global map;

determining a correspondence is invalid because a distance between the corresponding features is larger than a threshold;

removing the invalid correspondence; and

refining a location of the 3D submap, wherein the refining the location of the 3D submap comprises performing an iterative estimation of the location of the 3D submap until a first distance between corresponding features reaches a predetermined value.

17. The non-transitory computer-readable medium of claim 16 , comprising:

constructing the 3D submap based on images from a camera using visual simultaneous localization and mapping (SLAM); and

constructing the global map using LiDAR mapping.

18. The non-transitory computer-readable medium of claim 17 , further comprising:

extracting structured features and unstructured features from the 3D submap and the global map.

19. The non-transitory computer-readable medium of claim 18 , wherein the extracting the features from the 3D submap and the global map comprises:

voxelizing the 3D submap and the global map into voxels; and

estimating a distribution of 3D points within the voxels using a probabilistic model.

20. The non-transitory computer-readable medium of claim 18 , wherein the extracting the features from the 3D submap and the global map comprises:

classifying the extracted features into classes.