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>
46 #include <pcl/features/pfh_tools.h>
47 #include <pcl/common/centroid.h>
50 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
55 output.width = output.height = 0;
56 output.points.clear ();
63 output.width = output.height = 1;
64 output.points.resize (1);
67 computeFeature (output);
73 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
79 std::vector<pcl::PointIndices> &clusters,
81 unsigned int min_pts_per_cluster,
82 unsigned int max_pts_per_cluster)
86 PCL_ERROR (
"[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->
getInputCloud ()->points.size (), cloud.
points.size ());
91 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.
points.size (), normals.
points.size ());
96 std::vector<bool> processed (cloud.
points.size (),
false);
98 std::vector<int> nn_indices;
99 std::vector<float> nn_distances;
101 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
111 seed_queue.push_back (i);
114 for (std::size_t idx = 0; idx != seed_queue.size (); ++idx)
117 if (!tree->
radiusSearch (seed_queue[idx], tolerance, nn_indices, nn_distances))
123 for (std::size_t j = 1; j < nn_indices.size (); ++j)
125 if (processed[nn_indices[j]])
130 const double dot_p = normals.
points[seed_queue[idx]].getNormalVector3fMap().dot(
131 normals.
points[nn_indices[j]].getNormalVector3fMap());
133 if (std::acos (dot_p) < eps_angle)
135 processed[nn_indices[j]] =
true;
136 seed_queue.emplace_back (nn_indices[j]);
142 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
148 clusters.emplace_back (std::move(r));
154 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
157 std::vector<int> &indices_to_use,
158 std::vector<int> &indices_out,
159 std::vector<int> &indices_in,
162 indices_out.resize (cloud.
points.size ());
163 indices_in.resize (cloud.
points.size ());
168 for (
const int &index : indices_to_use)
170 if (cloud.
points[index].curvature > threshold)
172 indices_out[out] = index;
177 indices_in[in] = index;
182 indices_out.resize (out);
183 indices_in.resize (in);
187 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
193 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
194 output.width = output.height = 0;
195 output.points.clear ();
198 if (normals_->points.size () != surface_->points.size ())
200 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 ());
201 output.width = output.height = 0;
202 output.points.clear ();
206 centroids_dominant_orientations_.clear ();
209 std::vector<int> indices_out;
210 std::vector<int> indices_in;
211 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
214 normals_filtered_cloud->width =
static_cast<std::uint32_t
> (indices_in.size ());
215 normals_filtered_cloud->height = 1;
216 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
218 for (std::size_t i = 0; i < indices_in.size (); ++i)
220 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
221 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
222 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
225 std::vector<pcl::PointIndices> clusters;
227 if(normals_filtered_cloud->points.size() >= min_points_)
231 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
238 n3d.
compute (*normals_filtered_cloud);
241 normals_tree->setInputCloud (normals_filtered_cloud);
243 extractEuclideanClustersSmooth (*normals_filtered_cloud,
244 *normals_filtered_cloud,
248 eps_angle_threshold_,
249 static_cast<unsigned int> (min_points_));
254 vfh.setInputCloud (surface_);
255 vfh.setInputNormals (normals_);
256 vfh.setIndices(indices_);
257 vfh.setSearchMethod (this->tree_);
258 vfh.setUseGivenNormal (
true);
259 vfh.setUseGivenCentroid (
true);
260 vfh.setNormalizeBins (normalize_bins_);
261 vfh.setNormalizeDistance (
true);
262 vfh.setFillSizeComponent (
true);
266 if (!clusters.empty ())
269 for (
const auto &cluster : clusters)
271 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
272 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
274 for (
const auto &index : cluster.indices)
276 avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap ();
277 avg_centroid += normals_filtered_cloud->points[index].getVector4fMap ();
280 avg_normal /=
static_cast<float> (cluster.indices.size ());
281 avg_centroid /=
static_cast<float> (cluster.indices.size ());
283 Eigen::Vector4f centroid_test;
285 avg_normal.normalize ();
287 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
288 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
291 dominant_normals_.push_back (avg_norm);
292 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
296 output.points.resize (dominant_normals_.size ());
297 output.width =
static_cast<std::uint32_t
> (dominant_normals_.size ());
299 for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
302 vfh.setNormalToUse (dominant_normals_[i]);
303 vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
305 vfh.compute (vfh_signature);
311 Eigen::Vector4f avg_centroid;
313 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
314 centroids_dominant_orientations_.push_back (cloud_centroid);
317 vfh.setCentroidToUse (cloud_centroid);
318 vfh.setUseGivenNormal (
false);
321 vfh.compute (vfh_signature);
326 output.points[0] = vfh_signature.
points[0];
330 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
332 #endif // PCL_FEATURES_IMPL_VFH_H_
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given poin...
void filterNormalsWithHighCurvature(const pcl::PointCloud< PointNT > &cloud, std::vector< int > &indices_to_use, std::vector< int > &indices_out, std::vector< int > &indices_in, float threshold)
Removes normals with high curvature caused by real edges or noisy data.
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.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
shared_ptr< PointCloud< PointT > > Ptr
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
shared_ptr< pcl::search::Search< PointT > > Ptr
Feature represents the base feature class.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
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 setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
pcl::PCLHeader header
The point cloud header.