Make FarthestPointFilter respect user supplied indices (#6286)

* changed behavior to index into user indices instead of total pointcloud

* made index mapping lambda take auto parameter

Author: alexnavtt

filters/include/pcl/filters/impl/farthest_point_sampling.hpp

@@ -20,7 +20,7 @@
 template<typename PointT> void
 pcl::FarthestPointSampling<PointT>::applyFilter (Indices &indices)
 {
-  const std::size_t size = input_->size();
+  const std::size_t size = indices_->size();
   //if requested number of point is equal to the point cloud size, copy original cloud
   if (sample_size_ == size)
   {
@@ -41,22 +41,25 @@ pcl::FarthestPointSampling<PointT>::applyFilter (Indices &indices)
   std::mt19937 random_gen(seed_);
   std::uniform_int_distribution<index_t> dis(0, size -1);
 
+  //lambda to map filter indices back to pointcloud indices for increased readability
+  auto toCloudIndex = [this](const auto idx){return (*indices_)[idx];};
+
   //pick the first point at random
   index_t max_index = dis(random_gen);
   distances_to_selected_points[max_index] = -1.0;
-  indices.push_back(max_index);
+  indices.push_back(toCloudIndex(max_index));
 
   for (std::size_t j = 1; j < sample_size_; ++j)
   {
     index_t next_max_index = 0;
 
-    const PointT& max_index_point = (*input_)[max_index];
+    const PointT& max_index_point = (*input_)[toCloudIndex(max_index)];
     //recompute distances
     for (std::size_t i = 0; i < size; ++i)
     {
       if (distances_to_selected_points[i] == -1.0)
         continue;
-      distances_to_selected_points[i] = std::min(distances_to_selected_points[i], geometry::distance((*input_)[i], max_index_point));
+      distances_to_selected_points[i] = std::min(distances_to_selected_points[i], geometry::distance((*input_)[toCloudIndex(i)], max_index_point));
       if (distances_to_selected_points[i] > distances_to_selected_points[next_max_index])
         next_max_index = i;
     }
@@ -66,7 +69,7 @@ pcl::FarthestPointSampling<PointT>::applyFilter (Indices &indices)
     //elements are guaranteed to not be selected
     max_index = next_max_index;
     distances_to_selected_points[max_index] = -1.0;
-    indices.push_back(max_index);
+    indices.push_back(toCloudIndex(max_index));
     //set distance to -1 to ignore during max element search
   }
 
@@ -75,7 +78,7 @@ pcl::FarthestPointSampling<PointT>::applyFilter (Indices &indices)
     for (std::size_t k = 0; k < distances_to_selected_points.size(); ++k)
     {
       if (distances_to_selected_points[k] != -1.0)
-        (*removed_indices_).push_back(k);
+        (*removed_indices_).push_back(toCloudIndex(k));
     }
   }
 }