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;
55 output.points.clear ();
62 output.width = output.height = 1;
63 output.points.resize (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);
103 std::vector<int> nn_indices;
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 162 std::vector<int> &indices_to_use,
163 std::vector<int> &indices_out,
164 std::vector<int> &indices_in,
167 indices_out.resize (cloud.
size ());
168 indices_in.resize (cloud.
size ());
173 for (
const int &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;
200 output.points.clear ();
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;
207 output.points.clear ();
211 centroids_dominant_orientations_.clear ();
214 std::vector<int> indices_out;
215 std::vector<int> indices_in;
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.points.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>; 337 #endif // PCL_FEATURES_IMPL_VFH_H_ search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
shared_ptr< PointCloud< PointT > > Ptr
reference emplace_back(Args &&...args)
Emplace a new point in the cloud, at the end of the container.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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...
pcl::PCLHeader header
The point cloud header.
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
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 <setInputCloud (), setIndices ()> 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.
Define methods for centroid estimation and covariance matrix calculus.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut