Algo fixes (vertex in previous path, check path with obstacles)
visu fixes, dont show vertices removed from path
compilation fixes, path to octomap includes