41#ifndef PCL_FEATURES_IMPL_CVFH_H_
42#define PCL_FEATURES_IMPL_CVFH_H_
44#include <pcl/features/cvfh.h>
45#include <pcl/features/normal_3d.h>
49template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
72template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
78 std::vector<pcl::PointIndices> &
clusters,
83 if (tree->getInputCloud ()->size () != cloud.
size ())
85 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
86 "dataset (%zu) than the input cloud (%zu)!\n",
87 static_cast<std::size_t
>(tree->getInputCloud()->size()),
88 static_cast<std::size_t
>(cloud.
size()));
91 if (cloud.
size () != normals.
size ())
93 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
94 "cloud (%zu) different than normals (%zu)!\n",
95 static_cast<std::size_t
>(cloud.
size()),
96 static_cast<std::size_t
>(normals.
size()));
101 std::vector<bool> processed (cloud.
size (),
false);
104 std::vector<float> nn_distances;
106 for (std::size_t i = 0; i < cloud.
size (); ++i)
116 seed_queue.push_back (i);
119 for (std::size_t idx = 0; idx != seed_queue.size (); ++idx)
122 if (!tree->radiusSearch (seed_queue[idx], tolerance, nn_indices, nn_distances))
128 for (std::size_t j = 1; j < nn_indices.
size (); ++j)
130 if (processed[nn_indices[j]])
135 const double dot_p = normals[seed_queue[idx]].getNormalVector3fMap().dot(
136 normals[nn_indices[j]].getNormalVector3fMap());
138 if (std::acos (dot_p) < eps_angle)
140 processed[nn_indices[j]] =
true;
141 seed_queue.emplace_back (nn_indices[j]);
147 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
153 clusters.emplace_back (std::move(r));
159template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
175 if (cloud[index].curvature > threshold)
192template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
198 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().
c_str ());
203 if (normals_->size () != surface_->size ())
205 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 ());
206 output.width = output.height = 0;
211 centroids_dominant_orientations_.clear ();
216 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
219 normals_filtered_cloud->width = indices_in.
size ();
220 normals_filtered_cloud->height = 1;
221 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
223 for (std::size_t i = 0; i < indices_in.
size (); ++i)
225 (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
226 (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
227 (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
230 std::vector<pcl::PointIndices> clusters;
232 if(normals_filtered_cloud->size() >= min_points_)
236 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
240 n3d.setRadiusSearch (radius_normals_);
241 n3d.setSearchMethod (normals_tree_filtered);
242 n3d.setInputCloud (normals_filtered_cloud);
243 n3d.compute (*normals_filtered_cloud);
246 normals_tree->setInputCloud (normals_filtered_cloud);
248 extractEuclideanClustersSmooth (*normals_filtered_cloud,
249 *normals_filtered_cloud,
253 eps_angle_threshold_,
254 static_cast<unsigned int> (min_points_));
259 vfh.setInputCloud (surface_);
260 vfh.setInputNormals (normals_);
261 vfh.setIndices(indices_);
262 vfh.setSearchMethod (this->tree_);
263 vfh.setUseGivenNormal (
true);
264 vfh.setUseGivenCentroid (
true);
265 vfh.setNormalizeBins (normalize_bins_);
266 vfh.setNormalizeDistance (
true);
267 vfh.setFillSizeComponent (
true);
271 if (!clusters.empty ())
274 for (
const auto &cluster : clusters)
276 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
277 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
279 for (
const auto &index : cluster.indices)
281 avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
282 avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
285 avg_normal /=
static_cast<float> (cluster.indices.size ());
286 avg_centroid /=
static_cast<float> (cluster.indices.size ());
288 Eigen::Vector4f centroid_test;
290 avg_normal.normalize ();
292 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
293 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
296 dominant_normals_.push_back (avg_norm);
297 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
301 output.resize (dominant_normals_.size ());
302 output.width = dominant_normals_.size ();
304 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
307 vfh.setNormalToUse (dominant_normals_[i]);
308 vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
310 vfh.compute (vfh_signature);
311 output[i] = vfh_signature[0];
316 Eigen::Vector4f avg_centroid;
318 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
319 centroids_dominant_orientations_.push_back (cloud_centroid);
322 vfh.setCentroidToUse (cloud_centroid);
323 vfh.setUseGivenNormal (
false);
326 vfh.compute (vfh_signature);
331 output[0] = vfh_signature[0];
335#define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
Define methods for centroid estimation and covariance matrix calculus.
CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given poin...
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.
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.
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
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.
IndicesAllocator<> Indices
Type used for indices in PCL.