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> (floor (min_p[0] * inverse_leaf_size_[0]));
66 max_b_[0] =
static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
67 min_b_[1] =
static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
68 max_b_[1] =
static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
69 min_b_[2] =
static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
70 max_b_[2] =
static_cast<int> (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);
84 for (
size_t cp = 0; cp < indices_->size (); ++cp)
86 if (!input_->is_dense)
89 if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) ||
90 !pcl_isfinite (input_->points[(*indices_)[cp]].y) ||
91 !pcl_isfinite (input_->points[(*indices_)[cp]].z))
101 Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
102 ijk[0] =
static_cast<int> (floor (input_->points[(*indices_)[cp]].x * inverse_leaf_size_[0]));
103 ijk[1] =
static_cast<int> (floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1]));
104 ijk[2] =
static_cast<int> (floor (input_->points[(*indices_)[cp]].z * inverse_leaf_size_[2]));
107 int idx = (ijk - min_b_).dot (divb_mul_);
108 Leaf& leaf = leaves_[idx];
112 leaf.
idx = (*indices_)[cp];
117 float diff_cur = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<
float> ()).squaredNorm ();
118 float diff_prev = (input_->points[leaf.
idx].getVector4fMap () - ijk.cast<
float> ()).squaredNorm ();
121 if (diff_cur < diff_prev)
128 leaf.
idx = (*indices_)[cp];
133 output.
points.resize (leaves_.size ());
136 for (
typename boost::unordered_map<size_t, Leaf>::const_iterator it = leaves_.begin (); it != leaves_.end (); ++it)
137 output.
points[cp++] = input_->points[it->second.idx];
138 output.
width =
static_cast<uint32_t
> (output.
points.size ());
141 #define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>;
143 #endif // PCL_FILTERS_UNIFORM_SAMPLING_IMPL_H_