Add OpenMP to radius outlier removal (#6182)

* Added OMP support to RadiusOutlierRemoval.
* Added radius_outlier_removal benchmark.

benchmarks/CMakeLists.txt

@@ -23,6 +23,11 @@ PCL_ADD_BENCHMARK(filters_voxel_grid FILES filters/voxel_grid.cpp
                   ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd"
                             "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd")
 
+PCL_ADD_BENCHMARK(filters_radius_outlier_removal FILES filters/radius_outlier_removal.cpp
+                  LINK_WITH pcl_io pcl_filters
+                  ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd"
+                            "${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd")
+
 PCL_ADD_BENCHMARK(search_radius_search FILES search/radius_search.cpp
                   LINK_WITH pcl_io pcl_search
                   ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd"

benchmarks/filters/radius_outlier_removal.cpp

@@ -0,0 +1,84 @@
+#include <pcl/filters/radius_outlier_removal.h>
+#include <pcl/io/pcd_io.h> // for PCDReader
+
+#include <benchmark/benchmark.h>
+
+static void
+BM_RadiusOutlierRemoval(benchmark::State& state, const std::string& file)
+{
+  // Perform setup here
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PCDReader reader;
+  reader.read(file, *cloud);
+
+  pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
+  ror.setInputCloud(cloud);
+  ror.setRadiusSearch(0.02);
+  ror.setMinNeighborsInRadius(14);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxelized(
+      new pcl::PointCloud<pcl::PointXYZ>);
+  for (auto _ : state) {
+    // This code gets timed
+    ror.filter(*cloud_voxelized);
+  }
+}
+
+static void
+BM_RadiusOutlierRemovalOpenMP(benchmark::State& state, const std::string& file)
+{
+  // Perform setup here
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::PCDReader reader;
+  reader.read(file, *cloud);
+
+  pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
+  ror.setInputCloud(cloud);
+  ror.setRadiusSearch(0.02);
+  ror.setMinNeighborsInRadius(14);
+  ror.setNumberOfThreads(0);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxelized(
+      new pcl::PointCloud<pcl::PointXYZ>);
+  for (auto _ : state) {
+    // This code gets timed
+    ror.filter(*cloud_voxelized);
+  }
+}
+
+int
+main(int argc, char** argv)
+{
+  constexpr int runs = 100;
+
+  if (argc < 3) {
+    std::cerr
+        << "No test files given. Please download `table_scene_mug_stereo_textured.pcd` "
+           "and `milk_cartoon_all_small_clorox.pcd`, and pass their paths to the test."
+        << std::endl;
+    return (-1);
+  }
+
+  benchmark::RegisterBenchmark(
+      "BM_RadiusOutlierRemoval_milk", &BM_RadiusOutlierRemoval, argv[2])
+      ->Unit(benchmark::kMillisecond)
+      ->Iterations(runs);
+
+  benchmark::RegisterBenchmark(
+      "BM_RadiusOutlierRemoval_mug", &BM_RadiusOutlierRemoval, argv[1])
+      ->Unit(benchmark::kMillisecond)
+      ->Iterations(runs);
+
+  benchmark::RegisterBenchmark(
+      "BM_RadiusOutlierRemovalOpenMP_milk", &BM_RadiusOutlierRemovalOpenMP, argv[2])
+      ->Unit(benchmark::kMillisecond)
+      ->Iterations(runs);
+
+  benchmark::RegisterBenchmark(
+      "BM_RadiusOutlierRemovalOpenMP_mug", &BM_RadiusOutlierRemovalOpenMP, argv[1])
+      ->Unit(benchmark::kMillisecond)
+      ->Iterations(runs);
+
+  benchmark::Initialize(&argc, argv);
+  benchmark::RunSpecifiedBenchmarks();
+}

filters/include/pcl/filters/impl/filter_indices.hpp

@@ -100,7 +100,6 @@ pcl::FilterIndices<PointT>::applyFilter (PointCloud &output)
   }
   else
   {
-    output.is_dense = true;
     applyFilter (indices);
     pcl::copyPointCloud (*input_, indices, output);
   }

filters/include/pcl/filters/impl/radius_outlier_removal.hpp

@@ -72,88 +72,105 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
     return;
   }
 
