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
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 (
int i = 0; i < static_cast<int> (cloud.
points.size ()); ++i)
106 std::vector<unsigned int> seed_queue;
108 seed_queue.push_back (i);
112 while (sq_idx < static_cast<int> (seed_queue.size ()))
115 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
121 for (
size_t j = 1; j < nn_indices.size (); ++j)
123 if (processed[nn_indices[j]])
129 double dot_p = normals.
points[seed_queue[sq_idx]].normal[0] * normals.
points[nn_indices[j]].normal[0]
130 + normals.
points[seed_queue[sq_idx]].normal[1] * normals.
points[nn_indices[j]].normal[1]
131 + normals.
points[seed_queue[sq_idx]].normal[2] * normals.
points[nn_indices[j]].normal[2];
133 if (fabs (acos (dot_p)) < eps_angle)
135 processed[nn_indices[j]] =
true;
136 seed_queue.push_back (nn_indices[j]);
144 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
147 r.
indices.resize (seed_queue.size ());
148 for (
size_t j = 0; j < seed_queue.size (); ++j)
155 clusters.push_back (r);
161 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
164 std::vector<int> &indices_to_use,
165 std::vector<int> &indices_out,
166 std::vector<int> &indices_in,
169 indices_out.resize (cloud.
points.size ());
170 indices_in.resize (cloud.
points.size ());
175 for (
int i = 0; i < static_cast<int> (indices_to_use.size ()); i++)
177 if (cloud.
points[indices_to_use[i]].curvature > threshold)
179 indices_out[out] = indices_to_use[i];
184 indices_in[in] = indices_to_use[i];
189 indices_out.resize (out);
190 indices_in.resize (in);
194 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
200 PCL_ERROR (
"[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
201 output.width = output.height = 0;
202 output.points.clear ();
205 if (normals_->points.size () != surface_->points.size ())
207 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 ());
208 output.width = output.height = 0;
209 output.points.clear ();
213 centroids_dominant_orientations_.clear ();
216 std::vector<int> indices_out;
217 std::vector<int> indices_in;
218 filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
221 normals_filtered_cloud->width =
static_cast<uint32_t
> (indices_in.size ());
222 normals_filtered_cloud->height = 1;
223 normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
225 for (
size_t i = 0; i < indices_in.size (); ++i)
227 normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
228 normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
229 normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
232 std::vector<pcl::PointIndices> clusters;
234 if(normals_filtered_cloud->points.size() >= min_points_)
238 normals_tree_filtered->setInputCloud (normals_filtered_cloud);
245 n3d.
compute (*normals_filtered_cloud);
248 normals_tree->setInputCloud (normals_filtered_cloud);
250 extractEuclideanClustersSmooth (*normals_filtered_cloud,
251 *normals_filtered_cloud,
255 eps_angle_threshold_,
256 static_cast<unsigned int> (min_points_));
261 vfh.setInputCloud (surface_);
262 vfh.setInputNormals (normals_);
263 vfh.setIndices(indices_);
264 vfh.setSearchMethod (this->tree_);
265 vfh.setUseGivenNormal (
true);
266 vfh.setUseGivenCentroid (
true);
267 vfh.setNormalizeBins (normalize_bins_);
268 vfh.setNormalizeDistance (
true);
269 vfh.setFillSizeComponent (
true);
273 if (clusters.size () > 0)
276 for (
size_t i = 0; i < clusters.size (); ++i)
278 Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
279 Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
281 for (
size_t j = 0; j < clusters[i].indices.size (); j++)
283 avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
284 avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
287 avg_normal /=
static_cast<float> (clusters[i].indices.size ());
288 avg_centroid /=
static_cast<float> (clusters[i].indices.size ());
290 Eigen::Vector4f centroid_test;
292 avg_normal.normalize ();
294 Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
295 Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
298 dominant_normals_.push_back (avg_norm);
299 centroids_dominant_orientations_.push_back (avg_dominant_centroid);
303 output.points.resize (dominant_normals_.size ());
304 output.width =
static_cast<uint32_t
> (dominant_normals_.size ());
306 for (
size_t i = 0; i < dominant_normals_.size (); ++i)
309 vfh.setNormalToUse (dominant_normals_[i]);
310 vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
312 vfh.compute (vfh_signature);
318 Eigen::Vector4f avg_centroid;
320 Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
321 centroids_dominant_orientations_.push_back (cloud_centroid);
324 vfh.setCentroidToUse (cloud_centroid);
325 vfh.setUseGivenNormal (
false);
328 vfh.compute (vfh_signature);
333 output.points[0] = vfh_signature.
points[0];
337 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
339 #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...
boost::shared_ptr< PointCloud< PointT > > Ptr
virtual int radiusSearch(const PointT &point, double radius, std::vector< int > &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 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.
std::vector< int > indices
uint32_t width
The point cloud width (if organized as an image-structure).
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input 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.
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.
uint32_t height
The point cloud height (if organized as an image-structure).