As an improvement to the Octomap, instead of marking each voxel with the probability it has an obstacle, mark it with the most probable distance to the nearest surface. This is called a Signed Distance Field (SDF). This Provides a more precise representation of small objects.