41 #ifndef PCL_FEATURES_IMPL_FPFH_OMP_H_
42 #define PCL_FEATURES_IMPL_FPFH_OMP_H_
44 #include <pcl/features/fpfh_omp.h>
47 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
50 std::vector<int> spfh_indices_vec;
51 std::vector<int> spfh_hist_lookup (surface_->points.size ());
55 if (surface_ != input_ ||
56 indices_->size () != surface_->points.size ())
58 std::vector<int> nn_indices (k_);
59 std::vector<float> nn_dists (k_);
61 std::set<int> spfh_indices_set;
62 for (
size_t idx = 0; idx < indices_->size (); ++idx)
64 int p_idx = (*indices_)[idx];
65 if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
68 spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ());
70 spfh_indices_vec.resize (spfh_indices_set.size ());
71 std::copy (spfh_indices_set.begin (), spfh_indices_set.end (), spfh_indices_vec.begin ());
76 spfh_indices_vec.resize (indices_->size ());
77 for (
int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
78 spfh_indices_vec[idx] = idx;
82 size_t data_size = spfh_indices_vec.size ();
83 hist_f1_.setZero (data_size, nr_bins_f1_);
84 hist_f2_.setZero (data_size, nr_bins_f2_);
85 hist_f3_.setZero (data_size, nr_bins_f3_);
87 std::vector<int> nn_indices (k_);
88 std::vector<float> nn_dists (k_);
93 #pragma omp parallel for shared (spfh_hist_lookup) private (nn_indices, nn_dists) num_threads(threads_)
95 for (
int i = 0; i < static_cast<int> (spfh_indices_vec.size ()); ++i)
98 int p_idx = spfh_indices_vec[i];
101 if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
105 this->computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_);
108 spfh_hist_lookup[p_idx] = i;
112 int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
119 #pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
121 for (
int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
124 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
125 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
127 for (
int d = 0; d < nr_bins; ++d)
128 output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
130 output.is_dense =
false;
137 for (
size_t i = 0; i < nn_indices.size (); ++i)
138 nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
141 Eigen::VectorXf fpfh_histogram = Eigen::VectorXf::Zero (nr_bins);
142 weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram);
145 for (
int d = 0; d < nr_bins; ++d)
146 output.points[idx].histogram[d] = fpfh_histogram[d];
151 #define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP<T,NT,OutT>;
153 #endif // PCL_FEATURES_IMPL_FPFH_OMP_H_
FPFHEstimationOMP estimates the Fast Point Feature Histogram (FPFH) descriptor for a given point clou...