Systems and method are provided for controlling an autonomous vehicle. In one embodiment, a method includes: receiving sensor data from a sensor of the vehicle; determining a three dimensional point cloud map segment from the sensor data; determining a vehicle pose associated with the three-dimensional point cloud map segment; determining a pose difference based on the vehicle pose, another vehicle pose, and a two-step process, wherein the two-step process includes computing a coarse-granularity pose difference, and computing a fine-granularity pose difference; aligning the three dimensional point cloud map segment with another three dimensional point cloud map segment associated with the other vehicle pose based on the pose difference; and controlling the vehicle based on the aligned three dimensional point cloud map segments.