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
52 threads_ = omp_get_num_procs();
57 threads_ = nr_threads;
61 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
64 std::vector<int> spfh_indices_vec;
65 std::vector<int> spfh_hist_lookup (surface_->points.size ());
69 if (surface_ != input_ ||
70 indices_->size () != surface_->points.size ())
72 std::vector<int> nn_indices (k_);
73 std::vector<float> nn_dists (k_);
75 std::set<int> spfh_indices_set;
76 for (
size_t idx = 0; idx < indices_->size (); ++idx)
78 int p_idx = (*indices_)[idx];
79 if (!isFinite ((*input_)[p_idx]) ||
80 this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
83 spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ());
85 spfh_indices_vec.resize (spfh_indices_set.size ());
86 std::copy (spfh_indices_set.begin (), spfh_indices_set.end (), spfh_indices_vec.begin ());
91 spfh_indices_vec.resize (indices_->size ());
92 for (
int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
93 spfh_indices_vec[idx] = idx;
97 size_t data_size = spfh_indices_vec.size ();
98 hist_f1_.setZero (data_size, nr_bins_f1_);
99 hist_f2_.setZero (data_size, nr_bins_f2_);
100 hist_f3_.setZero (data_size, nr_bins_f3_);
102 std::vector<int> nn_indices (k_);
103 std::vector<float> nn_dists (k_);
108 #pragma omp parallel for shared (spfh_hist_lookup) private (nn_indices, nn_dists) num_threads(threads_)
110 for (
int i = 0; i < static_cast<int> (spfh_indices_vec.size ()); ++i)
113 int p_idx = spfh_indices_vec[i];
116 if (!isFinite ((*input_)[p_idx]) ||
117 this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
121 this->computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_);
124 spfh_hist_lookup[p_idx] = i;
128 int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
135 #pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
137 for (
int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
140 if (!isFinite ((*input_)[(*indices_)[idx]]) ||
141 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
143 for (
int d = 0; d < nr_bins; ++d)
144 output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
146 output.is_dense =
false;
153 for (
size_t i = 0; i < nn_indices.size (); ++i)
154 nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
157 Eigen::VectorXf fpfh_histogram = Eigen::VectorXf::Zero (nr_bins);
158 weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram);
161 for (
int d = 0; d < nr_bins; ++d)
162 output.points[idx].histogram[d] = fpfh_histogram[d];
167 #define PCL_INSTANTIATE_FPFHEstimationOMP(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimationOMP<T,NT,OutT>;
169 #endif // PCL_FEATURES_IMPL_FPFH_OMP_H_