Add OpenMP to radius outlier removal

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

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

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

@@ -72,88 +72,116 @@ 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, 2 = remove due to inf/nan
+  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])
+          // if a neighbor is furhter away than max distance, remove the point
+          // if negative is true we keep the point
+          if (!negative_ && nn_dists_max < nn_dists[k-1])
           {
-            chk_neighbors = true;
+            to_keep[i] = 0;
+            continue;
           }
-        }
-        else
-        {
-          chk_neighbors = true;
-          if (nn_dists_max < nn_dists[k-1])
+
+          // if a neighbor is closer than max distance and negative is true, remove the point
+          // if negative is false, we keep the point
+          if (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
-      if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
+      // Points having too few neighbors are outliers
+      if (!negative_ && k <= min_pts_radius_)
+      {
+        to_keep[i] = 0;
+        continue;
+      }
+
+      if (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

@@ -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 != 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_;
@@ -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

@@ -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;