Revision history [back]

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <octomap/OcTree.h>
octomap::OcTree  pcdToOctree(const std::string& pcd_file, double resolution)
{
  // Read in the env PCD file as Point Cloud and convert to OcTree
  // FIRST: Load PCD as PCL PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  if (pcl::io::loadPCDFile<pcl::PointXYZ> (pcd_file, cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read input PCD file \n");
    return (-1);
  }

  // SECOND: Add points to OcTree
  octomap::OcTree final_octree(resolution);
  for (auto p:cloud.points)
  {
    final_octree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
  }
  final_octree.updateInnerOccupancy();
  return final_octree;
}

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <octomap/OcTree.h>
octomap::OcTree  pcdToOctree(const std::string& pcd_file, double resolution)
{
  // Read in the env PCD file as Point Cloud and convert to OcTree
  // FIRST: Load PCD as PCL PointCloud
  pcl::PointCloud<pcl::PointXYZ> cloud;
  if (pcl::io::loadPCDFile<pcl::PointXYZ> (pcd_file, cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read input PCD file \n");
    return (-1);
  }

  // SECOND: Add points to OcTree
  octomap::OcTree final_octree(resolution);
  for (auto p:cloud.points)
  {
    final_octree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
  }
  final_octree.updateInnerOccupancy();
  return final_octree;
}