38 #ifndef PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_ 39 #define PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_ 41 #include <pcl/common/common.h> 42 #include <pcl/keypoints/uniform_sampling.h> 45 template <
typename Po
intInT>
void 51 PCL_WARN (
"[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ());
52 output.width = output.height = 0;
53 output.points.clear ();
58 output.is_dense =
true;
60 Eigen::Vector4f min_p, max_p;
62 pcl::getMinMax3D<PointInT>(*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);
83 for (
size_t cp = 0; cp < indices_->size (); ++cp)
85 if (!input_->is_dense)
87 if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) ||
88 !pcl_isfinite (input_->points[(*indices_)[cp]].y) ||
89 !pcl_isfinite (input_->points[(*indices_)[cp]].z))
92 Eigen::Vector4i ijk = Eigen::Vector4i::Zero ();
93 ijk[0] =
static_cast<int> (floor (input_->points[(*indices_)[cp]].x * inverse_leaf_size_[0]));
94 ijk[1] =
static_cast<int> (floor (input_->points[(*indices_)[cp]].y * inverse_leaf_size_[1]));
95 ijk[2] =
static_cast<int> (floor (input_->points[(*indices_)[cp]].z * inverse_leaf_size_[2]));
98 int idx = (ijk - min_b_).dot (divb_mul_);
99 Leaf& leaf = leaves_[idx];
103 leaf.
idx = (*indices_)[cp];
108 float diff_cur = (input_->points[(*indices_)[cp]].getVector4fMap () - ijk.cast<
float> ()).squaredNorm ();
109 float diff_prev = (input_->points[leaf.
idx].getVector4fMap () - ijk.cast<
float> ()).squaredNorm ();
112 if (diff_cur < diff_prev)
113 leaf.
idx = (*indices_)[cp];
117 output.points.resize (leaves_.size ());
120 for (
typename boost::unordered_map<size_t, Leaf>::const_iterator it = leaves_.begin (); it != leaves_.end (); ++it)
121 output.points[cp++] = it->second.idx;
122 output.width = static_cast<uint32_t> (output.points.size ());
125 #define PCL_INSTANTIATE_UniformSampling(T) template class PCL_EXPORTS pcl::UniformSampling<T>; 127 #endif // PCL_KEYPOINTS_UNIFORM_SAMPLING_IMPL_H_