SYSTEMS AND METHODS FOR ESTIMATING LANE ELEVATION FOR GENERATING 3D MAPS

System, methods, and other embodiments described herein relate to estimating lane elevation using headings for generating three-dimensional (3D) maps. In one embodiment, a method includes augmenting positions of detected lane boundaries with headings of a vehicle from sensor data. The method also includes adding the headings to vertices of two-dimensional (2D) lines using direction vectors between the vertices, wherein the 2D lines are directed towards the detected lane boundaries. The method also includes estimating elevations of the vertices including surrounding lane boundaries using a weighted average and vehicle poses from the sensor data, the surrounding lane boundaries having locations different than the positions and the weighted average factors directional differences between the vertices. The method also includes generating a 3D map of driving lanes having stacked roads identified using the elevations.