A system and process for laser scan alignment for vehicle trajectories is provided. A representation of the map surface allows multiple scans to be simultaneously aligned to each other. Laser scans are fit to the template surface, and the template surface is re-estimated based on the fit of the scans. The result is a pose trajectory estimate that drifts less than standard approaches, and is more robust to outliers (such as moving objects) in the laser's field of view.