An extension to the Inverse Perspective Mapping to handle non-flat roads