template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::VFHEstimation<PointInT, PointNT, PointOutT>::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)
{
Eigen::Vector4f pfh_tuple;
// Reset the whole thing
hist_f1_.setZero (nr_bins_f1_);
hist_f2_.setZero (nr_bins_f2_);
hist_f3_.setZero (nr_bins_f3_);
hist_f4_.setZero (nr_bins_f4_);
// Get the bounding box of the current cluster
Eigen::Vector4f min_pt, max_pt;
pcl::getMinMax3D (cloud, indices, min_pt, max_pt);
// Estimate the largest distance
double distance_normalization_factor = (std::max)((centroid_p - min_pt).norm (), (centroid_p - max_pt).norm ());
// Factorization constant
float hist_incr = 100.0 / (float)(indices.size () - 1);
// Iterate over all the points in the neighborhood
for (size_t idx = 0; idx < indices.size (); ++idx)
{
// Compute the pair P to NNi
if (!computePairFeatures (
centroid_p, centroid_n,
cloud.points[indices[idx]].getVector4fMap (), normals.points[indices[idx]].getNormalVector4fMap (),
pfh_tuple[0], pfh_tuple[1], pfh_tuple[2], pfh_tuple[3]))
continue;
// Normalize the f1, f2, f3, f4 features and push them in the histogram
int h_index = floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_));
if (h_index < 0) h_index = 0;
if (h_index >= nr_bins_f1_) h_index = nr_bins_f1_ - 1;
hist_f1_ (h_index) += hist_incr;
h_index = floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5));
if (h_index < 0) h_index = 0;
if (h_index >= nr_bins_f2_) h_index = nr_bins_f2_ - 1;
hist_f2_ (h_index) += hist_incr;
h_index = floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5));
if (h_index < 0) h_index = 0;
if (h_index >= nr_bins_f3_) h_index = nr_bins_f3_ - 1;
hist_f3_ (h_index) += hist_incr;
h_index = floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor));
if (h_index < 0) h_index = 0;
if (h_index >= nr_bins_f4_) h_index = nr_bins_f4_ - 1;
hist_f4_ (h_index) += hist_incr;
}
}
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::VFHEstimation<PointInT, PointNT, PointOutT>::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)
{
Eigen::Vector4f pfh_tuple;
// Reset the whole thing
hist_f1_.setZero (nr_bins_f1_);
hist_f2_.setZero (nr_bins_f2_);
hist_f3_.setZero (nr_bins_f3_);
hist_f4_.setZero (nr_bins_f4_);
// Get the bounding box of the current cluster
//Eigen::Vector4f min_pt, max_pt;
//pcl::getMinMax3D (cloud, indices, min_pt, max_pt);
//double distance_normalization_factor = (std::max)((centroid_p - min_pt).norm (), (centroid_p - max_pt).norm ());
//Instead of using the bounding box to normalize the VFH distance component, it is better to use the max_distance
//from any point to centroid. VFH is invariant to rotation about the roll axis but the bounding box is not,
//resulting in different normalization factors for point clouds that are just rotated about that axis.
double distance_normalization_factor = 1.0;
if ( normalize_distances_ ) {
Eigen::Vector4f max_pt;
pcl::getMaxDistance (cloud, indices, centroid_p, max_pt);
distance_normalization_factor = (centroid_p - max_pt).norm ();
}
// Factorization constant
float hist_incr;
if (normalize_bins_) {
hist_incr = 100.0 / (float)(indices.size () - 1);
} else {
hist_incr = 1.0;
}
float hist_incr_size_component;
if (size_component_)
hist_incr_size_component = hist_incr;
else
hist_incr_size_component = 0.0;
// Iterate over all the points in the neighborhood
for (size_t idx = 0; idx < indices.size (); ++idx)
{
// Compute the pair P to NNi
if (!computePairFeatures (centroid_p, centroid_n, cloud.points[indices[idx]].getVector4fMap (),
normals.points[indices[idx]].getNormalVector4fMap (), pfh_tuple[0], pfh_tuple[1],
pfh_tuple[2], pfh_tuple[3]))
continue;
// Normalize the f1, f2, f3, f4 features and push them in the histogram
int h_index = floor (nr_bins_f1_ * ((pfh_tuple[0] + M_PI) * d_pi_));
if (h_index < 0)
h_index = 0;
if (h_index >= nr_bins_f1_)
h_index = nr_bins_f1_ - 1;
hist_f1_ (h_index) += hist_incr;
h_index = floor (nr_bins_f2_ * ((pfh_tuple[1] + 1.0) * 0.5));
if (h_index < 0)
h_index = 0;
if (h_index >= nr_bins_f2_)
h_index = nr_bins_f2_ - 1;
hist_f2_ (h_index) += hist_incr;
h_index = floor (nr_bins_f3_ * ((pfh_tuple[2] + 1.0) * 0.5));
if (h_index < 0)
h_index = 0;
if (h_index >= nr_bins_f3_)
h_index = nr_bins_f3_ - 1;
hist_f3_ (h_index) += hist_incr;
if (normalize_distances_)
h_index = floor (nr_bins_f4_ * (pfh_tuple[3] / distance_normalization_factor));
else
h_index = pcl_round (pfh_tuple[3] * 100);
if (h_index < 0)
h_index = 0;
if (h_index >= nr_bins_f4_)
h_index = nr_bins_f4_ - 1;
hist_f4_ (h_index) += hist_incr_size_component;
}
}
