Fix the removed indices of UniformSampling

This ensures that all removed indices are set and not only those
of the indices that are visited.

filters/include/pcl/filters/impl/uniform_sampling.hpp

@@ -80,6 +80,7 @@ pcl::UniformSampling<PointT>::applyFilter (PointCloud &output)
   divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
 
   Filter<PointT>::removed_indices_->clear();
+  std::vector<bool> visited_indices (indices_->size (), false);
   // First pass: build a set of leaves with the point index closest to the leaf center
   for (size_t cp = 0; cp < indices_->size (); ++cp)
   {
@@ -90,10 +91,6 @@ pcl::UniformSampling<PointT>::applyFilter (PointCloud &output)
           !pcl_isfinite (input_->points[(*indices_)[cp]].y) || 
           !pcl_isfinite (input_->points[(*indices_)[cp]].z))
       {
-        if (Filter<PointT>::extract_removed_indices_)
-        {
-          Filter<PointT>::removed_indices_->push_back ((*indices_)[cp]);
-        }
         continue;
       }
     }
@@ -110,6 +107,7 @@ pcl::UniformSampling<PointT>::applyFilter (PointCloud &output)
     if (leaf.idx == -1)
     {
       leaf.idx = (*indices_)[cp];
+      visited_indices[leaf.idx] = true;
       continue;
     }
 
@@ -126,6 +124,19 @@ pcl::UniformSampling<PointT>::applyFilter (PointCloud &output)
       }
 
       leaf.idx = (*indices_)[cp];
+      visited_indices[leaf.idx] = true;
+    }
+  }
+
+  // All not visited indices should be removed
+  if (Filter<PointT>::extract_removed_indices_)
+  {
+    for (size_t cp = 0; cp < indices_->size (); ++cp)
+    {
+      if (!visited_indices[cp])
+      {
+        Filter<PointT>::removed_indices_->push_back (cp);
+      }
     }
   }