Fixing missing addData method in OctreePointCloudOccupancy