41#ifndef PCL_FEATURES_IMPL_OURCVFH_H_
42#define PCL_FEATURES_IMPL_OURCVFH_H_
44#include <pcl/features/our_cvfh.h>
45#include <pcl/features/vfh.h>
46#include <pcl/features/normal_3d.h>
47#include <pcl/common/io.h>
49#include <pcl/common/transforms.h>
52template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
75template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
84 if (tree->getInputCloud ()->size () != cloud.
size ())
86 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
87 "dataset (%zu) than the input cloud (%zu)!\n",
88 static_cast<std::size_t
>(tree->getInputCloud()->size()),
89 static_cast<std::size_t
>(cloud.
size()));
92 if (cloud.
size () != normals.
size ())
94 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
95 "cloud (%zu) different than normals (%zu)!\n",
96 static_cast<std::size_t
>(cloud.
size()),
97 static_cast<std::size_t
>(normals.
size()));
102 std::vector<bool> processed (cloud.
size (),
false);
105 std::vector<float> nn_distances;
107 for (std::size_t i = 0; i < cloud.
size (); ++i)
112 std::vector<std::size_t> seed_queue;
113 std::size_t sq_idx = 0;
114 seed_queue.push_back (i);
118 while (sq_idx < seed_queue.size ())
121 if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
127 for (std::size_t j = 1; j < nn_indices.
size (); ++j)
129 if (processed[nn_indices[j]])
135 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
136 + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] + normals[seed_queue[sq_idx]].normal[2]
137 * normals[nn_indices[j]].normal[2];
139 if (std::abs (std::acos (dot_p)) < eps_angle)
141 processed[nn_indices[j]] =
true;
142 seed_queue.push_back (nn_indices[j]);
150 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
153 r.
indices.resize (seed_queue.size ());
154 for (std::size_t j = 0; j < seed_queue.size (); ++j)
161 clusters.push_back (r);
167template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
181 if (cloud[index].curvature > threshold)
197template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
203 Eigen::Vector3f plane_normal;
204 plane_normal[0] = -centroid[0];
205 plane_normal[1] = -centroid[1];
206 plane_normal[2] = -centroid[2];
207 Eigen::Vector3f
z_vector = Eigen::Vector3f::UnitZ ();
208 plane_normal.normalize ();
217 (*
grid)[k].getVector4fMap () = (*processed)[k].getVector4fMap ();
221 Eigen::Vector4f
centroid4f (centroid[0], centroid[1], centroid[2], 0);
222 Eigen::Vector4f
normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
247 for (
const auto &index : indices.
indices)
249 Eigen::Vector3f
pvector = (*grid)[index].getVector3fMap ();
260 Eigen::JacobiSVD <Eigen::MatrixXf>
svd (
scatter, Eigen::ComputeFullV);
261 Eigen::Vector3f
evx =
svd.matrixV ().col (0);
262 Eigen::Vector3f
evy =
svd.matrixV ().col (1);
263 Eigen::Vector3f
evz =
svd.matrixV ().col (2);
272 for (
const auto& point:
grid->points)
274 Eigen::Vector3f
pvector = point.getVector3fMap ();
319 PCL_WARN (
"Both axes are equally easy/difficult to disambiguate\n");
380template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
386 cluster_axes_.clear ();
387 cluster_axes_.resize (centroids_dominant_orientations_.size ());
389 for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
392 std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >
transformations;
403 transforms_.push_back (transformation);
404 valid_transforms_.push_back (
true);
406 std::vector < Eigen::VectorXf >
quadrants (8);
429 for (
const auto& point:
grid->points)
431 Eigen::Vector4f p = point.getVector4fMap ();
436 float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f *
sigma_sq)));
437 float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f *
sigma_sq)));
438 float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f *
sigma_sq)));
443 for (std::size_t
ii = 0;
ii <= 3;
ii++)
444 weights[
ii] = 0.5f -
wx * 0.5f;
446 for (std::size_t
ii = 4;
ii <= 7;
ii++)
447 weights[
ii] = 0.5f +
wx * 0.5f;
451 for (std::size_t
ii = 0;
ii <= 3;
ii++)
452 weights[
ii] = 0.5f +
wx * 0.5f;
454 for (std::size_t
ii = 4;
ii <= 7;
ii++)
455 weights[
ii] = 0.5f -
wx * 0.5f;
461 for (std::size_t
ii = 0;
ii <= 1;
ii++)
462 weights[
ii] *= 0.5f -
wy * 0.5f;
463 for (std::size_t
ii = 4;
ii <= 5;
ii++)
464 weights[
ii] *= 0.5f -
wy * 0.5f;
466 for (std::size_t
ii = 2;
ii <= 3;
ii++)
467 weights[
ii] *= 0.5f +
wy * 0.5f;
469 for (std::size_t
ii = 6;
ii <= 7;
ii++)
470 weights[
ii] *= 0.5f +
wy * 0.5f;
474 for (std::size_t
ii = 0;
ii <= 1;
ii++)
475 weights[
ii] *= 0.5f +
wy * 0.5f;
476 for (std::size_t
ii = 4;
ii <= 5;
ii++)
477 weights[
ii] *= 0.5f +
wy * 0.5f;
479 for (std::size_t
ii = 2;
ii <= 3;
ii++)
480 weights[
ii] *= 0.5f -
wy * 0.5f;
482 for (std::size_t
ii = 6;
ii <= 7;
ii++)
483 weights[
ii] *= 0.5f -
wy * 0.5f;
489 for (std::size_t
ii = 0;
ii <= 7;
ii += 2)
490 weights[
ii] *= 0.5f -
wz * 0.5f;
492 for (std::size_t
ii = 1;
ii <= 7;
ii += 2)
493 weights[
ii] *= 0.5f +
wz * 0.5f;
498 for (std::size_t
ii = 0;
ii <= 7;
ii += 2)
499 weights[
ii] *= 0.5f +
wz * 0.5f;
501 for (std::size_t
ii = 1;
ii <= 7;
ii += 2)
502 weights[
ii] *= 0.5f -
wz * 0.5f;
522 for (
int d = 0; d < 308; ++d)
548template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
551 if (refine_clusters_ <= 0.f)
552 refine_clusters_ = 1.f;
557 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().
c_str ());
562 if (normals_->size () != surface_->size ())
564 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 ());
565 output.width = output.height = 0;
570 centroids_dominant_orientations_.clear ();
572 transforms_.clear ();
573 dominant_normals_.clear ();
578 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
581 normals_filtered_cloud->width = indices_in.
size ();
582 normals_filtered_cloud->height = 1;
583 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
586 indices_from_nfc_to_indices.resize (indices_in.
size ());
588 for (std::size_t i = 0; i < indices_in.
size (); ++i)
590 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
591 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
592 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
594 indices_from_nfc_to_indices[i] = indices_in[i];
597 std::vector<pcl::PointIndices> clusters;
599 if (normals_filtered_cloud->size () >= min_points_)
604 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
606 n3d.setRadiusSearch (radius_normals_);
607 n3d.setSearchMethod (normals_tree_filtered);
608 n3d.setInputCloud (normals_filtered_cloud);
609 n3d.compute (*normals_filtered_cloud);
613 normals_tree->setInputCloud (normals_filtered_cloud);
615 extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
616 eps_angle_threshold_,
static_cast<unsigned int> (min_points_));
618 std::vector<pcl::PointIndices> clusters_filtered;
619 int cluster_filtered_idx = 0;
620 for (
const auto &cluster : clusters)
627 clusters_.push_back (pi);
628 clusters_filtered.push_back (pi_filtered);
630 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
631 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
633 for (
const auto &index : cluster.indices)
635 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
636 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
639 avg_normal /=
static_cast<float> (cluster.indices.size ());
640 avg_centroid /=
static_cast<float> (cluster.indices.size ());
641 avg_normal.normalize ();
643 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
644 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
646 for (
const auto &index : cluster.indices)
649 double dot_p = avg_normal.dot ((*normals_filtered_cloud)[index].getNormalVector4fMap ());
650 if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
652 clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
653 clusters_filtered[cluster_filtered_idx].indices.push_back (index);
658 if (clusters_[cluster_filtered_idx].indices.empty ())
660 clusters_.pop_back ();
661 clusters_filtered.pop_back ();
664 cluster_filtered_idx++;
667 clusters = clusters_filtered;
672 vfh.setInputCloud (surface_);
673 vfh.setInputNormals (normals_);
674 vfh.setIndices (indices_);
675 vfh.setSearchMethod (this->tree_);
676 vfh.setUseGivenNormal (
true);
677 vfh.setUseGivenCentroid (
true);
678 vfh.setNormalizeBins (normalize_bins_);
682 if (!clusters.empty ())
684 for (
const auto &cluster : clusters)
686 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
687 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
689 for (
const auto &index : cluster.indices)
691 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
692 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
695 avg_normal /=
static_cast<float> (cluster.indices.size ());
696 avg_centroid /=
static_cast<float> (cluster.indices.size ());
697 avg_normal.normalize ();
700 dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
701 centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
705 output.resize (dominant_normals_.size ());
706 output.width = dominant_normals_.size ();
708 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
711 vfh.setNormalToUse (dominant_normals_[i]);
712 vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
714 vfh.compute (vfh_signature);
715 output[i] = vfh_signature[0];
721 computeRFAndShapeDistribution (cloud_input, output, clusters_);
726 PCL_WARN(
"No clusters were found in the surface... using VFH...\n");
727 Eigen::Vector4f avg_centroid;
729 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
730 centroids_dominant_orientations_.push_back (cloud_centroid);
733 vfh.setCentroidToUse (cloud_centroid);
734 vfh.setUseGivenNormal (
false);
737 vfh.compute (vfh_signature);
742 output[0] = vfh_signature[0];
743 Eigen::Matrix4f
id = Eigen::Matrix4f::Identity ();
744 transforms_.push_back (
id);
745 valid_transforms_.push_back (
false);
749#define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
Feature represents the base feature class.
virtual bool deinitCompute()
This method should get called after ending the actual computation.
OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram ...
bool sgurf(Eigen::Vector3f ¢roid, Eigen::Vector3f &normal_centroid, PointInTPtr &processed, std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations, PointInTPtr &grid, pcl::PointIndices &indices)
Computes SGURF.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
void filterNormalsWithHighCurvature(const pcl::PointCloud< PointNT > &cloud, pcl::Indices &indices_to_use, pcl::Indices &indices_out, pcl::Indices &indices_in, float threshold)
Removes normals with high curvature caused by real edges or noisy data.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
typename pcl::PointCloud< PointInT >::Ptr PointInTPtr
void computeRFAndShapeDistribution(PointInTPtr &processed, PointCloudOut &output, std::vector< pcl::PointIndices > &cluster_indices)
Computes SGURF and the shape distribution based on the selected SGURF.
shared_ptr< PointCloud< PointT > > Ptr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
shared_ptr< pcl::search::Search< PointT > > Ptr
Define standard C methods and C++ classes that are common to all methods.
void getMaxDistance(const pcl::PointCloud< PointT > &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
Get the point at maximum distance from a given point and a given pointcloud.
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
IndicesAllocator<> Indices
Type used for indices in PCL.