Systems and methods for 3D data based navigation using a watershed method are provided. In at least one embodiment, a method for segmenting three-dimensional frames of data comprises acquiring at least one frame from at least one sensor, wherein the at least one frame provides a three-dimensional description of an environment containing the at least one sensor; and identifying a surface in the at least one frame. The method further comprises computing at least one residual map for the at least one frame based on the orthogonal distance from data points on the surface to at least one polynomial surface fitted to the surface; and segmenting the at least one residual map by performing a watershed algorithm on the residual map.