38 #ifndef PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_
39 #define PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_
42 #include <pcl/filters/uniform_sampling.h>
45 template <
typename Po
intT>
void
51 PCL_WARN (
"[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ());
60 Eigen::Vector4f min_p, max_p;
62 pcl::getMinMax3D<PointT>(*input_, min_p, max_p);
65 min_b_[0] =
static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
66 max_b_[0] =
static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
67 min_b_[1] =
static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
68 max_b_[1] =
static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
69 min_b_[2] =
static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
70 max_b_[2] =
static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
73 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
80 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
82 removed_indices_->clear();
84 for (std::size_t cp = 0; cp < indices_->size (); ++cp)
86 if (!input_->is_dense)
89 if (!std::isfinite ((*input_)[(*indices_)[cp]].x) ||
90 !std::isfinite ((*input_)[(*indices_)[cp]].y) ||
91 !std::isfinite ((*input_)[(*indices_)[cp]].z))
93 if (extract_removed_indices_)
94 removed_indices_->push_back ((*indices_)[cp]);
99 Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
100 ijk[0] =
static_cast<int> (std::floor ((*input_)[(*indices_)[cp]].x * inverse_leaf_size_[0]));
101 ijk[1] =
static_cast<int> (std::floor ((*input_)[(*indices_)[cp]].y * inverse_leaf_size_[1]));
102 ijk[2] =
static_cast<int> (std::floor ((*input_)[(*indices_)[cp]].z * inverse_leaf_size_[2]));
105 int idx = (ijk - min_b_).dot (divb_mul_);
106 Leaf& leaf = leaves_[idx];
110 leaf.
idx = (*indices_)[cp];
115 Eigen::Vector4f voxel_center = (ijk.cast<
float>() + Eigen::Vector4f::Constant(0.5)) * search_radius_;
118 float diff_cur = ((*input_)[(*indices_)[cp]].getVector4fMap () - voxel_center).squaredNorm ();
119 float diff_prev = ((*input_)[leaf.
idx].getVector4fMap () - voxel_center).squaredNorm ();
122 if (diff_cur < diff_prev)
124 if (extract_removed_indices_)
125 removed_indices_->push_back (leaf.
idx);
126 leaf.
idx = (*indices_)[cp];
130 if (extract_removed_indices_)
131 removed_indices_->push_back ((*indices_)[cp]);
136 output.
resize (leaves_.size ());
139 for (
const auto& leaf : leaves_)
140 output[cp++] = (*input_)[leaf.second.idx];
144 #define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>;
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
Define standard C methods and C++ classes that are common to all methods.