From 976836be15b1aca6d0353363993dcf62a61f6040 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=83=E5=B0=8F=E4=B8=98=E4=BA=BA?= Date: Tue, 18 Jun 2024 18:45:32 +0800 Subject: [PATCH] Make UniformSampling inherit from FilterIndices instead of Filter (#6064) * test passed * remove useless part * modify for expression * modify for expression again * back Ptr typedef * add resize * add ::Ptr check * replace std::isfinate with pcl::isXYZFinite --- .../pcl/filters/impl/uniform_sampling.hpp | 55 +++++++-------- .../include/pcl/filters/uniform_sampling.h | 17 +++-- test/filters/test_uniform_sampling.cpp | 70 +++++++++++++++++-- 3 files changed, 99 insertions(+), 43 deletions(-) diff --git a/filters/include/pcl/filters/impl/uniform_sampling.hpp b/filters/include/pcl/filters/impl/uniform_sampling.hpp index d51a6bcee31..c1c0f251de1 100644 --- a/filters/include/pcl/filters/impl/uniform_sampling.hpp +++ b/filters/include/pcl/filters/impl/uniform_sampling.hpp @@ -40,22 +40,17 @@ #include #include +#include ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::UniformSampling::applyFilter (PointCloud &output) +pcl::UniformSampling::applyFilter (Indices &indices) { - // Has the input dataset been set already? - if (!input_) - { - PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ()); - output.width = output.height = 0; - output.clear (); - return; - } + // The arrays to be used + indices.resize (indices_->size ()); + removed_indices_->resize (indices_->size ()); - output.height = 1; // downsampling breaks the organized structure - output.is_dense = true; // we filter out invalid points + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator Eigen::Vector4f min_p, max_p; // Get the minimum and maximum dimensions @@ -79,27 +74,24 @@ pcl::UniformSampling::applyFilter (PointCloud &output) // Set up the division multiplier divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0); - removed_indices_->clear(); // First pass: build a set of leaves with the point index closest to the leaf center - for (std::size_t cp = 0; cp < indices_->size (); ++cp) + for (const auto& cp : *indices_) { if (!input_->is_dense) { // Check if the point is invalid - if (!std::isfinite ((*input_)[(*indices_)[cp]].x) || - !std::isfinite ((*input_)[(*indices_)[cp]].y) || - !std::isfinite ((*input_)[(*indices_)[cp]].z)) + if (!pcl::isXYZFinite ((*input_)[cp])) { if (extract_removed_indices_) - removed_indices_->push_back ((*indices_)[cp]); + (*removed_indices_)[rii++] = cp; continue; } } Eigen::Vector4i ijk = Eigen::Vector4i::Zero (); - ijk[0] = static_cast (std::floor ((*input_)[(*indices_)[cp]].x * inverse_leaf_size_[0])); - ijk[1] = static_cast (std::floor ((*input_)[(*indices_)[cp]].y * inverse_leaf_size_[1])); - ijk[2] = static_cast (std::floor ((*input_)[(*indices_)[cp]].z * inverse_leaf_size_[2])); + ijk[0] = static_cast (std::floor ((*input_)[cp].x * inverse_leaf_size_[0])); + ijk[1] = static_cast (std::floor ((*input_)[cp].y * inverse_leaf_size_[1])); + ijk[2] = static_cast (std::floor ((*input_)[cp].z * inverse_leaf_size_[2])); // Compute the leaf index int idx = (ijk - min_b_).dot (divb_mul_); @@ -111,7 +103,7 @@ pcl::UniformSampling::applyFilter (PointCloud &output) // First time we initialize the index if (leaf.idx == -1) { - leaf.idx = (*indices_)[cp]; + leaf.idx = cp; continue; } @@ -119,33 +111,36 @@ pcl::UniformSampling::applyFilter (PointCloud &output) Eigen::Vector4f voxel_center = (ijk.cast() + Eigen::Vector4f::Constant(0.5)) * search_radius_; voxel_center[3] = 0; // Check to see if this point is closer to the leaf center than the previous one we saved - float diff_cur = ((*input_)[(*indices_)[cp]].getVector4fMap () - voxel_center).squaredNorm (); + float diff_cur = ((*input_)[cp].getVector4fMap () - voxel_center).squaredNorm (); float diff_prev = ((*input_)[leaf.idx].getVector4fMap () - voxel_center).squaredNorm (); // If current point is closer, copy its index instead if (diff_cur < diff_prev) { if (extract_removed_indices_) - removed_indices_->push_back (leaf.idx); - leaf.idx = (*indices_)[cp]; + (*removed_indices_)[rii++] = leaf.idx; + leaf.idx = cp; } else { if (extract_removed_indices_) - removed_indices_->push_back ((*indices_)[cp]); + (*removed_indices_)[rii++] = cp; } } + removed_indices_->resize(rii); // Second pass: go over all leaves and copy data - output.resize (leaves_.size ()); - std::size_t cp = 0; - + indices.resize (leaves_.size ()); for (const auto& leaf : leaves_) { if (leaf.second.count >= min_points_per_voxel_) - output[cp++] = (*input_)[leaf.second.idx]; + indices[oii++] = leaf.second.idx; + } + + indices.resize (oii); + if(negative_){ + indices.swap(*removed_indices_); } - output.resize (cp); } #define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling; diff --git a/filters/include/pcl/filters/uniform_sampling.h b/filters/include/pcl/filters/uniform_sampling.h index d814adbf265..53b6c26dd24 100644 --- a/filters/include/pcl/filters/uniform_sampling.h +++ b/filters/include/pcl/filters/uniform_sampling.h @@ -39,7 +39,7 @@ #pragma once -#include +#include #include @@ -57,9 +57,11 @@ namespace pcl * \ingroup filters */ template - class UniformSampling: public Filter + class UniformSampling: public FilterIndices { - using PointCloud = typename Filter::PointCloud; + using PointCloud = typename FilterIndices::PointCloud; + + using FilterIndices::negative_; using Filter::filter_name_; using Filter::input_; @@ -76,7 +78,7 @@ namespace pcl /** \brief Empty constructor. */ UniformSampling (bool extract_removed_indices = false) : - Filter(extract_removed_indices), + FilterIndices(extract_removed_indices), leaves_ (), leaf_size_ (Eigen::Vector4f::Zero ()), inverse_leaf_size_ (Eigen::Vector4f::Zero ()), @@ -120,6 +122,7 @@ namespace pcl inline unsigned int getMinimumPointsNumberPerVoxel () const { return min_points_per_voxel_; } + protected: /** \brief Simple structure to hold an nD centroid and the number of points in a leaf. */ struct Leaf @@ -147,11 +150,11 @@ namespace pcl /** \brief Minimum number of points per voxel. */ unsigned int min_points_per_voxel_{0}; - /** \brief Downsample a Point Cloud using a voxelized grid approach - * \param[out] output the resultant point cloud message + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. */ void - applyFilter (PointCloud &output) override; + applyFilter (Indices &indices) override; }; } diff --git a/test/filters/test_uniform_sampling.cpp b/test/filters/test_uniform_sampling.cpp index 50a3446c036..7403a1d49cd 100644 --- a/test/filters/test_uniform_sampling.cpp +++ b/test/filters/test_uniform_sampling.cpp @@ -61,17 +61,75 @@ TEST(UniformSampling, extractRemovedIndices) // sure that each cell has at least one point. As a result, we expect 1000 points in // the output cloud and the rest in removed indices. - pcl::UniformSampling us(true); // extract removed indices - us.setInputCloud(xyz); - us.setRadiusSearch(0.1); + pcl::UniformSampling::Ptr us_ptr(new pcl::UniformSampling(true));// extract removed indices + us_ptr->setRadiusSearch(0.1); pcl::PointCloud output; - us.filter(output); + pcl::Indices indices; + + // Empty input cloud + us_ptr->filter(output); + us_ptr->filter(indices); - auto removed_indices = us.getRemovedIndices(); + us_ptr->setInputCloud(xyz); + // Cloud + us_ptr->filter(output); + // Indices + us_ptr->filter(indices); + + for (const auto& outputIndex : indices) + { + // Check if the point exists in the output cloud + bool found = false; + for (const auto& j : output) + { + if (j.x == (*xyz)[outputIndex].x && + j.y == (*xyz)[outputIndex].y && + j.z == (*xyz)[outputIndex].z) + { + found = true; + break; + } + } + + // Assert that the point was found in the output cloud + ASSERT_TRUE(found); + } + + auto removed_indices = us_ptr->getRemovedIndices(); ASSERT_EQ(output.size(), 1000); - EXPECT_EQ(removed_indices->size(), xyz->size() - 1000); + EXPECT_EQ(int(removed_indices->size()), int(xyz->size() - 1000)); std::set removed_indices_set(removed_indices->begin(), removed_indices->end()); ASSERT_EQ(removed_indices_set.size(), removed_indices->size()); + + // Negative + us_ptr->setNegative (true); + us_ptr->filter(output); + removed_indices = us_ptr->getRemovedIndices (); + EXPECT_EQ (int (removed_indices->size ()), 1000); + EXPECT_EQ (int (output.size ()), int (xyz->size() - 1000)); + + // Organized + us_ptr->setKeepOrganized (true); + us_ptr->setNegative (false); + us_ptr->filter(output); + removed_indices = us_ptr->getRemovedIndices (); + EXPECT_EQ (int (removed_indices->size ()), int(xyz->size() - 1000)); + for (std::size_t i = 0; i < removed_indices->size (); ++i) + { + EXPECT_TRUE (std::isnan (output.at ((*removed_indices)[i]).x)); + EXPECT_TRUE (std::isnan (output.at ((*removed_indices)[i]).y)); + EXPECT_TRUE (std::isnan (output.at ((*removed_indices)[i]).z)); + } + + EXPECT_EQ (output.width, xyz->width); + EXPECT_EQ (output.height, xyz->height); + + // Check input cloud with nan values + us_ptr->setInputCloud (output.makeShared ()); + us_ptr->setRadiusSearch(2); + us_ptr->filter (output); + removed_indices = us_ptr->getRemovedIndices (); + EXPECT_EQ (int (removed_indices->size ()), output.size()-1); } int