Point Cloud Library (PCL)  1.11.1
our_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: cvfh.hpp 5311 2012-03-26 22:02:04Z aaldoma $
38  *
39  */
40 
41 #ifndef PCL_FEATURES_IMPL_OURCVFH_H_
42 #define PCL_FEATURES_IMPL_OURCVFH_H_
43 
44 #include <pcl/features/our_cvfh.h>
45 #include <pcl/features/vfh.h>
46 #include <pcl/features/normal_3d.h>
47 #include <pcl/common/transforms.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
75  const pcl::PointCloud<pcl::PointNormal> &normals,
76  float tolerance,
78  std::vector<pcl::PointIndices> &clusters, double eps_angle,
79  unsigned int min_pts_per_cluster,
80  unsigned int max_pts_per_cluster)
81 {
82  if (tree->getInputCloud ()->size () != cloud.size ())
83  {
84  PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different point cloud "
85  "dataset (%zu) than the input cloud (%zu)!\n",
86  static_cast<std::size_t>(tree->getInputCloud()->size()),
87  static_cast<std::size_t>(cloud.size()));
88  return;
89  }
90  if (cloud.size () != normals.size ())
91  {
92  PCL_ERROR("[pcl::extractEuclideanClusters] Number of points in the input point "
93  "cloud (%zu) different than normals (%zu)!\n",
94  static_cast<std::size_t>(cloud.size()),
95  static_cast<std::size_t>(normals.size()));
96  return;
97  }
98 
99  // Create a bool vector of processed point indices, and initialize it to false
100  std::vector<bool> processed (cloud.size (), false);
101 
102  std::vector<int> nn_indices;
103  std::vector<float> nn_distances;
104  // Process all points in the indices vector
105  for (std::size_t i = 0; i < cloud.size (); ++i)
106  {
107  if (processed[i])
108  continue;
109 
110  std::vector<std::size_t> seed_queue;
111  std::size_t sq_idx = 0;
112  seed_queue.push_back (i);
113 
114  processed[i] = true;
115 
116  while (sq_idx < seed_queue.size ())
117  {
118  // Search for sq_idx
119  if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
120  {
121  sq_idx++;
122  continue;
123  }
124 
125  for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
126  {
127  if (processed[nn_indices[j]]) // Has this point been processed before ?
128  continue;
129 
130  //processed[nn_indices[j]] = true;
131  // [-1;1]
132 
133  double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0]
134  + normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] + normals[seed_queue[sq_idx]].normal[2]
135  * normals[nn_indices[j]].normal[2];
136 
137  if (std::abs (std::acos (dot_p)) < eps_angle)
138  {
139  processed[nn_indices[j]] = true;
140  seed_queue.push_back (nn_indices[j]);
141  }
142  }
143 
144  sq_idx++;
145  }
146 
147  // If this queue is satisfactory, add to the clusters
148  if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
149  {
151  r.indices.resize (seed_queue.size ());
152  for (std::size_t j = 0; j < seed_queue.size (); ++j)
153  r.indices[j] = seed_queue[j];
154 
155  std::sort (r.indices.begin (), r.indices.end ());
156  r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
157 
158  r.header = cloud.header;
159  clusters.push_back (r); // We could avoid a copy by working directly in the vector
160  }
161  }
162 }
163 
164 //////////////////////////////////////////////////////////////////////////////////////////////
165 template<typename PointInT, typename PointNT, typename PointOutT> void
167  std::vector<int> &indices_to_use,
168  std::vector<int> &indices_out, std::vector<int> &indices_in,
169  float threshold)
170 {
171  indices_out.resize (cloud.size ());
172  indices_in.resize (cloud.size ());
173 
174  std::size_t in, out;
175  in = out = 0;
176 
177  for (const int &index : indices_to_use)
178  {
179  if (cloud[index].curvature > threshold)
180  {
181  indices_out[out] = index;
182  out++;
183  }
184  else
185  {
186  indices_in[in] = index;
187  in++;
188  }
189  }
190 
191  indices_out.resize (out);
192  indices_in.resize (in);
193 }
194 
195 template<typename PointInT, typename PointNT, typename PointOutT> bool
196 pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid,
197  PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
198  PointInTPtr & grid, pcl::PointIndices & indices)
199 {
200 
201  Eigen::Vector3f plane_normal;
202  plane_normal[0] = -centroid[0];
203  plane_normal[1] = -centroid[1];
204  plane_normal[2] = -centroid[2];
205  Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
206  plane_normal.normalize ();
207  Eigen::Vector3f axis = plane_normal.cross (z_vector);
208  double rotation = -asin (axis.norm ());
209  axis.normalize ();
210 
211  Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
212 
213  grid->resize (processed->size ());
214  for (std::size_t k = 0; k < processed->size (); k++)
215  (*grid)[k].getVector4fMap () = (*processed)[k].getVector4fMap ();
216 
217  pcl::transformPointCloud (*grid, *grid, transformPC);
218 
219  Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
220  Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
221 
222  centroid4f = transformPC * centroid4f;
223  normal_centroid4f = transformPC * normal_centroid4f;
224 
225  Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
226 
227  Eigen::Vector4f farthest_away;
228  pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away);
229  farthest_away[3] = 0;
230 
231  float max_dist = (farthest_away - centroid4f).norm ();
232 
233  pcl::demeanPointCloud (*grid, centroid4f, *grid);
234 
235  Eigen::Matrix4f center_mat;
236  center_mat.setIdentity (4, 4);
237  center_mat (0, 3) = -centroid4f[0];
238  center_mat (1, 3) = -centroid4f[1];
239  center_mat (2, 3) = -centroid4f[2];
240 
241  Eigen::Matrix3f scatter;
242  scatter.setZero ();
243  float sum_w = 0.f;
244 
245  for (const int &index : indices.indices)
246  {
247  Eigen::Vector3f pvector = (*grid)[index].getVector3fMap ();
248  float d_k = (pvector).norm ();
249  float w = (max_dist - d_k);
250  Eigen::Vector3f diff = (pvector);
251  Eigen::Matrix3f mat = diff * diff.transpose ();
252  scatter += mat * w;
253  sum_w += w;
254  }
255 
256  scatter /= sum_w;
257 
258  Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
259  Eigen::Vector3f evx = svd.matrixV ().col (0);
260  Eigen::Vector3f evy = svd.matrixV ().col (1);
261  Eigen::Vector3f evz = svd.matrixV ().col (2);
262  Eigen::Vector3f evxminus = evx * -1;
263  Eigen::Vector3f evyminus = evy * -1;
264  Eigen::Vector3f evzminus = evz * -1;
265 
266  float s_xplus, s_xminus, s_yplus, s_yminus;
267  s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
268 
269  //disambiguate rf using all points
270  for (const auto& point: grid->points)
271  {
272  Eigen::Vector3f pvector = point.getVector3fMap ();
273  float dist_x, dist_y;
274  dist_x = std::abs (evx.dot (pvector));
275  dist_y = std::abs (evy.dot (pvector));
276 
277  if ((pvector).dot (evx) >= 0)
278  s_xplus += dist_x;
279  else
280  s_xminus += dist_x;
281 
282  if ((pvector).dot (evy) >= 0)
283  s_yplus += dist_y;
284  else
285  s_yminus += dist_y;
286 
287  }
288 
289  if (s_xplus < s_xminus)
290  evx = evxminus;
291 
292  if (s_yplus < s_yminus)
293  evy = evyminus;
294 
295  //select the axis that could be disambiguated more easily
296  float fx, fy;
297  float max_x = static_cast<float> (std::max (s_xplus, s_xminus));
298  float min_x = static_cast<float> (std::min (s_xplus, s_xminus));
299  float max_y = static_cast<float> (std::max (s_yplus, s_yminus));
300  float min_y = static_cast<float> (std::min (s_yplus, s_yminus));
301 
302  fx = (min_x / max_x);
303  fy = (min_y / max_y);
304 
305  Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
306  if (normal3f.dot (evz) < 0)
307  evz = evzminus;
308 
309  //if fx/y close to 1, it was hard to disambiguate
310  //what if both are equally easy or difficult to disambiguate, namely fy == fx or very close
311 
312  float max_axis = std::max (fx, fy);
313  float min_axis = std::min (fx, fy);
314 
315  if ((min_axis / max_axis) > axis_ratio_)
316  {
317  PCL_WARN ("Both axes are equally easy/difficult to disambiguate\n");
318 
319  Eigen::Vector3f evy_copy = evy;
320  Eigen::Vector3f evxminus = evx * -1;
321  Eigen::Vector3f evyminus = evy * -1;
322 
323  if (min_axis > min_axis_value_)
324  {
325  //combination of all possibilities
326  evy = evx.cross (evz);
327  Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
328  transformations.push_back (trans);
329 
330  evx = evxminus;
331  evy = evx.cross (evz);
332  trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
333  transformations.push_back (trans);
334 
335  evx = evy_copy;
336  evy = evx.cross (evz);
337  trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
338  transformations.push_back (trans);
339 
340  evx = evyminus;
341  evy = evx.cross (evz);
342  trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
343  transformations.push_back (trans);
344 
345  }
346  else
347  {
348  //1-st case (evx selected)
349  evy = evx.cross (evz);
350  Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
351  transformations.push_back (trans);
352 
353  //2-nd case (evy selected)
354  evx = evy_copy;
355  evy = evx.cross (evz);
356  trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
357  transformations.push_back (trans);
358  }
359  }
360  else
361  {
362  if (fy < fx)
363  {
364  evx = evy;
365  fx = fy;
366  }
367 
368  evy = evx.cross (evz);
369  Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
370  transformations.push_back (trans);
371 
372  }
373 
374  return true;
375 }
376 
377 //////////////////////////////////////////////////////////////////////////////////////////////
378 template<typename PointInT, typename PointNT, typename PointOutT> void
380  std::vector<pcl::PointIndices> & cluster_indices)
381 {
382  PointCloudOut ourcvfh_output;
383 
384  cluster_axes_.clear ();
385  cluster_axes_.resize (centroids_dominant_orientations_.size ());
386 
387  for (std::size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
388  {
389 
390  std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
392  sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
393 
394  // Make a note of how many transformations correspond to each cluster
395  cluster_axes_[i] = transformations.size ();
396 
397  for (const auto &transformation : transformations)
398  {
399 
400  pcl::transformPointCloud (*processed, *grid, transformation);
401  transforms_.push_back (transformation);
402  valid_transforms_.push_back (true);
403 
404  std::vector < Eigen::VectorXf > quadrants (8);
405  int size_hists = 13;
406  int num_hists = 8;
407  for (int k = 0; k < num_hists; k++)
408  quadrants[k].setZero (size_hists);
409 
410  Eigen::Vector4f centroid_p;
411  centroid_p.setZero ();
412  Eigen::Vector4f max_pt;
413  pcl::getMaxDistance (*grid, centroid_p, max_pt);
414  max_pt[3] = 0;
415  double distance_normalization_factor = (centroid_p - max_pt).norm ();
416 
417  float hist_incr;
418  if (normalize_bins_)
419  hist_incr = 100.0f / static_cast<float> (grid->size () - 1);
420  else
421  hist_incr = 1.0f;
422 
423  float * weights = new float[num_hists];
424  float sigma = 0.01f; //1cm
425  float sigma_sq = sigma * sigma;
426 
427  for (const auto& point: grid->points)
428  {
429  Eigen::Vector4f p = point.getVector4fMap ();
430  p[3] = 0.f;
431  float d = p.norm ();
432 
433  //compute weight for all octants
434  float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq))); //how is the weight distributed among two semi-cubes
435  float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
436  float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
437 
438  //distribute the weights using the x-coordinate
439  if (p[0] >= 0)
440  {
441  for (std::size_t ii = 0; ii <= 3; ii++)
442  weights[ii] = 0.5f - wx * 0.5f;
443 
444  for (std::size_t ii = 4; ii <= 7; ii++)
445  weights[ii] = 0.5f + wx * 0.5f;
446  }
447  else
448  {
449  for (std::size_t ii = 0; ii <= 3; ii++)
450  weights[ii] = 0.5f + wx * 0.5f;
451 
452  for (std::size_t ii = 4; ii <= 7; ii++)
453  weights[ii] = 0.5f - wx * 0.5f;
454  }
455 
456  //distribute the weights using the y-coordinate
457  if (p[1] >= 0)
458  {
459  for (std::size_t ii = 0; ii <= 1; ii++)
460  weights[ii] *= 0.5f - wy * 0.5f;
461  for (std::size_t ii = 4; ii <= 5; ii++)
462  weights[ii] *= 0.5f - wy * 0.5f;
463 
464  for (std::size_t ii = 2; ii <= 3; ii++)
465  weights[ii] *= 0.5f + wy * 0.5f;
466 
467  for (std::size_t ii = 6; ii <= 7; ii++)
468  weights[ii] *= 0.5f + wy * 0.5f;
469  }
470  else
471  {
472  for (std::size_t ii = 0; ii <= 1; ii++)
473  weights[ii] *= 0.5f + wy * 0.5f;
474  for (std::size_t ii = 4; ii <= 5; ii++)
475  weights[ii] *= 0.5f + wy * 0.5f;
476 
477  for (std::size_t ii = 2; ii <= 3; ii++)
478  weights[ii] *= 0.5f - wy * 0.5f;
479 
480  for (std::size_t ii = 6; ii <= 7; ii++)
481  weights[ii] *= 0.5f - wy * 0.5f;
482  }
483 
484  //distribute the weights using the z-coordinate
485  if (p[2] >= 0)
486  {
487  for (std::size_t ii = 0; ii <= 7; ii += 2)
488  weights[ii] *= 0.5f - wz * 0.5f;
489 
490  for (std::size_t ii = 1; ii <= 7; ii += 2)
491  weights[ii] *= 0.5f + wz * 0.5f;
492 
493  }
494  else
495  {
496  for (std::size_t ii = 0; ii <= 7; ii += 2)
497  weights[ii] *= 0.5f + wz * 0.5f;
498 
499  for (std::size_t ii = 1; ii <= 7; ii += 2)
500  weights[ii] *= 0.5f - wz * 0.5f;
501  }
502 
503  int h_index = (d <= 0) ? 0 : std::ceil (size_hists * (d / distance_normalization_factor)) - 1;
504  /* from http://www.pcl-users.org/OUR-CVFH-problem-td4028436.html
505  h_index will be 13 when d is computed on the farthest away point.
506 
507  adding the following after computing h_index fixes the problem:
508  */
509  if(h_index > 12)
510  h_index = 12;
511  for (int j = 0; j < num_hists; j++)
512  quadrants[j][h_index] += hist_incr * weights[j];
513 
514  }
515 
516  //copy to the cvfh signature
517  PointCloudOut vfh_signature;
518  vfh_signature.points.resize (1);
519  vfh_signature.width = vfh_signature.height = 1;
520  for (int d = 0; d < 308; ++d)
521  vfh_signature[0].histogram[d] = output[i].histogram[d];
522 
523  int pos = 45 * 3;
524  for (int k = 0; k < num_hists; k++)
525  {
526  for (int ii = 0; ii < size_hists; ii++, pos++)
527  {
528  vfh_signature[0].histogram[pos] = quadrants[k][ii];
529  }
530  }
531 
532  ourcvfh_output.push_back (vfh_signature[0]);
533  ourcvfh_output.width = ourcvfh_output.size ();
534  delete[] weights;
535  }
536  }
537 
538  if (!ourcvfh_output.points.empty ())
539  {
540  ourcvfh_output.height = 1;
541  }
542  output = ourcvfh_output;
543 }
544 
545 //////////////////////////////////////////////////////////////////////////////////////////////
546 template<typename PointInT, typename PointNT, typename PointOutT> void
548 {
549  if (refine_clusters_ <= 0.f)
550  refine_clusters_ = 1.f;
551 
552  // Check if input was set
553  if (!normals_)
554  {
555  PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
556  output.width = output.height = 0;
557  output.points.clear ();
558  return;
559  }
560  if (normals_->size () != surface_->size ())
561  {
562  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 ());
563  output.width = output.height = 0;
564  output.points.clear ();
565  return;
566  }
567 
568  centroids_dominant_orientations_.clear ();
569  clusters_.clear ();
570  transforms_.clear ();
571  dominant_normals_.clear ();
572 
573  // ---[ Step 0: remove normals with high curvature
574  std::vector<int> indices_out;
575  std::vector<int> indices_in;
576  filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);
577 
579  normals_filtered_cloud->width = indices_in.size ();
580  normals_filtered_cloud->height = 1;
581  normals_filtered_cloud->points.resize (normals_filtered_cloud->width);
582 
583  std::vector<int> indices_from_nfc_to_indices;
584  indices_from_nfc_to_indices.resize (indices_in.size ());
585 
586  for (std::size_t i = 0; i < indices_in.size (); ++i)
587  {
588  (*normals_filtered_cloud)[i].x = (*surface_)[indices_in[i]].x;
589  (*normals_filtered_cloud)[i].y = (*surface_)[indices_in[i]].y;
590  (*normals_filtered_cloud)[i].z = (*surface_)[indices_in[i]].z;
591  //(*normals_filtered_cloud)[i].getNormalVector4fMap() = (*normals_)[indices_in[i]].getNormalVector4fMap();
592  indices_from_nfc_to_indices[i] = indices_in[i];
593  }
594 
595  std::vector<pcl::PointIndices> clusters;
596 
597  if (normals_filtered_cloud->size () >= min_points_)
598  {
599  //recompute normals and use them for clustering
600  {
601  KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
602  normals_tree_filtered->setInputCloud (normals_filtered_cloud);
604  n3d.setRadiusSearch (radius_normals_);
605  n3d.setSearchMethod (normals_tree_filtered);
606  n3d.setInputCloud (normals_filtered_cloud);
607  n3d.compute (*normals_filtered_cloud);
608  }
609 
610  KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
611  normals_tree->setInputCloud (normals_filtered_cloud);
612 
613  extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
614  eps_angle_threshold_, static_cast<unsigned int> (min_points_));
615 
616  std::vector<pcl::PointIndices> clusters_filtered;
617  int cluster_filtered_idx = 0;
618  for (const auto &cluster : clusters)
619  {
620 
622  pcl::PointIndices pi_cvfh;
623  pcl::PointIndices pi_filtered;
624 
625  clusters_.push_back (pi);
626  clusters_filtered.push_back (pi_filtered);
627 
628  Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
629  Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
630 
631  for (const auto &index : cluster.indices)
632  {
633  avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
634  avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
635  }
636 
637  avg_normal /= static_cast<float> (cluster.indices.size ());
638  avg_centroid /= static_cast<float> (cluster.indices.size ());
639  avg_normal.normalize ();
640 
641  Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
642  Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
643 
644  for (const auto &index : cluster.indices)
645  {
646  //decide if normal should be added
647  double dot_p = avg_normal.dot ((*normals_filtered_cloud)[index].getNormalVector4fMap ());
648  if (std::abs (std::acos (dot_p)) < (eps_angle_threshold_ * refine_clusters_))
649  {
650  clusters_[cluster_filtered_idx].indices.push_back (indices_from_nfc_to_indices[index]);
651  clusters_filtered[cluster_filtered_idx].indices.push_back (index);
652  }
653  }
654 
655  //remove last cluster if no points found...
656  if (clusters_[cluster_filtered_idx].indices.empty ())
657  {
658  clusters_.pop_back ();
659  clusters_filtered.pop_back ();
660  }
661  else
662  cluster_filtered_idx++;
663  }
664 
665  clusters = clusters_filtered;
666 
667  }
668 
670  vfh.setInputCloud (surface_);
671  vfh.setInputNormals (normals_);
672  vfh.setIndices (indices_);
673  vfh.setSearchMethod (this->tree_);
674  vfh.setUseGivenNormal (true);
675  vfh.setUseGivenCentroid (true);
676  vfh.setNormalizeBins (normalize_bins_);
677  output.height = 1;
678 
679  // ---[ Step 1b : check if any dominant cluster was found
680  if (!clusters.empty ())
681  { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information
682  for (const auto &cluster : clusters) //for each cluster
683  {
684  Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
685  Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();
686 
687  for (const auto &index : cluster.indices)
688  {
689  avg_normal += (*normals_filtered_cloud)[index].getNormalVector4fMap ();
690  avg_centroid += (*normals_filtered_cloud)[index].getVector4fMap ();
691  }
692 
693  avg_normal /= static_cast<float> (cluster.indices.size ());
694  avg_centroid /= static_cast<float> (cluster.indices.size ());
695  avg_normal.normalize ();
696 
697  //append normal and centroid for the clusters
698  dominant_normals_.emplace_back (avg_normal[0], avg_normal[1], avg_normal[2]);
699  centroids_dominant_orientations_.emplace_back (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
700  }
701 
702  //compute modified VFH for all dominant clusters and add them to the list!
703  output.points.resize (dominant_normals_.size ());
704  output.width = dominant_normals_.size ();
705 
706  for (std::size_t i = 0; i < dominant_normals_.size (); ++i)
707  {
708  //configure VFH computation for CVFH
709  vfh.setNormalToUse (dominant_normals_[i]);
710  vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
712  vfh.compute (vfh_signature);
713  output[i] = vfh_signature[0];
714  }
715 
716  //finish filling the descriptor with the shape distribution
717  PointInTPtr cloud_input (new pcl::PointCloud<PointInT>);
718  pcl::copyPointCloud (*surface_, *indices_, *cloud_input);
719  computeRFAndShapeDistribution (cloud_input, output, clusters_); //this will set transforms_
720  }
721  else
722  { // ---[ Step 1b.1 : If no, compute a VFH using all the object points
723 
724  PCL_WARN("No clusters were found in the surface... using VFH...\n");
725  Eigen::Vector4f avg_centroid;
726  pcl::compute3DCentroid (*surface_, avg_centroid);
727  Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
728  centroids_dominant_orientations_.push_back (cloud_centroid);
729 
730  //configure VFH computation using all object points
731  vfh.setCentroidToUse (cloud_centroid);
732  vfh.setUseGivenNormal (false);
733 
735  vfh.compute (vfh_signature);
736 
737  output.points.resize (1);
738  output.width = 1;
739 
740  output[0] = vfh_signature[0];
741  Eigen::Matrix4f id = Eigen::Matrix4f::Identity ();
742  transforms_.push_back (id);
743  valid_transforms_.push_back (false);
744  }
745 }
746 
747 #define PCL_INSTANTIATE_OURCVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::OURCVFHEstimation<T,NT,OutT>;
748 
749 #endif // PCL_FEATURES_IMPL_OURCVFH_H_
void setUseGivenCentroid(bool use)
Set use_given_centroid_.
Definition: vfh.h:164
void compute(PointCloudOut &output)
Overloaded computed method from pcl::Feature.
Definition: our_cvfh.hpp:51
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
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: our_cvfh.hpp:166
typename pcl::PointCloud< PointInT >::Ptr PointInTPtr
Definition: our_cvfh.h:76
std::size_t size() const
Definition: point_cloud.h:459
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:566
void computeRFAndShapeDistribution(PointInTPtr &processed, PointCloudOut &output, std::vector< pcl::PointIndices > &cluster_indices)
Computes SGURF and the shape distribution based on the selected SGURF.
Definition: our_cvfh.hpp:379
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...
Definition: feature.h:201
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:243
void getMaxDistance(const pcl::PointCloud< PointT > &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
Get the point at maximum distance from a given point and a given pointcloud.
Definition: common.hpp:145
void setUseGivenNormal(bool use)
Set use_given_normal_.
Definition: vfh.h:145
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
void setNormalToUse(const Eigen::Vector3f &normal)
Set the normal to use.
Definition: vfh.h:155
void setCentroidToUse(const Eigen::Vector3f &centroid)
Set centroid_to_use_.
Definition: vfh.h:174
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:121
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:221
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > &centroid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Definition: centroid.hpp:627
::pcl::PCLHeader header
Definition: PointIndices.h:21
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:691
VFHEstimation estimates the Viewpoint Feature Histogram (VFH) descriptor for a given point cloud data...
Definition: vfh.h:72
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
void setNormalizeBins(bool normalize)
set normalize_bins_
Definition: vfh.h:183
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:408
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
bool sgurf(Eigen::Vector3f &centroid, Eigen::Vector3f &normal_centroid, PointInTPtr &processed, std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations, PointInTPtr &grid, pcl::PointIndices &indices)
Computes SGURF.
Definition: our_cvfh.hpp:196
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:167
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: search.h:125
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
Feature represents the base feature class.
Definition: feature.h:106
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:193
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
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:332
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
Definition: feature.h:345
OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram ...
Definition: our_cvfh.h:61