1. A method of obstacle mapping for navigation of an autonomous vehicle, the method comprising: providing an autonomous vehicle with an image capturing device; focusing the image capturing device at a predetermined number of different specified distances to capture an image at each of the specified distances; identifying which regions in each captured image are in focus; assigning a corresponding lens-focus distance to each of the regions that are in focus; forming a composite image based on each captured image, with each of the regions labeled with the corresponding lens-focus distance; and producing a three-dimensional obstacle map from the composite image, wherein the three-dimensional obstacle map has an x, y, z coordinate system, with x being proportional to pixel horizontal position, y being proportional to pixel vertical position, and z being the lens-focus distance.