43 #include <pcl/features/fpfh.h>
45 #include <pcl/common/point_tests.h>
46 #include <pcl/features/pfh_tools.h>
50 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
53 int p_idx,
int q_idx,
float &f1,
float &f2,
float &f3,
float &f4)
56 cloud[q_idx].getVector4fMap (), normals[q_idx].getNormalVector4fMap (),
62 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
65 int p_idx,
int row,
const std::vector<int> &indices,
66 Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
68 Eigen::Vector4f pfh_tuple;
71 int nr_bins_f1 =
static_cast<int> (hist_f1.cols ());
72 int nr_bins_f2 =
static_cast<int> (hist_f2.cols ());
73 int nr_bins_f3 =
static_cast<int> (hist_f3.cols ());
76 float hist_incr = 100.0f /
static_cast<float>(indices.size () - 1);
79 for (
const auto &index : indices)
86 if (!
computePairFeatures (cloud, normals, p_idx, index, pfh_tuple[0], pfh_tuple[1], pfh_tuple[2], pfh_tuple[3]))
90 int h_index =
static_cast<int> (std::floor (nr_bins_f1 * ((pfh_tuple[0] +
M_PI) * d_pi_)));
91 if (h_index < 0) h_index = 0;
92 if (h_index >= nr_bins_f1) h_index = nr_bins_f1 - 1;
93 hist_f1 (row, h_index) += hist_incr;
95 h_index =
static_cast<int> (std::floor (nr_bins_f2 * ((pfh_tuple[1] + 1.0) * 0.5)));
96 if (h_index < 0) h_index = 0;
97 if (h_index >= nr_bins_f2) h_index = nr_bins_f2 - 1;
98 hist_f2 (row, h_index) += hist_incr;
100 h_index =
static_cast<int> (std::floor (nr_bins_f3 * ((pfh_tuple[2] + 1.0) * 0.5)));
101 if (h_index < 0) h_index = 0;
102 if (h_index >= nr_bins_f3) h_index = nr_bins_f3 - 1;
103 hist_f3 (row, h_index) += hist_incr;
108 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
110 const Eigen::MatrixXf &hist_f1,
const Eigen::MatrixXf &hist_f2,
const Eigen::MatrixXf &hist_f3,
111 const std::vector<int> &indices,
const std::vector<float> &dists, Eigen::VectorXf &fpfh_histogram)
113 assert (indices.size () == dists.size ());
115 double sum_f1 = 0.0, sum_f2 = 0.0, sum_f3 = 0.0;
116 float weight = 0.0, val_f1, val_f2, val_f3;
119 const auto nr_bins_f1 = hist_f1.cols ();
120 const auto nr_bins_f2 = hist_f2.cols ();
121 const auto nr_bins_f3 = hist_f3.cols ();
122 const auto nr_bins_f12 = nr_bins_f1 + nr_bins_f2;
125 fpfh_histogram.setZero (nr_bins_f1 + nr_bins_f2 + nr_bins_f3);
128 for (std::size_t idx = 0; idx < indices.size (); ++idx)
135 weight = 1.0f / dists[idx];
138 for (Eigen::MatrixXf::Index f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
140 val_f1 = hist_f1 (indices[idx], f1_i) * weight;
142 fpfh_histogram[f1_i] += val_f1;
145 for (Eigen::MatrixXf::Index f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
147 val_f2 = hist_f2 (indices[idx], f2_i) * weight;
149 fpfh_histogram[f2_i + nr_bins_f1] += val_f2;
152 for (Eigen::MatrixXf::Index f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
154 val_f3 = hist_f3 (indices[idx], f3_i) * weight;
156 fpfh_histogram[f3_i + nr_bins_f12] += val_f3;
161 sum_f1 = 100.0 / sum_f1;
163 sum_f2 = 100.0 / sum_f2;
165 sum_f3 = 100.0 / sum_f3;
168 const auto denormalize_with = [](
auto factor)
170 return [=](
const auto& data) {
return data * factor; };
173 auto last = fpfh_histogram.data ();
174 last = std::transform(last, last + nr_bins_f1, last, denormalize_with (sum_f1));
175 last = std::transform(last, last + nr_bins_f2, last, denormalize_with (sum_f2));
176 std::transform(last, last + nr_bins_f3, last, denormalize_with (sum_f3));
180 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
182 Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
186 std::vector<int> nn_indices (k_);
187 std::vector<float> nn_dists (k_);
189 std::set<int> spfh_indices;
190 spfh_hist_lookup.resize (surface_->size ());
194 if (surface_ != input_ ||
195 indices_->size () != surface_->size ())
197 for (
const auto& p_idx: *indices_)
199 if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
202 spfh_indices.insert (nn_indices.begin (), nn_indices.end ());
208 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
209 spfh_indices.insert (
static_cast<int> (idx));
213 std::size_t data_size = spfh_indices.size ();
214 hist_f1.setZero (data_size, nr_bins_f1_);
215 hist_f2.setZero (data_size, nr_bins_f2_);
216 hist_f3.setZero (data_size, nr_bins_f3_);
220 for (
const auto& p_idx: spfh_indices)
223 if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
227 computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1, hist_f2, hist_f3);
230 spfh_hist_lookup[p_idx] = i;
236 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
241 std::vector<int> nn_indices (k_);
242 std::vector<float> nn_dists (k_);
244 std::vector<int> spfh_hist_lookup;
245 computeSPFHSignatures (spfh_hist_lookup, hist_f1_, hist_f2_, hist_f3_);
247 output.is_dense =
true;
249 if (input_->is_dense)
252 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
254 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
256 for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d)
257 output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
259 output.is_dense =
false;
265 for (
auto &nn_index : nn_indices)
266 nn_index = spfh_hist_lookup[nn_index];
269 weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
272 std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output[idx].histogram);
278 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
280 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
281 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
283 for (Eigen::Index d = 0; d < fpfh_histogram_.size (); ++d)
284 output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
286 output.is_dense =
false;
292 for (
auto &nn_index : nn_indices)
293 nn_index = spfh_hist_lookup[nn_index];
296 weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
299 std::copy_n(fpfh_histogram_.data (), fpfh_histogram_.size (), output[idx].histogram);
304 #define PCL_INSTANTIATE_FPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimation<T,NT,OutT>;