Octomap voxel occupancy probabilities getting stuck
I am using octomap to maintain an occupancy grid from depth camera point cloud measurements, and I've noticed that some nodes seem to get stuck in an occupied or free state, and never get updated by subsequent point cloud measurements. I'm calling insertPointCloud to integrate my sensor measurements, which should integrate free space along with a hit at the end of the ray:
_octree->insertPointCloud(pointCloud, octomap::point3d(_currentMAVROSPose.translation().x(), _currentMAVROSPose.translation().y(), _currentMAVROSPose.translation().z()));
Where I have provided the position of the sensor to ensure that the rays traced by octomap are correct.
I've made sure my parameters obey
probMiss < occupancy threshold < probHit
with the values 0.2, 0.5, and 0.8.
My understanding is that the log odds probability of occupancy should be incremented with every subsequent measurement so that cells that are marked as free or occupied in enough subsequent measurements will eventually cross the threshold. But no matter how many point cloud measurements I insert, in some cases the voxel occupancy probability at the inserted point just stays constant. With some debugging, I found that it is neither below or above the clamping range, yet it is not being updated. I do not have a BBX set for this octree, and there's no max range, so the point can't be out of range. Apparently the voxel containing the point may not be the endpoint of the ray due to discretization error, but no nearby voxels are updated either; it seems the rays are being ignored, leaving holes or dangling voxels in the map.
