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