A method and system for obstacle mapping for navigation of an autonomous vehicle is disclosed. The method comprises providing an autonomous vehicle with an image capturing device, and focusing the image capturing device at a predetermined number of different specified distances to capture an image at each of the specified distances. The method further comprises identifying which regions in the captured images are in focus, and assigning a corresponding lens-focus distance to each of the regions that are in focus. A composite image is formed from the captured images, with each of the regions labeled with the corresponding lens-focus distance. A three-dimensional obstacle map is then produced from the composite image. 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.