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