https://github.com/HKUST-Aerial-Robotics/SLIM/blob/a1e3fd7cce71f09c3152070443b2c2a787a8f756/modules/voxel.cpp#L135 It seems to miss the top layer voxel