From bcc9ed933959ea2ae69da4a426e608e1bb23d081 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Tue, 26 Nov 2024 08:46:22 +0100 Subject: [PATCH 1/9] Set out.is_dense after copying the points, since else out.is_dense is overwritten by the value of the input cloud. --- filters/include/pcl/filters/impl/filter_indices.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/filters/include/pcl/filters/impl/filter_indices.hpp b/filters/include/pcl/filters/impl/filter_indices.hpp index d408ee8db12..ed3b965205e 100644 --- a/filters/include/pcl/filters/impl/filter_indices.hpp +++ b/filters/include/pcl/filters/impl/filter_indices.hpp @@ -100,9 +100,9 @@ pcl::FilterIndices::applyFilter (PointCloud &output) } else { - output.is_dense = true; applyFilter (indices); pcl::copyPointCloud (*input_, indices, output); + output.is_dense = true; } } From 0b04d31f5f2a9c1e39fa31d724c95e69d32afc74 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Tue, 26 Nov 2024 08:46:49 +0100 Subject: [PATCH 2/9] Added OMP support to RadiusOutlierRemoval. --- .../filters/impl/radius_outlier_removal.hpp | 61 ++++++++++++++----- .../pcl/filters/radius_outlier_removal.h | 23 +++++++ test/filters/test_filters.cpp | 17 +++++- 3 files changed, 84 insertions(+), 17 deletions(-) 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; From c93bae9ead56dfebfe50ddaac4f8da6cfd002edd Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Tue, 26 Nov 2024 10:49:10 +0100 Subject: [PATCH 3/9] Fix missing sharing attributes. --- .../pcl/filters/impl/radius_outlier_removal.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index 387860d66fd..9b221959888 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -89,10 +89,10 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) #pragma omp parallel for \ default(none) \ schedule(dynamic,64) \ - firstprivate(nn_indices) \ - firstprivate(nn_dists) \ + firstprivate(nn_indices, nn_dists) \ + shared(nn_dists_max, mean_k, indices, oii, rii) \ num_threads(num_threads_) - for (ptrdiff_t i = 0; i < indices_->size(); i++) + for (ptrdiff_t i = 0; i < static_cast(indices_->size()); i++) { const index_t index = indices_->at(i); // Perform the nearest-k search @@ -153,10 +153,10 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) #pragma omp parallel for \ default(none) \ schedule(dynamic, 64) \ - firstprivate(nn_indices) \ - firstprivate(nn_dists) \ + firstprivate(nn_indices, nn_dists) \ + shared(nn_dists_max, mean_k, indices, oii, rii) \ num_threads(num_threads_) - for (ptrdiff_t i = 0; i < indices_->size(); i++) + for (ptrdiff_t i = 0; i < static_cast(indices_->size()); i++) { const index_t index = indices_->at(i); if (!pcl::isXYFinite((*input_)[index])) From fc3af6ae879a9c2871bcd5ca81155bf397e6e266 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Tue, 26 Nov 2024 14:18:27 +0100 Subject: [PATCH 4/9] Remove the need for #pragma critical, by saving if a point should be removed in an array. Do a second loop, to assign indexes to indices and removed_indices_. --- .../filters/impl/radius_outlier_removal.hpp | 48 ++++++++----------- 1 file changed, 21 insertions(+), 27 deletions(-) diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index 9b221959888..00a4e7a53de 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -79,6 +79,7 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) // The arrays to be used Indices nn_indices (mean_k); std::vector nn_dists(mean_k); + std::vector to_keep(indices_->size()); indices.resize (indices_->size ()); removed_indices_->resize (indices_->size ()); int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator @@ -90,11 +91,11 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) default(none) \ schedule(dynamic,64) \ firstprivate(nn_indices, nn_dists) \ - shared(nn_dists_max, mean_k, indices, oii, rii) \ + shared(nn_dists_max, mean_k, indices, to_keep) \ num_threads(num_threads_) for (ptrdiff_t i = 0; i < static_cast(indices_->size()); i++) { - const index_t index = indices_->at(i); + const index_t& index = (*indices_)[i]; // Perform the nearest-k search const int k = searcher_->nearestKSearch (index, mean_k, nn_indices, nn_dists); @@ -129,22 +130,12 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) // Unless negative was set, then it's the opposite condition if (!chk_neighbors) { - if (extract_removed_indices_) - { - #pragma omp critical - { - (*removed_indices_)[rii++] = index; - } - } - + to_keep[index] = 0; continue; } // Otherwise it was a normal point for output (inlier) - #pragma omp critical - { - indices[oii++] = index; - } + to_keep[index] = 1; } } // NaN or Inf values could exist => use radius search @@ -154,11 +145,11 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) default(none) \ schedule(dynamic, 64) \ firstprivate(nn_indices, nn_dists) \ - shared(nn_dists_max, mean_k, indices, oii, rii) \ + shared(nn_dists_max, mean_k, indices, to_keep) \ num_threads(num_threads_) for (ptrdiff_t i = 0; i < static_cast(indices_->size()); i++) { - const index_t index = indices_->at(i); + const index_t& index = (*indices_)[i]; if (!pcl::isXYFinite((*input_)[index])) continue; // Perform the radius search @@ -170,21 +161,24 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &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_) - { - #pragma omp critical - { - (*removed_indices_)[rii++] = index; - } - } + to_keep[index] = 0; continue; } // Otherwise it was a normal point for output (inlier) - #pragma omp critical - { - indices[oii++] = index; - } + to_keep[index] = 1; + } + } + + for (index_t i=0; i < static_cast(to_keep.size()); i++) + { + if (to_keep[i] == 1) + { + indices[oii++] = i; + } + else + { + (*removed_indices_)[rii++] = i; } } From 28efda62753ac66c237f7193ec0ea3e39ef9b271 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Sat, 30 Nov 2024 22:48:41 +0100 Subject: [PATCH 5/9] Preset to keep all points in radius outlier removal. Set those to remove to 0 in the to_keep array. Fix indexing and minor things. --- .../filters/impl/radius_outlier_removal.hpp | 74 +++++++++---------- .../pcl/filters/radius_outlier_removal.h | 4 +- test/filters/test_filters.cpp | 13 ++++ 3 files changed, 52 insertions(+), 39 deletions(-) diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index 00a4e7a53de..51d1c661202 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -79,7 +79,9 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) // The arrays to be used Indices nn_indices (mean_k); std::vector nn_dists(mean_k); - std::vector to_keep(indices_->size()); + // 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. + 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 @@ -91,11 +93,11 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) default(none) \ schedule(dynamic,64) \ firstprivate(nn_indices, nn_dists) \ - shared(nn_dists_max, mean_k, indices, to_keep) \ + shared(nn_dists_max, mean_k, to_keep) \ num_threads(num_threads_) for (ptrdiff_t i = 0; i < static_cast(indices_->size()); i++) { - const index_t& index = (*indices_)[i]; + const auto& index = (*indices_)[i]; // Perform the nearest-k search const int k = searcher_->nearestKSearch (index, mean_k, nn_indices, nn_dists); @@ -104,38 +106,22 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) 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]) { chk_neighbors = false; } - } } else { - chk_neighbors = negative_; + chk_neighbors = false; } - // 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 outliers if (!chk_neighbors) { - to_keep[index] = 0; + to_keep[i] = 0; continue; } - - // Otherwise it was a normal point for output (inlier) - to_keep[index] = 1; } } // NaN or Inf values could exist => use radius search @@ -145,40 +131,54 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) default(none) \ schedule(dynamic, 64) \ firstprivate(nn_indices, nn_dists) \ - shared(nn_dists_max, mean_k, indices, to_keep) \ + shared(to_keep) \ num_threads(num_threads_) for (ptrdiff_t i = 0; i < static_cast(indices_->size()); i++) { - const index_t& index = (*indices_)[i]; + const auto& index = (*indices_)[i]; if (!pcl::isXYFinite((*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. 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 (k <= min_pts_radius_) { - to_keep[index] = 0; + to_keep[i] = 0; continue; } - - // Otherwise it was a normal point for output (inlier) - to_keep[index] = 1; } } - for (index_t i=0; i < static_cast(to_keep.size()); i++) + if (!negative_) { - if (to_keep[i] == 1) + for (index_t i=0; i < static_cast(to_keep.size()); i++) { - indices[oii++] = i; + if (to_keep[i] == 1) + { + indices[oii++] = (*indices_)[i]; + } + else + { + (*removed_indices_)[rii++] = (*indices_)[i]; + } } - else - { - (*removed_indices_)[rii++] = i; + } + else + { + for (index_t i = 0; i < static_cast(to_keep.size()); i++) { + if (to_keep[i] == 0) { + indices[oii++] = (*indices_)[i]; + } + else { + (*removed_indices_)[rii++] = (*indices_)[i]; + } } } diff --git a/filters/include/pcl/filters/radius_outlier_removal.h b/filters/include/pcl/filters/radius_outlier_removal.h index bcbc66de57a..2f82d1600ce 100644 --- a/filters/include/pcl/filters/radius_outlier_removal.h +++ b/filters/include/pcl/filters/radius_outlier_removal.h @@ -151,10 +151,10 @@ namespace pcl setNumberOfThreads(unsigned int nr_threads = 0) { #ifdef _OPENMP - num_threads_ = nr_threads ? nr_threads : omp_get_num_procs(); + 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"); + PCL_WARN("OpenMP is not available. Keeping number of threads unchanged at 1\n"); } num_threads_ = 1; #endif diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index fc623cf7404..e45395d293f 100644 --- a/test/filters/test_filters.cpp +++ b/test/filters/test_filters.cpp @@ -1494,6 +1494,7 @@ 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); @@ -1506,7 +1507,14 @@ TEST (RadiusOutlierRemoval, Filters) EXPECT_EQ (cloud_out.width, 307); EXPECT_TRUE (cloud_out.is_dense); + 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); @@ -1519,6 +1527,11 @@ TEST (RadiusOutlierRemoval, Filters) 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(), 66399); + //EXPECT_FALSE(cloud_out_rgb_neg.is_dense); + // Test the pcl::PCLPointCloud2 method PCLPointCloud2 cloud_out2; RadiusOutlierRemoval outrem2; From 62779a525895a1d7cf3e0795e8cff0c885704ecc Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Mon, 2 Dec 2024 07:45:25 +0100 Subject: [PATCH 6/9] Fix filtering to work correctly with negative_ and extract_removed_indices_. --- .../filters/impl/radius_outlier_removal.hpp | 40 ++++++++++--------- test/filters/test_filters.cpp | 2 +- 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index 51d1c661202..87162005082 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -81,6 +81,7 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) 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, 2 = remove due to inf/nan std::vector to_keep(indices_->size(), 1); indices.resize (indices_->size ()); removed_indices_->resize (indices_->size ()); @@ -136,9 +137,9 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) for (ptrdiff_t i = 0; i < static_cast(indices_->size()); i++) { const auto& index = (*indices_)[i]; - if (!pcl::isXYFinite((*input_)[index])) + if (!pcl::isXYZFinite((*input_)[index])) { - to_keep[i] = 0; + to_keep[i] = 2; continue; } @@ -156,31 +157,32 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) } } - if (!negative_) - { for (index_t i=0; i < static_cast(to_keep.size()); i++) { - if (to_keep[i] == 1) + if (to_keep[i] == 2) { - indices[oii++] = (*indices_)[i]; + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[i]; + continue; } - else + + if (!negative_ && to_keep[i] == 0) { - (*removed_indices_)[rii++] = (*indices_)[i]; - } - } - } - else - { - for (index_t i = 0; i < static_cast(to_keep.size()); i++) { - if (to_keep[i] == 0) { - indices[oii++] = (*indices_)[i]; + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[i]; + continue; } - else { - (*removed_indices_)[rii++] = (*indices_)[i]; + + if (negative_ && to_keep[i] != 0) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[i]; + continue; } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = (*indices_)[i]; } - } // Resize the output arrays indices.resize (oii); diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index e45395d293f..ca4046a9d42 100644 --- a/test/filters/test_filters.cpp +++ b/test/filters/test_filters.cpp @@ -1529,7 +1529,7 @@ TEST (RadiusOutlierRemoval, Filters) outremNonDense.setNegative(true); outremNonDense.filter(cloud_out_rgb_neg); - EXPECT_EQ(cloud_out_rgb_neg.size(), 66399); + EXPECT_EQ(cloud_out_rgb_neg.size(), 606); //EXPECT_FALSE(cloud_out_rgb_neg.is_dense); // Test the pcl::PCLPointCloud2 method From 23f7300c2a1aea7e56feecd0c4c9fa64dc730d3b Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Tue, 3 Dec 2024 07:14:44 +0100 Subject: [PATCH 7/9] Make the negative test in the paralleled for loop. Simplify the second pass. --- .../filters/impl/radius_outlier_removal.hpp | 55 +++++++++---------- test/filters/test_filters.cpp | 2 +- 2 files changed, 28 insertions(+), 29 deletions(-) diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index 87162005082..cc50c8ea4b3 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -104,24 +104,32 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) // Check the number of neighbors // Note: nn_dists is sorted, so check the last item - bool chk_neighbors = true; if (k == mean_k) { - 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 = false; + to_keep[i] = 0; + continue; + } + + // 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]) + { + to_keep[i] = 0; + continue; } } else { - chk_neighbors = false; - } - - // Points having too few neighbors are outliers - if (!chk_neighbors) - { - to_keep[i] = 0; - continue; + if (!negative_) + { + // if too few neighbors, remove the point + to_keep[i] = 0; + continue; + } } } } @@ -139,7 +147,7 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) const auto& index = (*indices_)[i]; if (!pcl::isXYZFinite((*input_)[index])) { - to_keep[i] = 2; + to_keep[i] = 0; continue; } @@ -149,31 +157,22 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) const int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists, min_pts_radius_ + 1); // Points having too few neighbors are outliers - if (k <= min_pts_radius_) + 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(to_keep.size()); i++) { - if (to_keep[i] == 2) - { - if (extract_removed_indices_) - (*removed_indices_)[rii++] = (*indices_)[i]; - continue; - } - - if (!negative_ && to_keep[i] == 0) - { - if (extract_removed_indices_) - (*removed_indices_)[rii++] = (*indices_)[i]; - continue; - } - - if (negative_ && to_keep[i] != 0) + if (to_keep[i] == 0) { if (extract_removed_indices_) (*removed_indices_)[rii++] = (*indices_)[i]; diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index ca4046a9d42..7c747b9a992 100644 --- a/test/filters/test_filters.cpp +++ b/test/filters/test_filters.cpp @@ -1530,7 +1530,7 @@ TEST (RadiusOutlierRemoval, Filters) outremNonDense.setNegative(true); outremNonDense.filter(cloud_out_rgb_neg); EXPECT_EQ(cloud_out_rgb_neg.size(), 606); - //EXPECT_FALSE(cloud_out_rgb_neg.is_dense); + EXPECT_TRUE(cloud_out_rgb_neg.is_dense); // Test the pcl::PCLPointCloud2 method PCLPointCloud2 cloud_out2; From e1011b6cd8502cdeef4b50799348aa48b807381b Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Thu, 5 Dec 2024 13:31:22 +0100 Subject: [PATCH 8/9] Improve per review. --- .../pcl/filters/impl/filter_indices.hpp | 1 - .../filters/impl/radius_outlier_removal.hpp | 27 ++++++------------- test/filters/test_filters.cpp | 4 +-- 3 files changed, 10 insertions(+), 22 deletions(-) diff --git a/filters/include/pcl/filters/impl/filter_indices.hpp b/filters/include/pcl/filters/impl/filter_indices.hpp index ed3b965205e..de53d91c781 100644 --- a/filters/include/pcl/filters/impl/filter_indices.hpp +++ b/filters/include/pcl/filters/impl/filter_indices.hpp @@ -102,7 +102,6 @@ pcl::FilterIndices::applyFilter (PointCloud &output) { applyFilter (indices); pcl::copyPointCloud (*input_, indices, output); - output.is_dense = true; } } diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index cc50c8ea4b3..039487c2d81 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -81,7 +81,7 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) 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, 2 = remove due to inf/nan + // 0 = remove, 1 = keep std::vector to_keep(indices_->size(), 1); indices.resize (indices_->size ()); removed_indices_->resize (indices_->size ()); @@ -106,17 +106,10 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) // Note: nn_dists is sorted, so check the last item if (k == mean_k) { - // 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]) - { - to_keep[i] = 0; - continue; - } - - // 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]) + // 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])) { to_keep[i] = 0; continue; @@ -156,17 +149,13 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) // 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. const int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists, min_pts_radius_ + 1); - // Points having too few neighbors are outliers - if (!negative_ && k <= min_pts_radius_) + // 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; } - - if (negative_ && k > min_pts_radius_) { - to_keep[i] = 0; - continue; - } } } diff --git a/test/filters/test_filters.cpp b/test/filters/test_filters.cpp index 7c747b9a992..85e9a05b406 100644 --- a/test/filters/test_filters.cpp +++ b/test/filters/test_filters.cpp @@ -1525,12 +1525,12 @@ TEST (RadiusOutlierRemoval, Filters) EXPECT_EQ(cloud_out_rgb.size(), 240801); EXPECT_EQ(cloud_out_rgb.width, 240801); - EXPECT_TRUE(cloud_out_rgb.is_dense); + //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); + //EXPECT_TRUE(cloud_out_rgb_neg.is_dense); // Test the pcl::PCLPointCloud2 method PCLPointCloud2 cloud_out2; From c23f6ba02eefe956e0473bf3d1c2a98604fab6f8 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Thu, 5 Dec 2024 13:31:57 +0100 Subject: [PATCH 9/9] Added radius_outlier_removal benchmark. --- benchmarks/CMakeLists.txt | 5 ++ benchmarks/filters/radius_outlier_removal.cpp | 84 +++++++++++++++++++ 2 files changed, 89 insertions(+) create mode 100644 benchmarks/filters/radius_outlier_removal.cpp 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(); +}