diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt index c73a786c28a..bf353c70f90 100644 --- a/benchmarks/CMakeLists.txt +++ b/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" diff --git a/benchmarks/filters/radius_outlier_removal.cpp b/benchmarks/filters/radius_outlier_removal.cpp new file mode 100644 index 00000000000..66aa527d1a5 --- /dev/null +++ b/benchmarks/filters/radius_outlier_removal.cpp @@ -0,0 +1,84 @@ +#include +#include // for PCDReader + +#include + +static void +BM_RadiusOutlierRemoval(benchmark::State& state, const std::string& file) +{ + // Perform setup here + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PCDReader reader; + reader.read(file, *cloud); + + pcl::RadiusOutlierRemoval ror; + ror.setInputCloud(cloud); + ror.setRadiusSearch(0.02); + ror.setMinNeighborsInRadius(14); + + pcl::PointCloud::Ptr cloud_voxelized( + new pcl::PointCloud); + 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::Ptr cloud(new pcl::PointCloud); + pcl::PCDReader reader; + reader.read(file, *cloud); + + pcl::RadiusOutlierRemoval ror; + ror.setInputCloud(cloud); + ror.setRadiusSearch(0.02); + ror.setMinNeighborsInRadius(14); + ror.setNumberOfThreads(0); + + pcl::PointCloud::Ptr cloud_voxelized( + new pcl::PointCloud); + 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(); +} diff --git a/filters/include/pcl/filters/impl/filter_indices.hpp b/filters/include/pcl/filters/impl/filter_indices.hpp index d408ee8db12..de53d91c781 100644 --- a/filters/include/pcl/filters/impl/filter_indices.hpp +++ b/filters/include/pcl/filters/impl/filter_indices.hpp @@ -100,7 +100,6 @@ pcl::FilterIndices::applyFilter (PointCloud &output) } else { - output.is_dense = true; applyFilter (indices); pcl::copyPointCloud (*input_, indices, output); } diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index a84b2c27f71..039487c2d81 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -72,9 +72,17 @@ 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); + // 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 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 @@ -82,78 +90,87 @@ 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, nn_dists) \ + shared(nn_dists_max, mean_k, to_keep) \ + num_threads(num_threads_) + for (ptrdiff_t i = 0; i < static_cast(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(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(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); diff --git a/filters/include/pcl/filters/radius_outlier_removal.h b/filters/include/pcl/filters/radius_outlier_removal.h index b01d6619f8b..272d091b015 100644 --- a/filters/include/pcl/filters/radius_outlier_removal.h +++ b/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::input_; using PCLBase::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}; }; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index be9837475bb..85e9a05b406 100644 --- a/test/filters/test_filters.cpp +++ b/test/filters/test_filters.cpp @@ -1494,19 +1494,43 @@ TEST (RadiusOutlierRemoval, Filters) { // Test the PointCloud method PointCloud cloud_out; + PointCloud cloud_out_neg; // Remove outliers using a spherical density criterion RadiusOutlierRemoval 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 cloud_out_rgb; + PointCloud cloud_out_rgb_neg; + // 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); + + 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;