From bc6c2b7ebebd5c74d4519bcd291cd4024f00419b Mon Sep 17 00:00:00 2001
From: Wannes Van Loock <wannes@intermodalics.eu>
Date: Thu, 12 Oct 2017 08:35:06 +0200
Subject: [PATCH] Fix the removed indices of UniformSampling

This ensures that all removed indices are set and not only those
of the indices that are visited.
---
 .../pcl/filters/impl/uniform_sampling.hpp     | 19 +++++++++++++++----
 1 file changed, 15 insertions(+), 4 deletions(-)

diff --git a/filters/include/pcl/filters/impl/uniform_sampling.hpp b/filters/include/pcl/filters/impl/uniform_sampling.hpp
index d8a5b219d03..557a1b2f693 100644
--- a/filters/include/pcl/filters/impl/uniform_sampling.hpp
+++ b/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);
+      }
     }
   }