diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index a84b2c27f71..387860d66fd 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -72,9 +72,13 @@ pcl::RadiusOutlierRemoval::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 nn_dists (indices_->size ()); + Indices nn_indices (mean_k); + std::vector 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 @@ -82,14 +86,17 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) // 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::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; + } } } diff --git a/filters/include/pcl/filters/radius_outlier_removal.h b/filters/include/pcl/filters/radius_outlier_removal.h index 23f1d48317d..bcbc66de57a 100644 --- a/filters/include/pcl/filters/radius_outlier_removal.h +++ b/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::input_; using PCLBase::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}; }; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index be9837475bb..fc623cf7404 100644 --- a/test/filters/test_filters.cpp +++ b/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 cloud_out_rgb; + // Remove outliers using a spherical density criterion on non-dense pointcloud + RadiusOutlierRemoval 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;