template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Check if input was set
if (!normals_)
{
PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (normals_->points.size () != surface_->points.size ())
{
PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
centroids_dominant_orientations_.clear ();
// ---[ Step 0: remove normals with high curvature
std::vector<int> indices_out;
std::vector<int> indices_in;
filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
pcl::PointCloud<pcl::PointNormal>::Ptr normals_filtered_cloud (new pcl::PointCloud<pcl::PointNormal> ());
normals_filtered_cloud->width = static_cast<uint32_t> (indices_in.size ());
normals_filtered_cloud->height = 1;
normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
for (size_t i = 0; i < indices_in.size (); ++i)
{
normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
}
std::vector<pcl::PointIndices> clusters;
if(normals_filtered_cloud->points.size() >= min_points_)
{
//recompute normals and use them for clustering
KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
normals_tree_filtered->setInputCloud (normals_filtered_cloud);
NormalEstimator n3d;
n3d.setRadiusSearch (radius_normals_);
n3d.setSearchMethod (normals_tree_filtered);
n3d.setInputCloud (normals_filtered_cloud);
n3d.compute (*normals_filtered_cloud);
KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
normals_tree->setInputCloud (normals_filtered_cloud);
extractEuclideanClustersSmooth (*normals_filtered_cloud,
*normals_filtered_cloud,
cluster_tolerance_,
normals_tree,
clusters,
eps_angle_threshold_,
static_cast<unsigned int> (min_points_));
}
VFHEstimator vfh;
vfh.setInputCloud (surface_);
vfh.setInputNormals (normals_);
vfh.setIndices(indices_);
vfh.setSearchMethod (this->tree_);
vfh.setUseGivenNormal (true);
vfh.setUseGivenCentroid (true);
vfh.setNormalizeBins (normalize_bins_);
vfh.setNormalizeDistance (true);
vfh.setFillSizeComponent (true);
output.height = 1;
// ---[ Step 1b : check if any dominant cluster was found
if (clusters.size () > 0)
{ // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information
for (size_t i = 0; i < clusters.size (); ++i) //for each cluster
{
Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
for (size_t j = 0; j < clusters[i].indices.size (); j++)
{
avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
}
avg_normal /= static_cast<float> (clusters[i].indices.size ());
avg_centroid /= static_cast<float> (clusters[i].indices.size ());
Eigen::Vector4f centroid_test;
pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test);
avg_normal.normalize ();
Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
//append normal and centroid for the clusters
dominant_normals_.push_back (avg_norm);
centroids_dominant_orientations_.push_back (avg_dominant_centroid);
}
//compute modified VFH for all dominant clusters and add them to the list!
output.points.resize (dominant_normals_.size ());
output.width = static_cast<uint32_t> (dominant_normals_.size ());
for (size_t i = 0; i < dominant_normals_.size (); ++i)
{
//configure VFH computation for CVFH
vfh.setNormalToUse (dominant_normals_[i]);
vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
vfh.compute (vfh_signature);
output.points[i] = vfh_signature.points[0];
}
}
else
{ // ---[ Step 1b.1 : If no, compute CVFH using all the object points
Eigen::Vector4f avg_centroid;
pcl::compute3DCentroid (*surface_, avg_centroid);
Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
centroids_dominant_orientations_.push_back (cloud_centroid);
//configure VFH computation for CVFH using all object points
vfh.setCentroidToUse (cloud_centroid);
vfh.setUseGivenNormal (false);
pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
vfh.compute (vfh_signature);
output.points.resize (1);
output.width = 1;
output.points[0] = vfh_signature.points[0];
}
}
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
if (refine_clusters_ <= 0.f)
refine_clusters_ = 1.f;
// Check if input was set
if (!normals_)
{
PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
if (normals_->points.size () != surface_->points.size ())
{
PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.points.clear ();
return;
}
centroids_dominant_orientations_.clear ();
clusters_.clear ();
transforms_.clear ();
dominant_normals_.clear ();
// ---[ Step 0: remove normals with high curvature
std::vector<int> indices_out;
std::vector<int> indices_in;
filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
pcl::PointCloud<pcl::PointNormal>::Ptr normals_filtered_cloud (new pcl::PointCloud<pcl::PointNormal> ());
normals_filtered_cloud->width = static_cast<uint32_t> (indices_in.size ());
normals_filtered_cloud->height = 1;
normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
std::vector<int> indices_from_nfc_to_indices;
indices_from_nfc_to_indices.resize (indices_in.size ());
for (size_t i = 0; i < indices_in.size (); ++i)
{
normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
//normals_filtered_cloud->points[i].getNormalVector4fMap() = normals_->points[indices_in[i]].getNormalVector4fMap();
indices_from_nfc_to_indices[i] = indices_in[i];
}
std::vector<pcl::PointIndices> clusters;
if (normals_filtered_cloud->points.size () >= min_points_)
{
//recompute normals and use them for clustering
{
KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
normals_tree_filtered->setInputCloud (normals_filtered_cloud);
pcl::NormalEstimation<PointNormal, PointNormal> n3d;
n3d.setRadiusSearch (radius_normals_);
n3d.setSearchMethod (normals_tree_filtered);
n3d.setInputCloud (normals_filtered_cloud);
n3d.compute (*normals_filtered_cloud);
}
KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
normals_tree->setInputCloud (normals_filtered_cloud);
extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
eps_angle_threshold_, static_cast<unsigned int> (min_points_));
std::vector<pcl::PointIndices> clusters_filtered;
int cluster_filtered_idx = 0;
for (size_t i = 0; i < clusters.size (); i++)
{
pcl::PointIndices pi;
pcl::PointIndices pi_cvfh;
pcl::PointIndices pi_filtered;
clusters_.push_back (pi);
clusters_filtered.push_back (pi_filtered);
Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
for (size_t j = 0; j < clusters[i].indices.size (); j++)
{
avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
}
avg_normal /= static_cast<float> (clusters[i].indices.size ());
avg_centroid /= static_cast<float> (clusters[i].indices.size ());
avg_normal.normalize ();
Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
for (size_t j = 0; j < clusters[i].indices.size (); j++)
{
//decide if normal should be added
double dot_p = avg_normal.dot (normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ());
if (fabs (acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
{
clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[clusters[i].indices[j]]);
clusters_filtered[cluster_filtered_idx].indices.push_back (clusters[i].indices[j]);
}
}
//remove last cluster if no points found...
if (clusters_[cluster_filtered_idx].indices.size () == 0)
{
clusters_.erase (clusters_.end ());
clusters_filtered.erase (clusters_filtered.end ());
}
else
cluster_filtered_idx++;
}
clusters = clusters_filtered;
}
pcl::VFHEstimation<PointInT, PointNT, pcl::VFHSignature308> vfh;
vfh.setInputCloud (surface_);
vfh.setInputNormals (normals_);
vfh.setIndices (indices_);
vfh.setSearchMethod (this->tree_);
vfh.setUseGivenNormal (true);
vfh.setUseGivenCentroid (true);
vfh.setNormalizeBins (normalize_bins_);
output.height = 1;
// ---[ Step 1b : check if any dominant cluster was found
if (clusters.size () > 0)
{ // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information
for (size_t i = 0; i < clusters.size (); ++i) //for each cluster
{
Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
for (size_t j = 0; j < clusters[i].indices.size (); j++)
{
avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
}
avg_normal /= static_cast<float> (clusters[i].indices.size ());
avg_centroid /= static_cast<float> (clusters[i].indices.size ());
avg_normal.normalize ();
Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
//append normal and centroid for the clusters
dominant_normals_.push_back (avg_norm);
centroids_dominant_orientations_.push_back (avg_dominant_centroid);
}
//compute modified VFH for all dominant clusters and add them to the list!
output.points.resize (dominant_normals_.size ());
output.width = static_cast<uint32_t> (dominant_normals_.size ());
for (size_t i = 0; i < dominant_normals_.size (); ++i)
{
//configure VFH computation for CVFH
vfh.setNormalToUse (dominant_normals_[i]);
vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
vfh.compute (vfh_signature);
output.points[i] = vfh_signature.points[0];
}
//finish filling the descriptor with the shape distribution
PointInTPtr cloud_input (new pcl::PointCloud<PointInT>);
pcl::copyPointCloud (*surface_, *indices_, *cloud_input);
computeRFAndShapeDistribution (cloud_input, output, clusters_); //this will set transforms_
}
else
{ // ---[ Step 1b.1 : If no, compute a VFH using all the object points
PCL_WARN("No clusters were found in the surface... using VFH...\n");
Eigen::Vector4f avg_centroid;
pcl::compute3DCentroid (*surface_, avg_centroid);
Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
centroids_dominant_orientations_.push_back (cloud_centroid);
//configure VFH computation using all object points
vfh.setCentroidToUse (cloud_centroid);
vfh.setUseGivenNormal (false);
pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
vfh.compute (vfh_signature);
output.points.resize (1);
output.width = 1;
output.points[0] = vfh_signature.points[0];
Eigen::Matrix4f id = Eigen::Matrix4f::Identity ();
transforms_.push_back (id);
valid_transforms_.push_back (false);
}
}
