Point Cloud Library (PCL)  1.11.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/common/centroid.h>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template<typename PointInT, typename PointNT, typename PointOutT> void
51 {
53  {
54  output.width = output.height = 0;
55  output.clear ();
56  return;
57  }
58  // Resize the output dataset
59  // Important! We should only allocate precisely how many elements we will need, otherwise
60  // we risk at pre-allocating too much memory which could lead to bad_alloc
61  // (see http://dev.pointclouds.org/issues/657)
62  output.width = output.height = 1;
63  output.resize (1);
64 
65  // Perform the actual feature computation
66  computeFeature (output);
67 
69 }
70 
71 //////////////////////////////////////////////////////////////////////////////////////////////
72 template<typename PointInT, typename PointNT, typename PointOutT> void
75  const pcl::PointCloud<pcl::PointNormal> &normals,
76  float tolerance,
78  std::vector<pcl::PointIndices> &clusters,
79  double eps_angle,
80  unsigned int min_pts_per_cluster,
81  unsigned int max_pts_per_cluster)
82 {
83  if (tree->getInputCloud ()->size () != cloud.size ())
84  {
85  PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different point cloud "
86  "dataset (%zu) than the input cloud (%zu)!\n",
87  static_cast<std::size_t>(tree->getInputCloud()->size()),
88  static_cast<std::size_t>(cloud.size()));
89  return;
90  }
91  if (cloud.size () != normals.size ())
92  {
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()));
97  return;
98  }
99 
100  // Create a bool vector of processed point indices, and initialize it to false
101  std::vector<bool> processed (cloud.size (), false);
102 
103  pcl::Indices nn_indices;
104  std::vector<float> nn_distances;
105  // Process all points in the indices vector
106  for (std::size_t i = 0; i < cloud.size (); ++i)
107  {
108  if (processed[i])
109  continue;
110  processed[i] = true;
111 
113  r.header = cloud.header;
114  auto& seed_queue = r.indices;
115 
116  seed_queue.push_back (i);
117 
118  // loop has an emplace_back, making it difficult to use modern loops
119  for (std::size_t idx = 0; idx != seed_queue.size (); ++idx)
120  {
121  // Search for seed_queue[index]
122  if (!tree->radiusSearch (seed_queue[idx], tolerance, nn_indices, nn_distances))
123  {
124  continue;
125  }
126 
127  // skip index 0, since nn_indices[0] == idx, worth it?
128  for (std::size_t j = 1; j < nn_indices.size (); ++j)
129  {
130  if (processed[nn_indices[j]]) // Has this point been processed before ?
131  continue;
132 
133  //processed[nn_indices[j]] = true;
134  // [-1;1]
135  const double dot_p = normals[seed_queue[idx]].getNormalVector3fMap().dot(
136  normals[nn_indices[j]].getNormalVector3fMap());
137 
138  if (std::acos (dot_p) < eps_angle)
139  {
140  processed[nn_indices[j]] = true;
141  seed_queue.emplace_back (nn_indices[j]);
142  }
143  }
144  }
145 
146  // If this queue is satisfactory, add to the clusters
147  if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
148  {
149  std::sort (r.indices.begin (), r.indices.end ());
150  r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
151 
152  // Might be better to work directly in the cluster somehow
153  clusters.emplace_back (std::move(r)); // Trying to avoid a copy by moving
154  }
155  }
156 }
157 
158 //////////////////////////////////////////////////////////////////////////////////////////////
159 template<typename PointInT, typename PointNT, typename PointOutT> void
161  const pcl::PointCloud<PointNT> & cloud,
162  pcl::Indices &indices_to_use,
163  pcl::Indices &indices_out,
164  pcl::Indices &indices_in,
165  float threshold)
166 {
167  indices_out.resize (cloud.size ());
168  indices_in.resize (cloud.size ());
169 
170  std::size_t in, out;
171  in = out = 0;
172 
173  for (const auto &index : indices_to_use)
174  {
175  if (cloud[index].curvature > threshold)
176  {
177  indices_out[out] = index;
178  out++;
179  }
180  else
181  {
182  indices_in[in] = index;
183  in++;
184  }
185  }
186 
187  indices_out.resize (out);
188  indices_in.resize (in);
189 }
190 
191 //////////////////////////////////////////////////////////////////////////////////////////////
192 template<typename PointInT, typename PointNT, typename PointOutT> void
194 {
195  // Check if input was set
196  if (!normals_)
197  {
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.clear ();
201  return;
202  }
203  if (normals_->size () != surface_->size ())
204  {
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.clear ();
208  return;
209  }
210 
211  centroids_dominant_orientations_.clear ();
212 
213  // ---[ Step 0: remove normals with high curvature
214  pcl::Indices indices_out;
215  pcl::Indices indices_in;
216  filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
217 
219  normals_filtered_cloud->width = indices_in.size ();
220  normals_filtered_cloud->height = 1;
221  normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
222 
223  for (std::size_t i = 0; i < indices_in.size (); ++i)
224  {
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;
228  }
229 
230  std::vector<pcl::PointIndices> clusters;
231 
232  if(normals_filtered_cloud->size() >= min_points_)
233  {
234  //recompute normals and use them for clustering
235  KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
236  normals_tree_filtered->setInputCloud (normals_filtered_cloud);
237 
238 
240  n3d.setRadiusSearch (radius_normals_);
241  n3d.setSearchMethod (normals_tree_filtered);
242  n3d.setInputCloud (normals_filtered_cloud);
243  n3d.compute (*normals_filtered_cloud);
244 
245  KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
246  normals_tree->setInputCloud (normals_filtered_cloud);
247 
248  extractEuclideanClustersSmooth (*normals_filtered_cloud,
249  *normals_filtered_cloud,
250  cluster_tolerance_,
251  normals_tree,
252  clusters,
253  eps_angle_threshold_,
254  static_cast<unsigned int> (min_points_));
255 
256  }
257 
258  VFHEstimator vfh;
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);
268  output.height = 1;
269 
270  // ---[ Step 1b : check if any dominant cluster was found
271  if (!clusters.empty ())
272  { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information
273 
274  for (const auto &cluster : clusters) //for each cluster
275  {
276  Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
277  Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
278 
279  for (const auto &index : cluster.indices)
280  {
281  avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
282  avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
283  }
284 
285  avg_normal /= static_cast<float> (cluster.indices.size ());
286  avg_centroid /= static_cast<float> (cluster.indices.size ());
287 
288  Eigen::Vector4f centroid_test;
289  pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test);
290  avg_normal.normalize ();
291 
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]);
294 
295  //append normal and centroid for the clusters
296  dominant_normals_.push_back (avg_norm);
297  centroids_dominant_orientations_.push_back (avg_dominant_centroid);
298  }
299 
300  //compute modified VFH for all dominant clusters and add them to the list!
301  output.resize (dominant_normals_.size ());
302  output.width = dominant_normals_.size ();
303 
304  for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
305  {
306  //configure VFH computation for CVFH
307  vfh.setNormalToUse (dominant_normals_[i]);
308  vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
310  vfh.compute (vfh_signature);
311  output[i] = vfh_signature[0];
312  }
313  }
314  else
315  { // ---[ Step 1b.1 : If no, compute CVFH using all the object points
316  Eigen::Vector4f avg_centroid;
317  pcl::compute3DCentroid (*surface_, avg_centroid);
318  Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
319  centroids_dominant_orientations_.push_back (cloud_centroid);
320 
321  //configure VFH computation for CVFH using all object points
322  vfh.setCentroidToUse (cloud_centroid);
323  vfh.setUseGivenNormal (false);
324 
326  vfh.compute (vfh_signature);
327 
328  output.resize (1);
329  output.width = 1;
330 
331  output[0] = vfh_signature[0];
332  }
333 }
334 
335 #define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;
336 
337 #endif // PCL_FEATURES_IMPL_VFH_H_
pcl::CVFHEstimation
CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given poin...
Definition: cvfh.h:62
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:243
pcl::search::Search::getInputCloud
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: search.h:125
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
pcl::search::Search::radiusSearch
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.
pcl::PointIndices::header
::pcl::PCLHeader header
Definition: PointIndices.h:19
pcl::CVFHEstimation::compute
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: cvfh.hpp:50
pcl::PointCloud< pcl::PointNormal >
pcl::NormalEstimation::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:332
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
pcl::search::Search::Ptr
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::PointIndices
Definition: PointIndices.h:11
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:386
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::CVFHEstimation::PointCloudOut
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: cvfh.h:76
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:407
pcl::PointCloud::emplace_back
reference emplace_back(Args &&...args)
Emplace a new point in the cloud, at the end of the container.
Definition: point_cloud.h:665
pcl::Feature::setRadiusSearch
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:201
pcl::compute3DCentroid
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:56
pcl::CVFHEstimation::filterNormalsWithHighCurvature
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.
Definition: cvfh.hpp:160
centroid.h
pcl::Feature::setSearchMethod
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:167
pcl::Feature
Feature represents the base feature class.
Definition: feature.h:106