Added modified version of octomap for visualistion