profile_plane_extraction.cpp

Go to the documentation of this file.

// PCL:
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/file_io.h>
#include <pcl/filters/voxel_grid.h>

// stack includes:
#include "cob_3d_mapping_common/stop_watch.h"
#include "cob_3d_mapping_common/point_types.h"
#include "cob_3d_segmentation/plane_extraction.h"


typedef pcl::PointXYZRGB PointT;

class PerformanceTester
{
public:
  PerformanceTester()
    : p_vox(new pcl::PointCloud<PointT>)
  {
    voxel.setLeafSize(0.03,0.03,0.03);
    pe.setSaveToFile(false);
    pe.setClusterTolerance(0.06);
    pe.setMinPlaneSize(50);
    pe.setAlpha(0.2);
  }

  void
  computeForOneCloud(pcl::PointCloud<PointT>::ConstPtr cloud)
  {
    PrecisionStopWatch t;
    t.precisionStart();
    voxel.setInputCloud(cloud);
    voxel.filter(*p_vox);
    std::cout << t.precisionStop() << " | ";
    t.precisionStart();
    pe.extractPlanes(p_vox, v_cloud_hull, v_hull_polygons, v_coefficients_plane);
    std::cout << t.precisionStop() << std::endl;
  }

private:
  pcl::PointCloud<PointT>::Ptr p_vox;
  std::vector<pcl::PointCloud<PointT>, Eigen::aligned_allocator<pcl::PointCloud<PointT> > > v_cloud_hull;
  std::vector<std::vector<pcl::Vertices> > v_hull_polygons;
  std::vector<pcl::ModelCoefficients> v_coefficients_plane;

  pcl::VoxelGrid<PointT> voxel;
  PlaneExtraction pe;

};

int main (int argc, char** argv)
{
  if (argc != 2)
  {
    std::cout << "Please provide a folder containing multiple pcd files" << std::endl;
    return 0;
  }

  PerformanceTester pt;
  pcl::PCDReader r;
  std::vector<std::string> files;
  pcl::getAllPcdFilesInDirectory(argv[1], files);
  std::vector<pcl::PointCloud<PointT>::Ptr> clouds;
  for (std::vector<std::string>::iterator it = files.begin(); it != files.end(); ++it)
  {
    clouds.push_back(pcl::PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>));
    r.read( argv[1] + *it, *(clouds.back()) );
  }

  int i = 0;
  while (i < 20)
  {
    std::vector<pcl::PointCloud<PointT>::Ptr>::iterator p = clouds.begin();
    std::cout << "Run #"<< i << std::endl;
    while(p != clouds.end())
    {
      pt.computeForOneCloud(*p);
      ++p;
    }
    ++i;
  }

  return 0;
}