Simultaneous Three-Dimensional Mapping And Geolocation Of Road Surface