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;
71 bool key_found =
false;
74 for (
size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
76 for (
size_t j_idx = 0; j_idx < i_idx; ++j_idx)
96 key = std::pair<int, int> (p1, p2);
99 std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<std::pair<const std::pair<int, int>, Eigen::Vector4f> > >::iterator fm_it = feature_map_.find (key);
100 if (fm_it != feature_map_.end ())
102 pfh_tuple_ = fm_it->second;
109 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
117 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
121 f_index_[0] =
static_cast<int> (floor (nr_split * ((pfh_tuple_[0] + M_PI) * d_pi_)));
122 if (f_index_[0] < 0) f_index_[0] = 0;
123 if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;
125 f_index_[1] =
static_cast<int> (floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5)));
126 if (f_index_[1] < 0) f_index_[1] = 0;
127 if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;
129 f_index_[2] =
static_cast<int> (floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5)));
130 if (f_index_[2] < 0) f_index_[2] = 0;
131 if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;
136 for (
int d = 0; d < 3; ++d)
138 h_index += h_p * f_index_[d];
141 pfh_histogram[h_index] += hist_incr;
143 if (use_cache_ && !key_found)
146 feature_map_[key] = pfh_tuple_;
149 key_list_.push (key);
151 if (key_list_.size () > max_cache_size_)
154 feature_map_.erase (key_list_.front ());
163 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 167 feature_map_.clear ();
168 std::queue<std::pair<int, int> > empty;
169 std::swap (key_list_, empty);
171 pfh_histogram_.setZero (nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
175 std::vector<int> nn_indices (k_);
176 std::vector<float> nn_dists (k_);
180 if (input_->is_dense)
183 for (
size_t idx = 0; idx < indices_->size (); ++idx)
185 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
187 for (
int d = 0; d < pfh_histogram_.size (); ++d)
188 output.
points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
195 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
198 for (
int d = 0; d < pfh_histogram_.size (); ++d)
199 output.
points[idx].histogram[d] = pfh_histogram_[d];
205 for (
size_t idx = 0; idx < indices_->size (); ++idx)
207 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
208 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
210 for (
int d = 0; d < pfh_histogram_.size (); ++d)
211 output.
points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
218 computePointPFHSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfh_histogram_);
221 for (
int d = 0; d < pfh_histogram_.size (); ++d)
222 output.
points[idx].histogram[d] = pfh_histogram_[d];
227 #define PCL_INSTANTIATE_PFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHEstimation<T,NT,OutT>; 229 #endif // PCL_FEATURES_IMPL_PFH_H_ bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void computeFeature(PointCloudOut &output)
Estimate the Point Feature Histograms (PFH) descriptors at a set of points given by <setInputCloud ()...
bool computePairFeatures(const pcl::PointCloud< PointInT > &cloud, const pcl::PointCloud< PointNT > &normals, int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4)
Compute the 4-tuple representation containing the three angles and one distance between two points re...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void computePointPFHSignature(const pcl::PointCloud< PointInT > &cloud, const pcl::PointCloud< PointNT > &normals, const std::vector< int > &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
Estimate the PFH (Point Feature Histograms) individual signatures of the three angular (f1...
PCL_EXPORTS bool computePairFeatures(const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, float &f1, float &f2, float &f3, float &f4)
Compute the 4-tuple representation containing the three angles and one distance between two points re...