51 PCL_WARN (
"[pcl::%s::detectKeypoints] No input dataset given!\n", getClassName ().c_str ());
52 output.width = output.height = 0;
58 output.is_dense =
true;
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];
141 output.width = output.size ();