A method of localization for a non-transitory computer readable storage medium storing one or more programs is disclosed. The one or more programs comprise instructions, which when executed by a computing device, cause the computing device to perform by one or more autonomous vehicle driving modules execution of processing of images from a camera and data from a LiDAR using the following steps comprising: computing, in response to features from a 3D submap and features from a global map, matching score between corresponding features of a same class between the 3D submap and the global map; selecting, for each feature in the 3D submap, a corresponding feature with the highest matching score from the global map; determining a feature correspondence to be invalid if a distance between corresponding features is larger than a threshold; and removing the invalid feature correspondence.