bool PointCloudFunctions::statisticalOutlierRemovalAndSave(const cv::SparseMat &vmt, string fileName, int meanK, double stdDevMulThreshold)
{
// "Our sparse outlier removal is based on the computation of the distribution of point to neighbors distances in the input dataset.
// For each point, we compute the mean distance from it to all its neighbors. By assuming that the resulted distribution is
// Gaussian with a mean and a standard deviation, all points whose mean distances are outside an interval defined by the global
// distances mean and standard deviation can be considered as outliers and trimmed from the dataset."
PointCloud<PointXYZI>::Ptr cloud = convertToPointCloud(vmt);
PointCloud<PointXYZI>::Ptr cloud_filtered (new PointCloud<PointXYZI>);
// Create the filtering object
StatisticalOutlierRemoval<PointXYZI> sor;
sor.setInputCloud (cloud);
sor.setMeanK (meanK);
sor.setStddevMulThresh (stdDevMulThreshold);
sor.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;
}
cv::SparseMat PointCloudFunctions::statisticalOutlierRemoval(const cv::SparseMat &vmt, int meanK, double stdDevMulThreshold)
{
PointCloud<PointXYZI>::Ptr cloud = convertToPointCloud(vmt);
PointCloud<PointXYZI>::Ptr cloud_filtered (new PointCloud<PointXYZI>);
// Create the filtering object
StatisticalOutlierRemoval<PointXYZI> sor;
sor.setInputCloud (cloud);
sor.setMeanK (meanK);
sor.setStddevMulThresh (stdDevMulThreshold);
sor.filter (*cloud_filtered);
cloud->clear();
cloud.reset();
return convertToSparseMat(cloud_filtered, vmt.dims(), vmt.size());
}
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);
}
int main(int argc, char** argv)
{
if(argc <= 1 || console::find_argument(argc, argv, "-h") >= 0) {
printUsage(argv[0]);
}
//read file
vector<string> paths;
if(console::find_argument(argc,argv,"--file")>= 0) {
vector<int> indices(pcl::console::parse_file_extension_argument(argc, argv, "pcd"));
if (pcl::console::find_argument(argc, argv, "--save") >= 0) {
indices.erase(indices.end()-1);
}
Utilities::getFiles(argv, indices, paths);
indices.clear();
indices = pcl::console::parse_file_extension_argument(argc, argv, "ply");
Utilities::getFiles(argv, indices, paths);
}
// or read a folder
if(console::find_argument(argc,argv,"--folder")>= 0) {
Utilities::getFiles(argv[pcl::console::find_argument(argc, argv, "--folder") + 1], paths);
}
vector<PCLPointCloud2> cloud_blob;
PointCloud<PointXYZ>::Ptr ptr_cloud (new PointCloud<PointXYZ>);
Utilities::read(paths, cloud_blob);
Utilities::convert2XYZ(cloud_blob, ptr_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
float media = 50, devest = 1.0, size;
string axis ("z");
string savePath;
if(console::find_argument(argc,argv,"--save")>= 0) {
savePath = argv[console::find_argument(argc,argv,"--save") + 1];
}
Timer timer;
Log* ptr_log;
Log log(savePath);
ptr_log = &log;
string configuration("Filter:\n");
/* Statistical Filter */
if(console::find_argument(argc,argv,"-s")>= 0) {
if(!isAlpha(argv[console::find_argument(argc,argv,"-s") + 1])) {
media = atof(argv[console::find_argument(argc,argv,"-s") + 1]);
devest = atof(argv[console::find_argument(argc,argv,"-s") + 2]);
}
StatisticalOutlierRemoval<PointXYZ> sor;
sor.setInputCloud (ptr_cloud);
sor.setMeanK (media);
sor.setStddevMulThresh (devest);
sor.filter (*cloud_filtered);
configuration += "Statistical Outlier Removal\n";
configuration += "media: "+ to_string(media) +"\n";
configuration += "Desvest: "+ to_string(devest) +"\n";
configuration += "total point after filer: "+ to_string(cloud_filtered->height * cloud_filtered->width) +"\n";
configuration += "Time to complete: "+ timer.report() +"\n";
cout << configuration << endl;
ptr_log->write(configuration);
}
timer.reset();
/* Voxel Filter */
if(console::find_argument(argc,argv, "-v") >= 0) {
if(!isAlpha(argv[console::find_argument(argc,argv,"-v") + 1])) {
size = atof(argv[console::find_argument(argc,argv,"-v") + 1]);
}
// Create the filtering object
VoxelGrid<PointXYZ> sor;
sor.setInputCloud (ptr_cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
configuration += "Voxel Grid\n";
configuration += "size of voxel: "+ to_string(size) +"\n";
configuration += "lief size: "+ to_string(0.01) +","+ to_string(0.01) +"," +to_string(0.01)+"\n";
configuration += "total point after filer: "+ to_string(cloud_filtered->height * cloud_filtered->width) +"\n";
configuration += "Time to complete: "+ timer.report() +"\n";
cout << configuration << endl;
ptr_log->write(configuration);
}
timer.reset();
/* PassThroug Filter */
if(console::find_argument(argc,argv, "-p") >= 0) {
axis = argv[console::find_argument(argc,argv,"-p") + 1];
// Create the filtering object
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (ptr_cloud);
pass.setFilterFieldName (axis);
pass.setFilterLimits (0.0, 1.0);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
configuration += "PassThroug\n";
configuration += "axis: "+ axis +"\n";
configuration += "range: "+ to_string(0.0) +","+ to_string(1.0) + "\n";
configuration += "total point after filer: "+ to_string(cloud_filtered->height * cloud_filtered->width) +"\n";
configuration += "Time to complete: "+ timer.report() +"\n";
cout << configuration << endl;
ptr_log->write(configuration);
}
/* Save */
if(console::find_argument(argc,argv,"--save")>= 0) {
int how_many_files = atoi(argv[console::find_argument(argc,argv,"--save") + 2]);
Utilities::writePCDFile(ptr_cloud, savePath, how_many_files );
}
ptr_log->close();
return 0;
}
