71 float hist_incr = 100.0f /
static_cast<float> (indices.size () * (indices.size () - 1) / 2);
73 std::pair<int, int> key;
99 key = std::pair<int, int> (p1, p2);
102 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);
103 if (
fm_it != feature_map_.end ())
105 pfh_tuple_ =
fm_it->second;
112 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
120 pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
124 f_index_[0] =
static_cast<int> (std::floor (
nr_split * ((pfh_tuple_[0] +
M_PI) * d_pi_)));
125 if (f_index_[0] < 0) f_index_[0] = 0;
128 f_index_[1] =
static_cast<int> (std::floor (
nr_split * ((pfh_tuple_[1] + 1.0) * 0.5)));
129 if (f_index_[1] < 0) f_index_[1] = 0;
132 f_index_[2] =
static_cast<int> (std::floor (
nr_split * ((pfh_tuple_[2] + 1.0) * 0.5)));
133 if (f_index_[2] < 0) f_index_[2] = 0;
139 for (
const int &d : f_index_)
149 feature_map_[key] = pfh_tuple_;
152 key_list_.push (key);
154 if (key_list_.size () > max_cache_size_)
157 feature_map_.erase (key_list_.front ());
170 feature_map_.clear ();
171 std::queue<std::pair<int, int> > empty;
172 std::swap (key_list_, empty);
174 pfh_histogram_.setZero (nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
183 if (input_->is_dense)
186 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
188 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_,
nn_indices,
nn_dists) == 0)
190 for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d)
191 output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
198 computePointPFHSignature (*surface_, *normals_,
nn_indices, nr_subdiv_, pfh_histogram_);
201 for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d)
202 output[idx].histogram[d] = pfh_histogram_[d];
208 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
210 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
211 this->searchForNeighbors ((*indices_)[idx], search_parameter_,
nn_indices,
nn_dists) == 0)
213 for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d)
214 output[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
221 computePointPFHSignature (*surface_, *normals_,
nn_indices, nr_subdiv_, pfh_histogram_);
224 for (Eigen::Index d = 0; d < pfh_histogram_.size (); ++d)
225 output[idx].histogram[d] = pfh_histogram_[d];
void computePointPFHSignature(const pcl::PointCloud< PointInT > &cloud, const pcl::PointCloud< PointNT > &normals, const pcl::Indices &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
Estimate the PFH (Point Feature Histograms) individual signatures of the three angular (f1,...
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...
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...