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::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::vector<int> spfh_hist_lookup;
computeSPFHSignatures (spfh_hist_lookup, hist_f1_, hist_f2_, hist_f3_);
output.is_dense = true;
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
if (input_->is_dense)
{
// Iterate over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// ... 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];
}
}
else
{
// Iterate over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// ... 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];
}
}
}
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 ();
}
}
