Added OMP support to RadiusOutlierRemoval.

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

@@ -72,24 +72,31 @@ 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);
   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) \
+    firstprivate(nn_dists) \
+    num_threads(num_threads_)
+    for (ptrdiff_t i = 0; i < indices_->size(); i++)
     {
+      const index_t index = indices_->at(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
@@ -123,35 +130,61 @@ pcl::RadiusOutlierRemoval<PointT>::applyFilterIndices (Indices &indices)
       if (!chk_neighbors)
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = index;
+        {
+          #pragma omp critical
+          {
+            (*removed_indices_)[rii++] = index;
+          }
+        }
+
         continue;
       }
 
       // Otherwise it was a normal point for output (inlier)
-      indices[oii++] = index;
+      #pragma omp critical
+      {
+        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) \
+    firstprivate(nn_dists) \
+    num_threads(num_threads_)
+    for (ptrdiff_t i = 0; i < indices_->size(); i++)
     {
+      const index_t index = indices_->at(i);
+      if (!pcl::isXYFinite((*input_)[index]))
+        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
       if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
       {
         if (extract_removed_indices_)
-          (*removed_indices_)[rii++] = index;
+        {
+          #pragma omp critical
+          {
+            (*removed_indices_)[rii++] = index;
+          }
+        }
         continue;
       }
 
       // Otherwise it was a normal point for output (inlier)
-      indices[oii++] = index;
+      #pragma omp critical
+      {
+        indices[oii++] = index;
+      }
     }
   }
 

filters/include/pcl/filters/radius_outlier_removal.h

@@ -142,6 +142,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 ? nr_threads : omp_get_num_procs();
+#else
+        if (num_threads_ != 1) {
+          PCL_WARN("OpenMP is not available. Keeping number of threads unchanged at 1");
+        }
+        num_threads_ = 1;
+#endif
+      }
+
     protected:
       using PCLBase<PointT>::input_;
       using PCLBase<PointT>::indices_;
@@ -177,6 +195,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

@@ -1499,14 +1499,25 @@ TEST (RadiusOutlierRemoval, Filters)
   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);
+
+  PointCloud<PointXYZRGB> cloud_out_rgb;
+  // 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);
 
   // Test the pcl::PCLPointCloud2 method
   PCLPointCloud2 cloud_out2;