From 7680ea828b5752761f77cc42f4d4389c75de726a Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Fri, 6 Sep 2019 16:41:44 +0200 Subject: [PATCH] Improve uniform sampling tool --- tools/uniform_sampling.cpp | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/tools/uniform_sampling.cpp b/tools/uniform_sampling.cpp index bf33f7be42d..b35a73c8a70 100644 --- a/tools/uniform_sampling.cpp +++ b/tools/uniform_sampling.cpp @@ -93,20 +93,32 @@ compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output // Estimate TicToc tt; tt.tic (); - + print_highlight (stderr, "Computing "); - UniformSampling us; - us.setInputCloud (xyz); - us.setRadiusSearch (radius); - PointCloud output_; - us.filter (output_); + UniformSampling us(true); // extract removed indices + us.setInputCloud(xyz); + us.setRadiusSearch(radius); + PointCloud temp; + us.filter(temp); + + pcl::PointIndices removed_indices; + // These are "fake" indices generated by initCompute(), already sorted + auto input_indices = us.getIndices(); + // Removed indices, not guaranteed to be sorted + us.getRemovedIndices(removed_indices); + std::sort(removed_indices.indices.begin(), removed_indices.indices.end()); + // Compute retained indices as a set difference between all and removed + std::vector retained; + std::set_difference(input_indices->begin(), + input_indices->end(), + removed_indices.indices.begin(), + removed_indices.indices.end(), + std::inserter(retained, retained.begin())); + pcl::copyPointCloud(*input, retained, output); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); - print_value ("%d", output_.size()); print_info (" points]\n"); - - // Convert data back - toPCLPointCloud2 (output_, output); + print_value ("%d", retained.size()); print_info (" points]\n"); } void