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>
50template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
53 if (input_->size () < 2 || (surface_ && surface_->size () < 2))
55 PCL_ERROR (
"[pcl::VFHEstimation::initCompute] Input dataset must have at least 2 points!\n");
58 if (search_radius_ == 0 && k_ == 0)
64template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
69 output.width = output.height = 0;
74 output.header = input_->header;
80 output.width = output.height = 1;
81 output.is_dense = input_->is_dense;
85 computeFeature (output);
91template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
93 const Eigen::Vector4f ¢roid_n,
98 Eigen::Vector4f pfh_tuple;
100 for (
int i = 0; i < 4; ++i)
102 hist_f_[i].setZero (nr_bins_f_[i]);
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);
128 float hist_incr_size_component = 0;;
130 hist_incr_size_component = hist_incr;
133 for (
const auto &index : indices)
137 normals[index].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1],
138 pfh_tuple[2], pfh_tuple[3]))
142 for (
int i = 0; i < 3; ++i)
144 const int raw_index =
static_cast<int> (std::floor (nr_bins_f_[i] * ((pfh_tuple[i] +
M_PI) * d_pi_)));
145 const int h_index = std::max(std::min(raw_index, nr_bins_f_[i] - 1), 0);
146 hist_f_[i] (h_index) += hist_incr;
149 if (hist_incr_size_component)
152 if (normalize_distances_)
153 h_index =
static_cast<int> (std::floor (nr_bins_f_[3] * (pfh_tuple[3] / distance_normalization_factor)));
155 h_index =
static_cast<int> (
pcl_round (pfh_tuple[3] * 100));
157 h_index = std::max (std::min (h_index, nr_bins_f_[3] - 1), 0);
158 hist_f_[3] (h_index) += hist_incr_size_component;
163template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
167 Eigen::Vector4f xyz_centroid (0, 0, 0, 0);
169 if (use_given_centroid_)
170 xyz_centroid = centroid_to_use_;
175 Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
178 if (use_given_normal_)
179 normal_centroid = normal_to_use_;
183 if (normals_->is_dense)
185 for (
const auto& index: *indices_)
187 normal_centroid.noalias () += (*normals_)[index].getNormalVector4fMap ();
189 cp = indices_->size();
194 for (
const auto& index: *indices_)
196 if (!std::isfinite ((*normals_)[index].normal[0]) ||
197 !std::isfinite ((*normals_)[index].normal[1]) ||
198 !std::isfinite ((*normals_)[index].normal[2]))
200 normal_centroid.noalias () += (*normals_)[index].getNormalVector4fMap ();
204 normal_centroid /=
static_cast<float> (
cp);
208 Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
209 Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
213 computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
216 hist_vp_.setZero (nr_bins_vp_);
218 float hist_incr = 1.0;
220 hist_incr = 100.0 /
static_cast<double> (indices_->size ());
222 for (
const auto& index: *indices_)
224 Eigen::Vector4f normal ((*normals_)[index].normal[0],
225 (*normals_)[index].normal[1],
226 (*normals_)[index].normal[2], 0);
228 double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
229 std::size_t fi =
static_cast<std::size_t
> (std::floor (alpha * hist_vp_.size ()));
230 fi = std::max<std::size_t> (0u, fi);
231 fi = std::min<std::size_t> (hist_vp_.size () - 1, fi);
233 hist_vp_ [fi] += hist_incr;
242 auto outPtr = std::begin (output[0].histogram);
244 for (
int i = 0; i < 4; ++i)
246 outPtr = std::copy_n (hist_f_[i].data (), hist_f_[i].size (), outPtr);
248 outPtr = std::copy_n (hist_vp_.data (), hist_vp_.size (), outPtr);
251#define PCL_INSTANTIATE_VFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::VFHEstimation<T,NT,OutT>;
Define methods for centroid estimation and covariance matrix calculus.
Feature represents the base feature class.
PointCloud represents the base class in PCL for storing collections of 3D points.
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
bool initCompute() override
This method should get called before starting the actual computation.
void computePointSPFHSignature(const Eigen::Vector4f ¢roid_p, const Eigen::Vector4f ¢roid_n, const pcl::PointCloud< PointInT > &cloud, const pcl::PointCloud< PointNT > &normals, const pcl::Indices &indices)
Estimate the SPFH (Simple Point Feature Histograms) signatures of the angular (f1,...
Define standard C methods and C++ classes that are common to all methods.
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.
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.
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...
int cp(int from, int to)
Returns field copy operation code.
IndicesAllocator<> Indices
Type used for indices in PCL.
__inline double pcl_round(double number)
Win32 doesn't seem to have rounding functions.