Point Cloud Library (PCL)  1.9.1-dev
cvfh.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_FEATURES_IMPL_CVFH_H_
42 #define PCL_FEATURES_IMPL_CVFH_H_
43 
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>
48 
49 //////////////////////////////////////////////////////////////////////////////////////////////
50 template<typename PointInT, typename PointNT, typename PointOutT> void
52 {
54  {
55  output.width = output.height = 0;
56  output.points.clear ();
57  return;
58  }
59  // Resize the output dataset
60  // Important! We should only allocate precisely how many elements we will need, otherwise
61  // we risk at pre-allocating too much memory which could lead to bad_alloc
62  // (see http://dev.pointclouds.org/issues/657)
63  output.width = output.height = 1;
64  output.points.resize (1);
65 
66  // Perform the actual feature computation
67  computeFeature (output);
68 
70 }
71 
72 //////////////////////////////////////////////////////////////////////////////////////////////
73 template<typename PointInT, typename PointNT, typename PointOutT> void
76  const pcl::PointCloud<pcl::PointNormal> &normals,
77  float tolerance,
79  std::vector<pcl::PointIndices> &clusters,
80  double eps_angle,
81  unsigned int min_pts_per_cluster,
82  unsigned int max_pts_per_cluster)
83 {
84  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
85  {
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 ());
87  return;
88  }
89  if (cloud.points.size () != normals.points.size ())
90  {
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 ());
92  return;
93  }
94 
95  // Create a bool vector of processed point indices, and initialize it to false
96  std::vector<bool> processed (cloud.points.size (), false);
97 
98  std::vector<int> nn_indices;
99  std::vector<float> nn_distances;
100  // Process all points in the indices vector
101  for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
102  {
103  if (processed[i])
104  continue;
105 
106  std::vector<unsigned int> seed_queue;
107  int sq_idx = 0;
108  seed_queue.push_back (i);
109 
110  processed[i] = true;
111 
112  while (sq_idx < static_cast<int> (seed_queue.size ()))
113  {
114  // Search for sq_idx
115  if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
116  {
117  sq_idx++;
118  continue;
119  }
120 
121  for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
122  {
123  if (processed[nn_indices[j]]) // Has this point been processed before ?
124  continue;
125 
126  //processed[nn_indices[j]] = true;
127  // [-1;1]
128 
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];
132 
133  if (std::abs (std::acos (dot_p)) < eps_angle)
134  {
135  processed[nn_indices[j]] = true;
136  seed_queue.push_back (nn_indices[j]);
137  }
138  }
139 
140  sq_idx++;
141  }
142 
143  // If this queue is satisfactory, add to the clusters
144  if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
145  {
147  r.indices.resize (seed_queue.size ());
148  for (size_t j = 0; j < seed_queue.size (); ++j)
149  r.indices[j] = seed_queue[j];
150 
151  std::sort (r.indices.begin (), r.indices.end ());
152  r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
153 
154  r.header = cloud.header;
155  clusters.push_back (r); // We could avoid a copy by working directly in the vector
156  }
157  }
158 }
159 
160 //////////////////////////////////////////////////////////////////////////////////////////////
161 template<typename PointInT, typename PointNT, typename PointOutT> void
163  const pcl::PointCloud<PointNT> & cloud,
164  std::vector<int> &indices_to_use,
165  std::vector<int> &indices_out,
166  std::vector<int> &indices_in,
167  float threshold)
168 {
169  indices_out.resize (cloud.points.size ());
170  indices_in.resize (cloud.points.size ());
171 
172  size_t in, out;
173  in = out = 0;
174 
175  for (const int &index : indices_to_use)
176  {
177  if (cloud.points[index].curvature > threshold)
178  {
179  indices_out[out] = index;
180  out++;
181  }
182  else
183  {
184  indices_in[in] = index;
185  in++;
186  }
187  }
188 
189  indices_out.resize (out);
190  indices_in.resize (in);
191 }
192 
193 //////////////////////////////////////////////////////////////////////////////////////////////
194 template<typename PointInT, typename PointNT, typename PointOutT> void
196 {
197  // Check if input was set
198  if (!normals_)
199  {
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 ();
203  return;
204  }
205  if (normals_->points.size () != surface_->points.size ())
206  {
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 ();
210  return;
211  }
212 
213  centroids_dominant_orientations_.clear ();
214 
215  // ---[ Step 0: remove normals with high curvature
216  std::vector<int> indices_out;
217  std::vector<int> indices_in;
218  filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
219 
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);
224 
225  for (size_t i = 0; i < indices_in.size (); ++i)
226  {
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;
230  }
231 
232  std::vector<pcl::PointIndices> clusters;
233 
234  if(normals_filtered_cloud->points.size() >= min_points_)
235  {
236  //recompute normals and use them for clustering
237  KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
238  normals_tree_filtered->setInputCloud (normals_filtered_cloud);
239 
240 
242  n3d.setRadiusSearch (radius_normals_);
243  n3d.setSearchMethod (normals_tree_filtered);
244  n3d.setInputCloud (normals_filtered_cloud);
245  n3d.compute (*normals_filtered_cloud);
246 
247  KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
248  normals_tree->setInputCloud (normals_filtered_cloud);
249 
250  extractEuclideanClustersSmooth (*normals_filtered_cloud,
251  *normals_filtered_cloud,
252  cluster_tolerance_,
253  normals_tree,
254  clusters,
255  eps_angle_threshold_,
256  static_cast<unsigned int> (min_points_));
257 
258  }
259 
260  VFHEstimator vfh;
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);
270  output.height = 1;
271 
272  // ---[ Step 1b : check if any dominant cluster was found
273  if (!clusters.empty ())
274  { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information
275 
276  for (const auto &cluster : clusters) //for each cluster
277  {
278  Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
279  Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
280 
281  for (const auto &index : cluster.indices)
282  {
283  avg_normal += normals_filtered_cloud->points[index].getNormalVector4fMap ();
284  avg_centroid += normals_filtered_cloud->points[index].getVector4fMap ();
285  }
286 
287  avg_normal /= static_cast<float> (cluster.indices.size ());
288  avg_centroid /= static_cast<float> (cluster.indices.size ());
289 
290  Eigen::Vector4f centroid_test;
291  pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test);
292  avg_normal.normalize ();
293 
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]);
296 
297  //append normal and centroid for the clusters
298  dominant_normals_.push_back (avg_norm);
299  centroids_dominant_orientations_.push_back (avg_dominant_centroid);
300  }
301 
302  //compute modified VFH for all dominant clusters and add them to the list!
303  output.points.resize (dominant_normals_.size ());
304  output.width = static_cast<uint32_t> (dominant_normals_.size ());
305 
306  for (size_t i = 0; i < dominant_normals_.size (); ++i)
307  {
308  //configure VFH computation for CVFH
309  vfh.setNormalToUse (dominant_normals_[i]);
310  vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
312  vfh.compute (vfh_signature);
313  output.points[i] = vfh_signature.points[0];
314  }
315  }
316  else
317  { // ---[ Step 1b.1 : If no, compute CVFH using all the object points
318  Eigen::Vector4f avg_centroid;
319  pcl::compute3DCentroid (*surface_, avg_centroid);
320  Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
321  centroids_dominant_orientations_.push_back (cloud_centroid);
322 
323  //configure VFH computation for CVFH using all object points
324  vfh.setCentroidToUse (cloud_centroid);
325  vfh.setUseGivenNormal (false);
326 
328  vfh.compute (vfh_signature);
329 
330  output.points.resize (1);
331  output.width = 1;
332 
333  output.points[0] = vfh_signature.points[0];
334  }
335 }
336 
337 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
338 
339 #endif // PCL_FEATURES_IMPL_VFH_H_
void setUseGivenCentroid(bool use)
Set use_given_centroid_.
Definition: vfh.h:162
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:426
CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given poin...
Definition: cvfh.h:63
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.
Definition: cvfh.hpp:162
std::vector< int > indices
Definition: PointIndices.h:19
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:200
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:242
void setUseGivenNormal(bool use)
Set use_given_normal_.
Definition: vfh.h:143
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:431
typename pcl::search::Search< PointNormal >::Ptr KdTreePtr
Definition: cvfh.h:78
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:429
void setNormalToUse(const Eigen::Vector3f &normal)
Set the normal to use.
Definition: vfh.h:153
void setCentroidToUse(const Eigen::Vector3f &centroid)
Set centroid_to_use_.
Definition: vfh.h:172
::pcl::PCLHeader header
Definition: PointIndices.h:17
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:444
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
Definition: vfh.h:70
void setNormalizeBins(bool normalize)
set normalize_bins_
Definition: vfh.h:181
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:423
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: vfh.hpp:65
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:166
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: search.h:124
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: cvfh.hpp:51
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
Feature represents the base feature class.
Definition: feature.h:105
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:192
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:80
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:50
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:331
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
Definition: feature.h:344
void setFillSizeComponent(bool fill_size)
set size_component_
Definition: vfh.h:201
Define methods for centroid estimation and covariance matrix calculus.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: cvfh.h:77
void setNormalizeDistance(bool normalize)
set normalize_distances_
Definition: vfh.h:191