Fixed old problem with gaps in the topological maps. Topological maps are now generated correctly. Next it would be good to generate them incrementally by only processing new/small area of the occupancy grid instead of the whole occupancy grid.