Add OpenMP Parallelization to FarthestPointSampling (#6287)

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

* made index mapping lambda take auto parameter

* Added parallel implementation of farthest_point_sampling

* Added `default(none)` to the omp parallel clause

* Fixed first set of review comments

* Fixed race condition and added separate reduction section

* Renamed nr_threads_ to num_threads_

* Switched num_threads_ == 0 to num_threads != 0

Author: alexnavtt

filters/include/pcl/filters/farthest_point_sampling.h

@@ -79,12 +79,36 @@ namespace pcl
           return (seed_);
         }
 
+        /** \brief Set the number of threads to use when operating in parallel 
+          * \param num_threads the number of threads to use
+          */
+        inline void
+        setNumberOfThreads (unsigned int num_threads)
+        {
+          #ifdef _OPENMP
+          num_threads_ = num_threads != 0 ? num_threads : omp_get_num_procs();
+          #else
+          if (num_threads_ != 1) 
+            PCL_WARN("OpenMP is not available. Keeping number of threads unchanged at 1\n");
+          #endif
+        }
+
+        /** \brief Get the value of the internal \a num_threads_ parameter.
+          */
+        inline unsigned int 
+        getNumberOfThreads () const
+        {
+          return num_threads_;
+        }
+
       protected:
 
         /** \brief Number of points that will be returned. */
         std::size_t sample_size_;
         /** \brief Random number seed. */
         unsigned int seed_;
+        /** \brief Number of threads */
+        unsigned int num_threads_{1};
 
         /** \brief Sample of point indices
           * \param indices indices of the filtered point cloud

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

@@ -51,23 +51,36 @@ pcl::FarthestPointSampling<PointT>::applyFilter (Indices &indices)
 
   for (std::size_t j = 1; j < sample_size_; ++j)
   {
-    index_t next_max_index = 0;
-    
     const PointT& max_index_point = (*input_)[toCloudIndex(max_index)];
-    //recompute distances
-    for (std::size_t i = 0; i < size; ++i)
+
+    #pragma omp parallel \
+    default(none) \
+    shared(distances_to_selected_points, max_index, max_index_point, size, toCloudIndex) \
+    num_threads(num_threads_)
     {
-      if (distances_to_selected_points[i] == -1.0)
-        continue;
-      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;
-    }
+      std::ptrdiff_t local_max_index = max_index;
+
+      #pragma omp for
+      for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t>(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_)[toCloudIndex(i)], max_index_point));
+        if (distances_to_selected_points[i] > distances_to_selected_points[local_max_index]) {
+            local_max_index = i;
+        }
+      }
+
+      #pragma omp critical
+      {
+        if (distances_to_selected_points[local_max_index] > distances_to_selected_points[max_index]) 
+          max_index = local_max_index;
+      }
+    } //pragma omp parallel
 
     //select farthest point based on previously calculated distances
     //since distance is set to -1 for all selected elements,previously selected 
     //elements are guaranteed to not be selected
-    max_index = next_max_index;
     distances_to_selected_points[max_index] = -1.0;
     indices.push_back(toCloudIndex(max_index));
     //set distance to -1 to ignore during max element search