+  // Note: k includes the query point, so is always at least 1
+  const int mean_k = min_pts_radius_ + 1;
+  const double nn_dists_max = search_radius_ * search_radius_;
+
   // The arrays to be used
-  Indices nn_indices (indices_->size ());
-  std::vector<float> nn_dists (indices_->size ());
+  Indices nn_indices (mean_k);
+  std::vector<float> nn_dists(mean_k);
+  // Set to keep all points and in the filtering set those we don't want to keep, assuming
+  // we want to keep the majority of the points.
+  // 0 = remove, 1 = keep
+  std::vector<std::uint8_t> to_keep(indices_->size(), 1);
   indices.resize (indices_->size ());
   removed_indices_->resize (indices_->size ());
   int oii = 0, rii = 0;  // oii = output indices iterator, rii = removed indices iterator
 
   // If the data is dense => use nearest-k search
   if (input_->is_dense)
   {
-    // Note: k includes the query point, so is always at least 1
-    int mean_k = min_pts_radius_ + 1;
-    double nn_dists_max = search_radius_ * search_radius_;
-
-    for (const auto& index : (*indices_))
+    #pragma omp parallel for \
+    default(none) \
+    schedule(dynamic,64) \
+    firstprivate(nn_indices, nn_dists)                                                 \
+    shared(nn_dists_max, mean_k, to_keep) \
+    num_threads(num_threads_)
+    for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(indices_->size()); i++)
     {
+      const auto& index = (*indices_)[i];
       // Perform the nearest-k search
-      int k = searcher_->nearestKSearch (index, mean_k, nn_indices, nn_dists);
+      const int k = searcher_->nearestKSearch (index, mean_k, nn_indices, nn_dists);
 
       // Check the number of neighbors
       // Note: nn_dists is sorted, so check the last item
-      bool chk_neighbors = true;
       if (k == mean_k)
       {
-        if (negative_)
-        {
-          chk_neighbors = false;
-          if (nn_dists_max < nn_dists[k-1])
-          {
-            chk_neighbors = true;
-          }
-        }
-        else
-        {
-          chk_neighbors = true;
-          if (nn_dists_max < nn_dists[k-1])
+          // if negative_ is false and a neighbor is further away than max distance, remove the point
+          // or
+          // if negative is true and a neighbor is closer than max distance, remove the point
+          if ((!negative_ && nn_dists_max < nn_dists[k-1]) || (negative_ && nn_dists_max >= nn_dists[k - 1]))
           {
-            chk_neighbors = false;
+            to_keep[i] = 0;
+            continue;
           }
-        }
       }
       else
       {
-        chk_neighbors = negative_;
-      }
-
-      // Points having too few neighbors are outliers and are passed to removed indices
-      // Unless negative was set, then it's the opposite condition
-      if (!chk_neighbors)
-      {
-        if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = index;
-        continue;
+        if (!negative_)
+        {
+          // if too few neighbors, remove the point
+          to_keep[i] = 0;
+          continue;
+        }
       }
-
-      // Otherwise it was a normal point for output (inlier)
-      indices[oii++] = index;
     }
   }
   // NaN or Inf values could exist => use radius search
   else
   {
-    for (const auto& index : (*indices_))
+    #pragma omp parallel for \
+    default(none) \
+    schedule(dynamic, 64) \
+    firstprivate(nn_indices, nn_dists) \
+    shared(to_keep) \
+    num_threads(num_threads_)
+    for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(indices_->size()); i++)
     {
+      const auto& index = (*indices_)[i];
+      if (!pcl::isXYZFinite((*input_)[index]))
+      {
+        to_keep[i] = 0;
+        continue;
+      }
+
       // Perform the radius search
       // Note: k includes the query point, so is always at least 1
       // last parameter (max_nn) is the maximum number of neighbors returned. If enough neighbors are found so that point can not be an outlier, we stop searching.
-      int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists, min_pts_radius_ + 1);
+      const int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists, min_pts_radius_ + 1);
 
-      // Points having too few neighbors are outliers and are passed to removed indices
-      // Unless negative was set, then it's the opposite condition
+      // Points having too few neighbors are removed
+      // or if negative_ is true, then if it has too many neighbors
       if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
