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);
}