41 #ifndef PCL_FEATURES_IMPL_FPFH_OMP_H_
42 #define PCL_FEATURES_IMPL_FPFH_OMP_H_
46 #include <pcl/features/fpfh_omp.h>
49 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
54 threads_ = omp_get_num_procs();
59 threads_ = nr_threads;
63 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
66 std::vector<int> spfh_indices_vec;
67 std::vector<int> spfh_hist_lookup (surface_->points.size ());
71 if (surface_ != input_ ||
72 indices_->size () != surface_->points.size ())
74 std::vector<int> nn_indices (k_);
75 std::vector<float> nn_dists (k_);
77 std::set<int> spfh_indices_set;
78 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
80 int p_idx = (*indices_)[idx];
81 if (!isFinite ((*input_)[p_idx]) ||
82 this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
85 spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ());
87 spfh_indices_vec.resize (spfh_indices_set.size ());
88 std::copy (spfh_indices_set.cbegin (), spfh_indices_set.cend (), spfh_indices_vec.begin ());
93 spfh_indices_vec.resize (indices_->size ());
94 std::iota(spfh_indices_vec.begin (), spfh_indices_vec.end (),
95 static_cast<decltype(spfh_indices_vec)::value_type>(0));
99 const auto data_size = spfh_indices_vec.size ();
100 hist_f1_.setZero (data_size, nr_bins_f1_);
101 hist_f2_.setZero (data_size, nr_bins_f2_);
102 hist_f3_.setZero (data_size, nr_bins_f3_);
104 std::vector<int> nn_indices (k_);
105 std::vector<float> nn_dists (k_);
110 #pragma omp parallel for shared (spfh_hist_lookup) private (nn_indices, nn_dists) num_threads(threads_)
112 for (std::ptrdiff_t i = 0; i < static_cast<std::ptrdiff_t> (spfh_indices_vec.size ()); ++i)
115 int p_idx = spfh_indices_vec[i];
118 if (!isFinite ((*input_)[p_idx]) ||
119 this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
123 this->computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_);
126 spfh_hist_lookup[p_idx] = i;
130 int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
137 #pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
139 for (std::ptrdiff_t idx = 0; idx < static_cast<std::ptrdiff_t> (indices_->size ()); ++idx)
142 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
143 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
145 for (
int d = 0; d < nr_bins; ++d)
146 output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
148 output.is_dense =
false;
155 for (
int &nn_index : nn_indices)
156 nn_index = spfh_hist_lookup[nn_index];
159 Eigen::VectorXf fpfh_histogram = Eigen::VectorXf::Zero (nr_bins);
160 weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram);
163 for (
int d = 0; d < nr_bins; ++d)
164 output.points[idx].histogram[d] = fpfh_histogram[d];
169 #define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP<T,NT,OutT>;
171 #endif // PCL_FEATURES_IMPL_FPFH_OMP_H_