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
74 output.header = input_->header;
81 output.is_dense = input_->is_dense;
91template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
100 for (
int i = 0; i < 4; ++i)
102 hist_f_[i].setZero (nr_bins_f_[i]);
115 if (normalize_distances_)
126 hist_incr = 100.0f /
static_cast<float> (indices.size () - 1);
133 for (
const auto &index : indices)
142 for (
int i = 0; i < 3; ++i)
152 if (normalize_distances_)
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.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
Feature represents the base feature class.
virtual bool deinitCompute()
This method should get called after ending the actual computation.
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.