Skip to content

Commit

Permalink
Make UniformSampling inherit from FilterIndices instead of Filter (#6064
Browse files Browse the repository at this point in the history 
)

* 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
  • Loading branch information
@QiuYilin
QiuYilin authored Jun 18, 2024
1 parent eb9c003 commit 976836b
Show file tree
Hide file tree
Showing 3 changed files with 99 additions and 43 deletions.
55 changes: 25 additions & 30 deletions filters/include/pcl/filters/impl/uniform_sampling.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,22 +40,17 @@

#include <pcl/common/common.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/common/point_tests.h>

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::UniformSampling<PointT>::applyFilter (PointCloud &output)
pcl::UniformSampling<PointT>::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
Expand All @@ -79,27 +74,24 @@ pcl::UniformSampling<PointT>::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<int> (std::floor ((*input_)[(*indices_)[cp]].x * inverse_leaf_size_[0]));
ijk[1] = static_cast<int> (std::floor ((*input_)[(*indices_)[cp]].y * inverse_leaf_size_[1]));
ijk[2] = static_cast<int> (std::floor ((*input_)[(*indices_)[cp]].z * inverse_leaf_size_[2]));
ijk[0] = static_cast<int> (std::floor ((*input_)[cp].x * inverse_leaf_size_[0]));
ijk[1] = static_cast<int> (std::floor ((*input_)[cp].y * inverse_leaf_size_[1]));
ijk[2] = static_cast<int> (std::floor ((*input_)[cp].z * inverse_leaf_size_[2]));

// Compute the leaf index
int idx = (ijk - min_b_).dot (divb_mul_);
Expand All @@ -111,41 +103,44 @@ pcl::UniformSampling<PointT>::applyFilter (PointCloud &output)
// First time we initialize the index
if (leaf.idx == -1)
{
leaf.idx = (*indices_)[cp];
leaf.idx = cp;
continue;
}

// Compute the voxel center
Eigen::Vector4f voxel_center = (ijk.cast<float>() + 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<T>;
Expand Down
17 changes: 10 additions & 7 deletions filters/include/pcl/filters/uniform_sampling.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@

#pragma once

#include <pcl/filters/filter.h>
#include <pcl/filters/filter_indices.h>

#include <unordered_map>

Expand All @@ -57,9 +57,11 @@ namespace pcl
* \ingroup filters
*/
template <typename PointT>
class UniformSampling: public Filter<PointT>
class UniformSampling: public FilterIndices<PointT>
{
using PointCloud = typename Filter<PointT>::PointCloud;
using PointCloud = typename FilterIndices<PointT>::PointCloud;

using FilterIndices<PointT>::negative_;

using Filter<PointT>::filter_name_;
using Filter<PointT>::input_;
Expand All @@ -76,7 +78,7 @@ namespace pcl

/** \brief Empty constructor. */
UniformSampling (bool extract_removed_indices = false) :
Filter<PointT>(extract_removed_indices),
FilterIndices<PointT>(extract_removed_indices),
leaves_ (),
leaf_size_ (Eigen::Vector4f::Zero ()),
inverse_leaf_size_ (Eigen::Vector4f::Zero ()),
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
};
}

Expand Down
70 changes: 64 additions & 6 deletions test/filters/test_uniform_sampling.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<pcl::PointXYZ> us(true); // extract removed indices
us.setInputCloud(xyz);
us.setRadiusSearch(0.1);
pcl::UniformSampling<pcl::PointXYZ>::Ptr us_ptr(new pcl::UniformSampling<pcl::PointXYZ>(true));// extract removed indices
us_ptr->setRadiusSearch(0.1);
pcl::PointCloud<pcl::PointXYZ> 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<int> 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
Expand Down

0 comments on commit 976836b

Please sign in to comment.