Improve uniform sampling tool

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<PointXYZ> us;
-  us.setInputCloud (xyz);
-  us.setRadiusSearch (radius);
-  PointCloud<PointXYZ> output_;
-  us.filter (output_);
+  UniformSampling<PointXYZ> us(true); // extract removed indices
+  us.setInputCloud(xyz);
+  us.setRadiusSearch(radius);
+  PointCloud<PointXYZ> 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<int> 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