A method for navigating at least one substantially discontinuous surface feature encountered by a transport device, the transport device traveling a path (413, 415) over a surface, the surface including the at least one substantially discontinuous surface feature, the path (413, 415) including a starting point and an ending point, the method comprising: accessing point cloud data representing the surface; filtering the point cloud data so as to form filtered point cloud data; forming (155, 157) the filtered point cloud data into processable parts (154, 757); merging the processable parts (154, 757) into at least one concave polygon (759, 759A, 711); locating and labeling the at least one substantially discontinuous surface feature in the at least one concave polygon (759, 759A, 711), the locating and labeling forming labeled point cloud data; creating graphing polygons based at least on the at least one concave polygon (759, 759A, 711), the graphing polygons forming a route topology, wherein creating the graphing polygons based at least on the at least one concave polygon (759, 759A, 711) comprises: smoothing exterior edges of the at least one concave polygon (759, 759A, 711) by expanding the at least one concave polygon (759, 759A, 771) to form an expanded polygon; contracting the expanded polygon to form a contracted polygon and a drivable margin based on the smoothed exterior edges; adding at least one substantially discontinuous surface feature trajectory to the contracted polygon; partitioning the contracted polygon into the graphing polygons, the graphing polygons being convex polygons; and removing interior edges formed during the partitioning process to form a simplified set of the graphing polygons according to at least one pre-selected criterion; transforming the point cloud data into a global coordinate system; determining boundaries of the at least one substantially discontinuous surface feature; creating substantially discontinuous surface feature buffers of a pre-selected size around the boundaries; determining which of the at least one substantially discontinuous surface feature can be traversed based at least on at least one substantially discontinuous surface feature traversal criterion; creating an edge/weight graph based at least on the at least one substantially discontinuous surface feature that can be traversed, the transformed point cloud data, and the route topology; and choosing the path (413, 415) from the starting point to the ending point based at least on the edge/weight graph. accessing point cloud data representing the surface; filtering the point cloud data so as to form filtered point cloud data; forming (155, 157) the filtered point cloud data into processable parts (154, 757); merging the processable parts (154, 757) into at least one concave polygon (759, 759A, 711); locating and labeling the at least one substantially discontinuous surface feature in the at least one concave polygon (759, 759A, 711), the locating and labeling forming labeled point cloud data; creating graphing polygons based at least on the at least one concave polygon (759, 759A, 711), the graphing polygons forming a route topology, wherein creating the graphing polygons based at least on the at least one concave polygon (759, 759A, 711) comprises: smoothing exterior edges of the at least one concave polygon (759, 759A, 711) by expanding the at least one concave polygon (759, 759A, 771) to form an expanded polygon; contracting the expanded polygon to form a contracted polygon and a drivable margin based on the smoothed exterior edges; adding at least one substantially discontinuous surface feature trajectory to the contracted polygon; partitioning the contracted polygon into the graphing polygons, the graphing polygons being convex polygons; and removing interior edges formed during the partitioning process to form a simplified set of the graphing polygons according to at least one pre-selected criterion; smoothing exterior edges of the at least one concave polygon (759, 759A, 711) by expanding the at least one concave polygon (759, 759A, 771) to form an expanded polygon; contracting the expanded polygon to form a contracted polygon and a drivable margin based on the smoothed exterior edges; adding at least one substantially discontinuous surface feature trajectory to the contracted polygon; partitioning the contracted polygon into the graphing polygons, the graphing polygons being convex polygons; and removing interior edges formed during the partitioning process to form a simplified set of the graphing polygons according to at least one pre-selected criterion; transforming the point cloud data into a global coordinate system; determining boundaries of the at least one substantially discontinuous surface feature; creating substantially discontinuous surface feature buffers of a pre-selected size around the boundaries; determining which of the at least one substantially discontinuous surface feature can be traversed based at least on at least one substantially discontinuous surface feature traversal criterion; creating an edge/weight graph based at least on the at least one substantially discontinuous surface feature that can be traversed, the transformed point cloud data, and the route topology; and choosing the path (413, 415) from the starting point to the ending point based at least on the edge/weight graph.