lots of bugfixes in pointcloud Octomap updater