Finally fixed bug that was causing plateaus by only adding to the priority queue voxels with at least one labeled neighbor.