Exemple #1
0
bool PointCloudFunctions::radiusOutlierRemovalAndSave(const cv::SparseMat &vmt, string fileName, double radius, int minNeighbors)
{
    //    "The user specifies a number of neighbors which every indice must have within a specified radius to remain in the PointCloud."

    PointCloud<PointXYZI>::Ptr cloud = convertToPointCloud(vmt);
    PointCloud<PointXYZI>::Ptr cloud_filtered (new PointCloud<PointXYZI>);

    // Create the filtering object
    RadiusOutlierRemoval <PointXYZI> ror;
    ror.setInputCloud(cloud);
    ror.setRadiusSearch (radius);
    ror.setMinNeighborsInRadius (minNeighbors);
    ror.setNegative (true);
    ror.filter (*cloud_filtered);

    cloud->clear();
    cloud.reset();

    bool resultRet = (pcl::io::savePCDFileASCII (fileName, *cloud_filtered) >= 0); //FIXME: what is the return value? it's not mentioned in http://docs.pointclouds.org/1.6.0/group__io.html#ga5e406a5854fa8ad026cad85158fef266

    cloud_filtered->clear();
    cloud_filtered.reset();

    return resultRet;
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         std::string method,
         int min_pts, double radius,
         int mean_k, double std_dev_mul, bool negative)
{

  PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
      xyz_cloud (new pcl::PointCloud<PointXYZ> ());
  fromROSMsg (*input, *xyz_cloud_pre);
  
  std::vector<int> index_vector;
  removeNaNFromPointCloud<PointXYZ> (*xyz_cloud_pre, *xyz_cloud, index_vector);

      
  TicToc tt;
  tt.tic ();
  PointCloud<PointXYZ>::Ptr xyz_cloud_filtered (new PointCloud<PointXYZ> ());
  if (method == "statistical")
  {
    StatisticalOutlierRemoval<PointXYZ> filter;
    filter.setInputCloud (xyz_cloud);
    filter.setMeanK (mean_k);
    filter.setStddevMulThresh (std_dev_mul);
    filter.setNegative (negative);
    PCL_INFO ("Computing filtered cloud with mean_k %d, std_dev_mul %f, inliers %d\n", filter.getMeanK (), filter.getStddevMulThresh (), filter.getNegative ());
    filter.filter (*xyz_cloud_filtered);
  }
  else if (method == "radius")
  {
    RadiusOutlierRemoval<PointXYZ> filter;
    filter.setInputCloud (xyz_cloud);
    filter.setRadiusSearch (radius);
    filter.setMinNeighborsInRadius (min_pts);
    PCL_INFO ("Computing filtered cloud with radius %f, min_pts %d\n", radius, min_pts);
    filter.filter (*xyz_cloud_filtered);
  }
  else
  {
    PCL_ERROR ("%s is not a valid filter name! Quitting!\n", method.c_str ());
    return;
  }
    
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_filtered->width * xyz_cloud_filtered->height); print_info (" points]\n");

  toROSMsg (*xyz_cloud_filtered, output);
}