41 #include <pcl/features/pfh.h>
43 #include <pcl/common/point_tests.h>
47 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
50 int p_idx,
int q_idx,
float &f1,
float &f2,
float &f3,
float &f4)
53 cloud[q_idx].getVector4fMap (), normals[q_idx].getNormalVector4fMap (),
59 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
62 const pcl::Indices &indices,
int nr_split, Eigen::VectorXf &pfh_histogram)
67 pfh_histogram.setZero ();
70 float hist_incr = 100.0f /
static_cast<float> (indices.size () * (indices.size () - 1) / 2);
72 std::pair<int, int> key;
73 bool key_found =
false;
76 for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
78 for (std::size_t j_idx = 0; j_idx < i_idx; ++j_idx)
98 key = std::pair<int, int> (p1, p2);
101 std::map<std::pair<int, int>, Eigen::Vector4f, std::less<>, Eigen::aligned_allocator<std::pair<const std::pair<int, int>, Eigen::Vector4f> > >::iterator fm_it = feature_map_.find (key);
102 if (fm_it != feature_map_.end ())
104 pfh_tuple_ = fm_it->second;
111 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
119 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
123 f_index_[0] =
static_cast<int> (std::floor (nr_split * ((pfh_tuple_[0] +
M_PI) * d_pi_)));
124 if (f_index_[0] < 0) f_index_[0] = 0;
125 if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;
127 f_index_[1] =
static_cast<int> (std::floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5)));
128 if (f_index_[1] < 0) f_index_[1] = 0;
129 if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;
131 f_index_[2] =
static_cast<int> (std::floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5)));
132 if (f_index_[2] < 0) f_index_[2] = 0;
133 if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;
138 for (
const int &d : f_index_)
143 pfh_histogram[h_index] += hist_incr;
145 if (use_cache_ && !key_found)
148 feature_map_[key] = pfh_tuple_;
151 key_list_.push (key);
153 if (key_list_.size () > max_cache_size_)
156 feature_map_.erase (key_list_.front ());
165 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
169 feature_map_.clear ();
170 std::queue<std::pair<int, int> > empty;
171 std::swap (key_list_, empty);
173 pfh_histogram_.setZero (nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
178 std::vector<float> nn_dists (k_);
180 output.is_dense =
true;
182 if (input_->is_dense)
185 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
187 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
189 for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d)
190 output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
192 output.is_dense =
false;
197 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
200 for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d)
201 output[idx].histogram[d] = pfh_histogram_[d];
207 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
209 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
210 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
212 for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d)
213 output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
215 output.is_dense =
false;
220 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
223 for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d)
224 output[idx].histogram[d] = pfh_histogram_[d];
229 #define PCL_INSTANTIATE_PFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHEstimation<T,NT,OutT>;