39 #ifndef PCL_FEATURES_IMPL_PFH_H_
40 #define PCL_FEATURES_IMPL_PFH_H_
42 #include <pcl/features/pfh.h>
45 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
48 int p_idx,
int q_idx,
float &f1,
float &f2,
float &f3,
float &f4)
51 cloud.
points[q_idx].getVector4fMap (), normals.
points[q_idx].getNormalVector4fMap (),
57 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
60 const std::vector<int> &indices,
int nr_split, Eigen::VectorXf &pfh_histogram)
65 pfh_histogram.setZero ();
68 float hist_incr = 100.0f /
static_cast<float> (indices.size () * (indices.size () - 1) / 2);
70 std::pair<int, int> key;
72 for (
size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
74 for (
size_t j_idx = 0; j_idx < i_idx; ++j_idx)
94 key = std::pair<int, int> (p1, p2);
97 std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<Eigen::Vector4f> >::iterator fm_it = feature_map_.find (key);
98 if (fm_it != feature_map_.end ())
99 pfh_tuple_ = fm_it->second;
104 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
110 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
114 f_index_[0] =
static_cast<int> (floor (nr_split * ((pfh_tuple_[0] + M_PI) * d_pi_)));
115 if (f_index_[0] < 0) f_index_[0] = 0;
116 if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;
118 f_index_[1] =
static_cast<int> (floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5)));
119 if (f_index_[1] < 0) f_index_[1] = 0;
120 if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;
122 f_index_[2] =
static_cast<int> (floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5)));
123 if (f_index_[2] < 0) f_index_[2] = 0;
124 if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;
129 for (
int d = 0; d < 3; ++d)
131 h_index += h_p * f_index_[d];
134 pfh_histogram[h_index] += hist_incr;
139 feature_map_[key] = pfh_tuple_;
142 key_list_.push (key);
144 if (key_list_.size () > max_cache_size_)
147 feature_map_.erase (key_list_.back ());
156 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
160 feature_map_.clear ();
161 std::queue<std::pair<int, int> > empty;
162 std::swap (key_list_, empty);
164 pfh_histogram_.setZero (nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
168 std::vector<int> nn_indices (k_);
169 std::vector<float> nn_dists (k_);
173 if (input_->is_dense)
176 for (
size_t idx = 0; idx < indices_->size (); ++idx)
178 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
180 for (
int d = 0; d < pfh_histogram_.size (); ++d)
181 output.
points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
188 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
191 for (
int d = 0; d < pfh_histogram_.size (); ++d)
192 output.
points[idx].histogram[d] = pfh_histogram_[d];
198 for (
size_t idx = 0; idx < indices_->size (); ++idx)
200 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
201 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
203 for (
int d = 0; d < pfh_histogram_.size (); ++d)
204 output.
points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
211 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
214 for (
int d = 0; d < pfh_histogram_.size (); ++d)
215 output.
points[idx].histogram[d] = pfh_histogram_[d];
220 #define PCL_INSTANTIATE_PFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHEstimation<T,NT,OutT>;
222 #endif // PCL_FEATURES_IMPL_PFH_H_