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 float diff_cur = ((*input_)[(*indices_)[cp]].getVector4fMap () - ijk.cast<
float> ()).squaredNorm ();
116 float diff_prev = ((*input_)[leaf.
idx].getVector4fMap () - ijk.cast<
float> ()).squaredNorm ();
119 if (diff_cur < diff_prev)
121 if (extract_removed_indices_)
122 removed_indices_->push_back (leaf.
idx);
123 leaf.
idx = (*indices_)[cp];
127 if (extract_removed_indices_)
128 removed_indices_->push_back ((*indices_)[cp]);
133 output.
resize (leaves_.size ());
136 for (
const auto& leaf : leaves_)
137 output[cp++] = (*input_)[leaf.second.idx];
141 #define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>;
143 #endif // PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_