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); } }