Point cloud(PointCloud2) to OctoMap