Add octomap and octovis 1.6.1