Added PCL2Octomap stuff