A system and method including receiving a digital representation of a road that includes one or more lane representations; defining a factor graph including variable nodes and constraint nodes that encode lane boundary constraints for the lane map; translating the factor graph into a nonlinear optimization problem for altitudes of the lane map; generating a solution to the optimization problem; and generating a corrected lane map with an optimized altitude for the road based on the generated solution.