int Utils::combinePointCloud(const CloudPtr &cloud1, const CloudPtr &cloud2, const CloudPtr &combinedCloud, float leaf_size)
{
    CloudPtr cloudp(new Cloud());
    (*cloudp) = (*cloud1) + (*cloud2);
    if (leaf_size > 0)
        downSamplePointCloud(cloudp, combinedCloud, leaf_size);
    else
        (*combinedCloud) = (*cloudp);
    return 0;
}
int Utils::downSamplePointCloud(const CloudPtr &cloud_in, const CloudPtr &cloud_out, float leaf_size)
{
    CloudPtr cloudp(new Cloud());
    (*cloudp) = (*cloud_in);
    pcl::VoxelGrid<Point> sor;
    sor.setInputCloud(cloudp);
    sor.setLeafSize(leaf_size, leaf_size, leaf_size);
    sor.filter(*cloud_out);
    std::cout << (*cloudp).points.size() << "->" << (*cloud_out).points.size() << std::endl;
    return 0;
}
int Utils::combinePointCloud(const std::vector<CloudPtr> clouds, const CloudPtr &combinedCloud, float leaf_size)
{
    CloudPtr cloudp(new Cloud());
    for (auto i = clouds.begin(); i != clouds.end(); i++)
        (*cloudp) += (**i);
    if (leaf_size > 0)
        downSamplePointCloud(cloudp, combinedCloud, leaf_size);
    else
        (*combinedCloud) = (*cloudp);
    return 0;
}
Esempio n. 4
0
int main(int argc, char** argv)
{
  if (argc < 2)
    {
      PCL_ERROR("Usage: %s gt.pcd\n", argv[0]);
      exit(1);
    }

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudp (new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloudp) == -1)
    {
      PCL_ERROR("Failed to load file %s\n", argv[1]);
      exit(1);
    }
  else
    PCL_WARN("Loaded PCD %s\n", argv[1]);

  pcl::ModelCoefficients::Ptr coeff (new pcl::ModelCoefficients ());
  pcl::PointIndices::Ptr inlier = RansacMethod(cloudp, coeff);
  // RaycostMethod(cloudp, center);

  printf("%lf %lf %lf %lf\n",  coeff->values[0], coeff->values[1], coeff->values[2], coeff->values[3]);

#if 0
  // Separate and colorize inliers and outliers
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr clr_cloud = ColorizeCloud(cloudp, inlier);

  // Display the final result
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  viewer = SetupViewer(clr_cloud, coeff);

  while( !viewer->wasStopped())
    {
      viewer->spinOnce(100);
      boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }
#endif
  return 0;
}