+      {
+        to_keep[i] = 0;
+        continue;
+      }
+    }
+  }
+
+    for (index_t i=0; i < static_cast<index_t>(to_keep.size()); i++)
+    {
+      if (to_keep[i] == 0)
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = index;
+          (*removed_indices_)[rii++] = (*indices_)[i];
         continue;
       }
 
       // Otherwise it was a normal point for output (inlier)
-      indices[oii++] = index;
+      indices[oii++] = (*indices_)[i];
     }
-  }
 
   // Resize the output arrays
   indices.resize (oii);

filters/include/pcl/filters/radius_outlier_removal.h

@@ -143,6 +143,24 @@ namespace pcl
         */
       inline void
       setSearchMethod (const SearcherPtr &searcher) { searcher_ = searcher; }
+
+      /** \brief Set the number of threads to use.
+       * \param nr_threads the number of hardware threads to use (0 sets the value back
+       * to automatic)
+       */
+      void
+      setNumberOfThreads(unsigned int nr_threads = 0)
+      {
+#ifdef _OPENMP
+        num_threads_ = nr_threads != 0 ? nr_threads : omp_get_num_procs();
+#else
+        if (num_threads_ != 1) {
+          PCL_WARN("OpenMP is not available. Keeping number of threads unchanged at 1\n");
+        }
+        num_threads_ = 1;
+#endif
+      }
+
     protected:
       using PCLBase<PointT>::input_;
       using PCLBase<PointT>::indices_;
@@ -178,6 +196,11 @@ namespace pcl
 
       /** \brief The minimum number of neighbors that a point needs to have in the given search radius to be considered an inlier. */
       int min_pts_radius_{1};
+
+      /**
+       * @brief Number of threads used during filtering
+       */
+      int num_threads_{1};
   };
 
   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

test/filters/test_filters.cpp

@@ -1494,19 +1494,43 @@ TEST (RadiusOutlierRemoval, Filters)
 {
   // Test the PointCloud<PointT> method
   PointCloud<PointXYZ> cloud_out;
+  PointCloud<PointXYZ> cloud_out_neg;
   // Remove outliers using a spherical density criterion
   RadiusOutlierRemoval<PointXYZ> outrem;
   outrem.setInputCloud (cloud);
   outrem.setRadiusSearch (0.02);
   outrem.setMinNeighborsInRadius (14);
+  outrem.setNumberOfThreads(4);
   outrem.filter (cloud_out);
 
   EXPECT_EQ (cloud_out.size (), 307);
   EXPECT_EQ (cloud_out.width, 307);
   EXPECT_TRUE (cloud_out.is_dense);
-  EXPECT_NEAR (cloud_out[cloud_out.size () - 1].x, -0.077893, 1e-4);
-  EXPECT_NEAR (cloud_out[cloud_out.size () - 1].y, 0.16039, 1e-4);
-  EXPECT_NEAR (cloud_out[cloud_out.size () - 1].z, -0.021299, 1e-4);
+
+  outrem.setNegative(true);
+  outrem.filter(cloud_out_neg);
+
+  EXPECT_EQ(cloud_out_neg.size(), 90);
+  EXPECT_TRUE(cloud_out_neg.is_dense);
+
+  PointCloud<PointXYZRGB> cloud_out_rgb;
+  PointCloud<PointXYZRGB> cloud_out_rgb_neg;
+  // Remove outliers using a spherical density criterion on non-dense pointcloud
+  RadiusOutlierRemoval<PointXYZRGB> outremNonDense;
+  outremNonDense.setInputCloud(cloud_organized);
+  outremNonDense.setRadiusSearch(0.02);
+  outremNonDense.setMinNeighborsInRadius(14);
+  outremNonDense.setNumberOfThreads(4);
+  outremNonDense.filter(cloud_out_rgb);
+
+  EXPECT_EQ(cloud_out_rgb.size(), 240801);
+  EXPECT_EQ(cloud_out_rgb.width, 240801);
+  //EXPECT_TRUE(cloud_out_rgb.is_dense);
+
+  outremNonDense.setNegative(true);
+  outremNonDense.filter(cloud_out_rgb_neg);
+  EXPECT_EQ(cloud_out_rgb_neg.size(), 606);
+  //EXPECT_TRUE(cloud_out_rgb_neg.is_dense);
 
   // Test the pcl::PCLPointCloud2 method
   PCLPointCloud2 cloud_out2;