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: constructing a 3D submap and a global map; extracting features from the 3D submap and the global map; matching features extracted from the 3D submap against features extracted from the global map; refining feature correspondence; and refining location of the 3D submap.