Constraint-free topological mapping and path planning by maxima detection of the kernel spatial clearance density

Asserting the inherent topology of the environment perceived by a robot is a key prerequisite of high-level decision making. This is achieved through the construction of a concise representation of the environment that endows a robot with the ability to operate in a coarse-to-fine strategy. In this paper, we propose a novel topological segmentation method of generic metric maps operating concurrently as a path-planning algorithm. First, we apply a Gaussian Distance Transform on the map that weighs points belonging to free space according to the proximity of the surrounding free area in a noise resilient mode. We define a region as the set of all the points that locally converge to a common point of maximum space clearance and employ a weighed meanshift gradient ascent onto the kernel space clearance density in order to detect the maxima that characterize the regions. The spatial intra-connectivity of each cluster is ensured by allowing only for linearly unobstructed mean-shifts which in parallel serves as a path-planning algorithm by concatenating the consecutive mean-shift vectors of the convergence paths. Experiments on structured and unstructured environments demonstrate the effectiveness and potential of the proposed approach.