41 #ifndef PCL_FEATURES_IMPL_VFH_H_
42 #define PCL_FEATURES_IMPL_VFH_H_
44 #include <pcl/features/vfh.h>
45 #include <pcl/features/pfh_tools.h>
46 #include <pcl/common/common.h>
47 #include <pcl/common/centroid.h>
50 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
53 if (input_->points.size () < 2 || (surface_ && surface_->points.size () < 2))
55 PCL_ERROR (
"[pcl::VFHEstimation::initCompute] Input dataset must have at least 2 points!\n");
58 if (search_radius_ == 0 && k_ == 0)
64 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
74 output.
header = input_->header;
85 computeFeature (output);
91 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
93 const Eigen::Vector4f ¢roid_n,
96 const std::vector<int> &indices)
98 Eigen::Vector4f pfh_tuple;
100 hist_f1_.setZero (nr_bins_f1_);
101 hist_f2_.setZero (nr_bins_f2_);
102 hist_f3_.setZero (nr_bins_f3_);
103 hist_f4_.setZero (nr_bins_f4_);
114 double distance_normalization_factor = 1.0;
115 if (normalize_distances_)
117 Eigen::Vector4f max_pt;
120 distance_normalization_factor = (centroid_p - max_pt).norm ();
126 hist_incr = 100.0f /
static_cast<float> (indices.size () - 1);
130 float hist_incr_size_component;
132 hist_incr_size_component = hist_incr;
134 hist_incr_size_component = 0.0;
137 for (
size_t idx = 0; idx < indices.size (); ++idx)
141 normals.
points[indices[idx]].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1],
142 pfh_tuple[2], pfh_tuple[3]))
146 int h_index =
static_cast<int> (floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_)));
149 if (h_index >= nr_bins_f1_)
150 h_index = nr_bins_f1_ - 1;
151 hist_f1_ (h_index) += hist_incr;
153 h_index =
static_cast<int> (floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5)));
156 if (h_index >= nr_bins_f2_)
157 h_index = nr_bins_f2_ - 1;
158 hist_f2_ (h_index) += hist_incr;
160 h_index =
static_cast<int> (floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5)));
163 if (h_index >= nr_bins_f3_)
164 h_index = nr_bins_f3_ - 1;
165 hist_f3_ (h_index) += hist_incr;
167 if (normalize_distances_)
168 h_index =
static_cast<int> (floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor)));
170 h_index =
static_cast<int> (pcl_round (pfh_tuple[3] * 100));
174 if (h_index >= nr_bins_f4_)
175 h_index = nr_bins_f4_ - 1;
177 hist_f4_ (h_index) += hist_incr_size_component;
181 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
185 Eigen::Vector4f xyz_centroid;
187 if (use_given_centroid_)
188 xyz_centroid = centroid_to_use_;
193 Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
197 if (use_given_normal_)
198 normal_centroid = normal_to_use_;
201 if (normals_->is_dense)
203 for (
size_t i = 0; i < indices_->size (); ++i)
205 normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
212 for (
size_t i = 0; i < indices_->size (); ++i)
214 if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
216 !pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
218 !pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
220 normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
224 normal_centroid /=
static_cast<float> (cp);
228 Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
229 Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
233 computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
236 output.points.resize (1);
241 for (
int d = 0; d < hist_f1_.size (); ++d)
242 output.points[0].histogram[d + 0] = hist_f1_[d];
244 size_t data_size = hist_f1_.size ();
245 for (
int d = 0; d < hist_f2_.size (); ++d)
246 output.points[0].histogram[d + data_size] = hist_f2_[d];
248 data_size += hist_f2_.size ();
249 for (
int d = 0; d < hist_f3_.size (); ++d)
250 output.points[0].histogram[d + data_size] = hist_f3_[d];
252 data_size += hist_f3_.size ();
253 for (
int d = 0; d < hist_f4_.size (); ++d)
254 output.points[0].histogram[d + data_size] = hist_f4_[d];
257 hist_vp_.setZero (nr_bins_vp_);
261 hist_incr = 100.0 /
static_cast<double> (indices_->size ());
265 for (
size_t i = 0; i < indices_->size (); ++i)
267 Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
268 normals_->points[(*indices_)[i]].normal[1],
269 normals_->points[(*indices_)[i]].normal[2], 0);
271 double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
272 int fi =
static_cast<int> (floor (alpha * static_cast<double> (hist_vp_.size ())));
275 if (fi > (static_cast<int> (hist_vp_.size ()) - 1))
276 fi = static_cast<int> (hist_vp_.size ()) - 1;
278 hist_vp_ [fi] +=
static_cast<float> (hist_incr);
280 data_size += hist_f4_.size ();
282 for (
int d = 0; d < hist_vp_.size (); ++d)
283 output.points[0].histogram[d + data_size] = hist_vp_[d];
286 #define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation<T,NT,OutT>;
288 #endif // PCL_FEATURES_IMPL_VFH_H_
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Feature represents the base feature class.
bool initCompute()
This method should get called before starting the actual computation.
pcl::PCLHeader header
The point cloud header.
void computePointSPFHSignature(const Eigen::Vector4f ¢roid_p, const Eigen::Vector4f ¢roid_n, const pcl::PointCloud< PointInT > &cloud, const pcl::PointCloud< PointNT > &normals, const std::vector< int > &indices)
Estimate the SPFH (Simple Point Feature Histograms) signatures of the angular (f1, f2, f3) and distance (f4) features for a given point from its neighborhood.
uint32_t width
The point cloud width (if organized as an image-structure).
uint32_t height
The point cloud height (if organized as an image-structure).
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void getMaxDistance(const pcl::PointCloud< PointT > &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
Get the point at maximum distance from a given point and a given pointcloud.
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...
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.