如何在PCL中使用Voxel Filter后的SACSegmentation？

Question

我想在Voxel滤镜后使用SACsegmentation，但是滤镜的输出是一个 pcl::PCLPointCloud2 而 seg.setInputCloud 算法的方法需要一个 pcl::PCLPointCloud2ConstPtr.我在其他话题中发现，可以使用下面的句子 pcl::fromPCLPointCloud2(*cloud_filtered, *filt_cloudPtr); 来进行转换，但它没有工作。

这是我要实现的功能。

sensor_msgs::PointCloud2 process_cloud(const sensor_msgs::PointCloud2ConstPtr& input)
{
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;

  pcl_conversions::toPCL(*input, *cloud);

  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloudPtr);
  sor.setLeafSize (0.1, 0.1, 0.1);
  sor.filter (cloud_filtered);

  pcl::PCLPointCloud2ConstPtr filt_cloudPtr(new pcl::PCLPointCloud2);

  pcl::fromPCLPointCloud2(*cloud_filtered, *filt_cloudPtr);

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PCLPointCloud2> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

  seg.setInputCloud (filt_cloudPtr);
  seg.segment (*inliers, *coefficients);
}

Answer 1

而不是 pcl::PCLPointCloud2，你为什么不使用 pcl::PointXYZRGB.

void cloudReceive(const sensor_msgs::PointCloud2ConstPtr& inputMsg){
  pcl::fromROSMsg(*inputMsg, *tempCloud);

  pcl::VoxelGrid<pcl::PointXYZRGB> vg;
  vg.setInputCloud(tempCloud);
  vg.setLeafSize(vgleaf, vgleaf, vgleaf);
  vg.filter(*tempCloud);


  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);

  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations(maxIter);
  seg.setDistanceThreshold(disThres);

  seg.setInputCloud(tempCloud);
  seg.segment(*inliers, *coefficients);

  if(inliers->indices.size() == 0){
    std::cout << "no planar found" << std::endl;
  }

  pcl::ExtractIndices<pcl::PointXYZRGB> ei;
  ei.setInputCloud(input);
  ei.setIndices(inliers);
  ei.setNegative(false);
  ei.filter(*BackCloud);
  ei.setNegative(true);
  ei.filter(*tempCloud);


  pcl::toROSMsg(*tempCloud, outputMsg);
  pclpub.publish(outputMsg);
  tempCloud->clear();
}