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>
49 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
54 output.width = output.height = 0;
62 output.width = output.height = 1;
66 computeFeature (output);
72 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
78 std::vector<pcl::PointIndices> &clusters,
80 unsigned int min_pts_per_cluster,
81 unsigned int max_pts_per_cluster)
85 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
86 "dataset (%zu) than the input cloud (%zu)!\n",
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;
147 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
153 clusters.emplace_back (std::move(r));
159 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
167 indices_out.resize (cloud.
size ());
168 indices_in.resize (cloud.
size ());
173 for (
const auto &index : indices_to_use)
175 if (cloud[index].curvature > threshold)
177 indices_out[out] = index;
182 indices_in[in] = index;
187 indices_out.resize (out);
188 indices_in.resize (in);
192 template<
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 ());
199 output.width = output.height = 0;
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);
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.
Feature represents the base feature class.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void resize(std::size_t count)
Resizes the container to contain count elements.
reference emplace_back(Args &&...args)
Emplace a new point in the cloud, at the end of the container.
pcl::PCLHeader header
The point cloud header.
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
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
virtual int radiusSearch(const PointT &point, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
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.