template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeSPFHSignatures (std::vector<int> &spfh_hist_lookup,
Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
{
// Allocate enough space to hold the NN search results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
std::set<int> spfh_indices;
spfh_hist_lookup.resize (surface_->points.size ());
// Build a list of (unique) indices for which we will need to compute SPFH signatures
// (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_])
if (surface_ != input_ ||
indices_->size () != surface_->points.size ())
{
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
int p_idx = (*indices_)[idx];
if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;
spfh_indices.insert (nn_indices.begin (), nn_indices.end ());
}
}
else
{
// Special case: When a feature must be computed at every point, there is no need for a neighborhood search
for (size_t idx = 0; idx < indices_->size (); ++idx)
spfh_indices.insert (static_cast<int> (idx));
}
// Initialize the arrays that will store the SPFH signatures
size_t data_size = spfh_indices.size ();
hist_f1.setZero (data_size, nr_bins_f1_);
hist_f2.setZero (data_size, nr_bins_f2_);
hist_f3.setZero (data_size, nr_bins_f3_);
// Compute SPFH signatures for every point that needs them
std::set<int>::iterator spfh_indices_itr = spfh_indices.begin ();
for (int i = 0; i < static_cast<int> (spfh_indices.size ()); ++i)
{
// Get the next point index
int p_idx = *spfh_indices_itr;
++spfh_indices_itr;
// Find the neighborhood around p_idx
if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;
// Estimate the SPFH signature around p_idx
computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1, hist_f2, hist_f3);
// Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices
spfh_hist_lookup[p_idx] = i;
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Check if input was set
if (!normals_)
{
ROS_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (normals_->points.size () != surface_->points.size ())
{
ROS_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
size_t data_size = indices_->size ();
// Reset the whole thing
hist_f1_.setZero (data_size, nr_bins_f1_);
hist_f2_.setZero (data_size, nr_bins_f2_);
hist_f3_.setZero (data_size, nr_bins_f3_);
int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
// Iterating over the entire index vector
for (size_t idx = 0; idx < data_size; ++idx)
{
int p_idx = (*indices_)[idx];
searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);
// Estimate the FPFH signature at each patch
computePointSPFHSignature (*surface_, *normals_, p_idx, nn_indices,
hist_f1_, hist_f2_, hist_f3_);
}
fpfh_histogram_.setZero (nr_bins);
// Iterating over the entire index vector
for (size_t idx = 0; idx < data_size; ++idx)
{
searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// Copy into the resultant cloud
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
fpfh_histogram_.setZero ();
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// ---[ Step 1a : compute the centroid in XYZ space
Eigen::Vector4f xyz_centroid;
if (use_given_centroid_)
xyz_centroid = centroid_to_use_;
else
compute3DCentroid (*surface_, *indices_, xyz_centroid); // Estimate the XYZ centroid
// ---[ Step 1b : compute the centroid in normal space
Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
int cp = 0;
// If the data is dense, we don't need to check for NaN
if (use_given_normal_)
normal_centroid = normal_to_use_;
else
{
if (normals_->is_dense)
{
for (size_t i = 0; i < indices_->size (); ++i)
{
normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
cp++;
}
}
// NaN or Inf values could exist => check for them
else
{
for (size_t i = 0; i < indices_->size (); ++i)
{
if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
||
!pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
||
!pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
continue;
normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
cp++;
}
}
normal_centroid /= cp;
}
// Compute the direction of view from the viewpoint to the centroid
Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
d_vp_p.normalize ();
// Estimate the SPFH at nn_indices[0] using the entire cloud
computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
// We only output _1_ signature
output.points.resize (1);
output.width = 1;
output.height = 1;
// Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature
for (int d = 0; d < hist_f1_.size (); ++d)
output.points[0].histogram[d + 0] = hist_f1_[d];
size_t data_size = hist_f1_.size ();
for (int d = 0; d < hist_f2_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f2_[d];
data_size += hist_f2_.size ();
for (int d = 0; d < hist_f3_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f3_[d];
data_size += hist_f3_.size ();
for (int d = 0; d < hist_f4_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f4_[d];
// ---[ Step 2 : obtain the viewpoint component
hist_vp_.setZero (nr_bins_vp_);
double hist_incr;
if (normalize_bins_)
hist_incr = 100.0 / (double)(indices_->size ());
else
hist_incr = 1.0;
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
normals_->points[(*indices_)[i]].normal[1],
normals_->points[(*indices_)[i]].normal[2], 0);
// Normalize
double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
int fi = floor (alpha * hist_vp_.size ());
if (fi < 0)
fi = 0;
if (fi > ((int)hist_vp_.size () - 1))
fi = hist_vp_.size () - 1;
// Bin into the histogram
hist_vp_ [fi] += hist_incr;
}
data_size += hist_f4_.size ();
// Copy the resultant signature
for (int d = 0; d < hist_vp_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_vp_[d];
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Allocate enough space to hold the NN search results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
std::set<int> spfh_indices;
std::vector<int> spfh_hist_lookup (surface_->points.size ());
// Build a list of (unique) indices for which we will need to compute SPFH signatures
// (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_])
if (surface_ != input_ ||
indices_->size () != surface_->points.size ())
{
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
int p_idx = (*indices_)[idx];
this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);
spfh_indices.insert (nn_indices.begin (), nn_indices.end ());
}
}
else
{
// Special case: When a feature must be computed at every point, there is no need for a neighborhood search
for (size_t idx = 0; idx < indices_->size (); ++idx)
spfh_indices.insert (idx);
}
// Initialize the arrays that will store the SPFH signatures
size_t data_size = spfh_indices.size ();
hist_f1_.setZero (data_size, nr_bins_f1_);
hist_f2_.setZero (data_size, nr_bins_f2_);
hist_f3_.setZero (data_size, nr_bins_f3_);
// Compute SPFH signatures for every point that needs them
std::set<int>::iterator spfh_indices_itr = spfh_indices.begin ();
for (size_t i = 0; i < spfh_indices.size (); ++i)
{
// Get the next point index
int p_idx = *spfh_indices_itr;
++spfh_indices_itr;
// Find the neighborhood around p_idx
this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);
// Estimate the SPFH signature around p_idx
computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_);
// Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices
spfh_hist_lookup[p_idx] = i;
}
// Intialize the array that will store the FPFH signature
int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
fpfh_histogram_.setZero (nr_bins);
// Iterate over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
// Find the indices of point idx's neighbors...
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
// ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
// instead of indices into surface_->points
for (size_t i = 0; i < nn_indices.size (); ++i)
nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
// Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// ...and copy it into the output cloud
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
fpfh_histogram_.setZero ();
}
}
