Point Cloud Library (PCL)  1.14.1
implicit_shape_model.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  *
35  * Implementation of the ISM algorithm described in "Hough Transforms and 3D SURF for robust three dimensional classification"
36  * by Jan Knopp, Mukta Prasad, Geert Willems, Radu Timofte, and Luc Van Gool
37  *
38  * Authors: Roman Shapovalov, Alexander Velizhev, Sergey Ushakov
39  */
40 
41 #ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_
42 #define PCL_IMPLICIT_SHAPE_MODEL_HPP_
43 
44 #include "../implicit_shape_model.h"
45 #include <pcl/filters/voxel_grid.h> // for VoxelGrid
46 #include <pcl/filters/extract_indices.h> // for ExtractIndices
47 
48 #include <pcl/memory.h> // for dynamic_pointer_cast
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointT>
53 
54 //////////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointT>
57 {
58  votes_class_.clear ();
59  votes_origins_.reset ();
60  votes_.reset ();
61  k_ind_.clear ();
62  k_sqr_dist_.clear ();
63  tree_.reset ();
64 }
65 
66 //////////////////////////////////////////////////////////////////////////////////////////////
67 template <typename PointT> void
69  pcl::InterestPoint& vote, const PointT &vote_origin, int votes_class)
70 {
71  tree_is_valid_ = false;
72  votes_->points.insert (votes_->points.end (), vote);// TODO: adjust height and width
73 
74  votes_origins_->points.push_back (vote_origin);
75  votes_class_.push_back (votes_class);
76 }
77 
78 //////////////////////////////////////////////////////////////////////////////////////////////
79 template <typename PointT> typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr
81 {
82  pcl::PointXYZRGB point;
84  colored_cloud->height = 0;
85  colored_cloud->width = 1;
86 
87  if (cloud != nullptr)
88  {
89  colored_cloud->height += cloud->size ();
90  point.r = 255;
91  point.g = 255;
92  point.b = 255;
93  for (const auto& i_point: *cloud)
94  {
95  point.x = i_point.x;
96  point.y = i_point.y;
97  point.z = i_point.z;
98  colored_cloud->points.push_back (point);
99  }
100  }
101 
102  point.r = 0;
103  point.g = 0;
104  point.b = 255;
105  for (const auto &i_vote : votes_->points)
106  {
107  point.x = i_vote.x;
108  point.y = i_vote.y;
109  point.z = i_vote.z;
110  colored_cloud->points.push_back (point);
111  }
112  colored_cloud->height += votes_->size ();
113 
114  return (colored_cloud);
115 }
116 
117 //////////////////////////////////////////////////////////////////////////////////////////////
118 template <typename PointT> void
120  std::vector<pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
121  int in_class_id,
122  double in_non_maxima_radius,
123  double in_sigma)
124 {
125  validateTree ();
126 
127  const std::size_t n_vote_classes = votes_class_.size ();
128  if (n_vote_classes == 0)
129  return;
130  for (std::size_t i = 0; i < n_vote_classes ; i++)
131  assert ( votes_class_[i] == in_class_id );
132 
133  // heuristic: start from NUM_INIT_PTS different locations selected uniformly
134  // on the votes. Intuitively, it is likely to get a good location in dense regions.
135  constexpr int NUM_INIT_PTS = 100;
136  double SIGMA_DIST = in_sigma;// rule of thumb: 10% of the object radius
137  const double FINAL_EPS = SIGMA_DIST / 100;// another heuristic
138 
139  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
140  std::vector<double> peak_densities (NUM_INIT_PTS);
141  double max_density = -1.0;
142  for (int i = 0; i < NUM_INIT_PTS; i++)
143  {
144  Eigen::Vector3f old_center;
145  const auto idx = votes_->size() * i / NUM_INIT_PTS;
146  Eigen::Vector3f curr_center = (*votes_)[idx].getVector3fMap();
147 
148  do
149  {
150  old_center = curr_center;
151  curr_center = shiftMean (old_center, SIGMA_DIST);
152  } while ((old_center - curr_center).norm () > FINAL_EPS);
153 
154  pcl::PointXYZ point;
155  point.x = curr_center (0);
156  point.y = curr_center (1);
157  point.z = curr_center (2);
158  double curr_density = getDensityAtPoint (point, SIGMA_DIST);
159  assert (curr_density >= 0.0);
160 
161  peaks[i] = curr_center;
162  peak_densities[i] = curr_density;
163 
164  if ( max_density < curr_density )
165  max_density = curr_density;
166  }
167 
168  //extract peaks
169  std::vector<bool> peak_flag (NUM_INIT_PTS, true);
170  for (int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
171  {
172  // find best peak with taking into consideration peak flags
173  double best_density = -1.0;
174  Eigen::Vector3f strongest_peak;
175  int best_peak_ind (-1);
176  int peak_counter (0);
177  for (int i = 0; i < NUM_INIT_PTS; i++)
178  {
179  if ( !peak_flag[i] )
180  continue;
181 
182  if ( peak_densities[i] > best_density)
183  {
184  best_density = peak_densities[i];
185  strongest_peak = peaks[i];
186  best_peak_ind = i;
187  ++peak_counter;
188  }
189  }
190 
191  if( peak_counter == 0 )
192  break;// no peaks
193 
194  pcl::ISMPeak peak;
195  peak.x = strongest_peak(0);
196  peak.y = strongest_peak(1);
197  peak.z = strongest_peak(2);
198  peak.density = best_density;
199  peak.class_id = in_class_id;
200  out_peaks.push_back ( peak );
201 
202  // mark best peaks and all its neighbors
203  peak_flag[best_peak_ind] = false;
204  for (int i = 0; i < NUM_INIT_PTS; i++)
205  {
206  // compute distance between best peak and all unmarked peaks
207  if ( !peak_flag[i] )
208  continue;
209 
210  double dist = (strongest_peak - peaks[i]).norm ();
211  if ( dist < in_non_maxima_radius )
212  peak_flag[i] = false;
213  }
214  }
215 }
216 
217 //////////////////////////////////////////////////////////////////////////////////////////////
218 template <typename PointT> void
220 {
221  if (!tree_is_valid_)
222  {
223  if (tree_ == nullptr)
224  tree_.reset (new pcl::KdTreeFLANN<pcl::InterestPoint>);
225  tree_->setInputCloud (votes_);
226  k_ind_.resize ( votes_->size (), -1 );
227  k_sqr_dist_.resize ( votes_->size (), 0.0f );
228  tree_is_valid_ = true;
229  }
230 }
231 
232 //////////////////////////////////////////////////////////////////////////////////////////////
233 template <typename PointT> Eigen::Vector3f
234 pcl::features::ISMVoteList<PointT>::shiftMean (const Eigen::Vector3f& snap_pt, const double in_sigma_dist)
235 {
236  validateTree ();
237 
238  Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
239  double denom = 0.0;
240 
242  pt.x = snap_pt[0];
243  pt.y = snap_pt[1];
244  pt.z = snap_pt[2];
245  std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
246 
247  for (std::size_t j = 0; j < n_pts; j++)
248  {
249  double kernel = (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
250  Eigen::Vector3f vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[k_ind_[j]].z);
251  wgh_sum += vote_vec * static_cast<float> (kernel);
252  denom += kernel;
253  }
254  assert (denom > 0.0); // at least one point is close. In fact, this case should be handled too
255 
256  return (wgh_sum / static_cast<float> (denom));
257 }
258 
259 //////////////////////////////////////////////////////////////////////////////////////////////
260 template <typename PointT> double
262  const PointT &point, double sigma_dist)
263 {
264  validateTree ();
265 
266  const std::size_t n_vote_classes = votes_class_.size ();
267  if (n_vote_classes == 0)
268  return (0.0);
269 
270  double sum_vote = 0.0;
271 
273  pt.x = point.x;
274  pt.y = point.y;
275  pt.z = point.z;
276  std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
277 
278  for (std::size_t j = 0; j < num_of_pts; j++)
279  sum_vote += (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
280 
281  return (sum_vote);
282 }
283 
284 //////////////////////////////////////////////////////////////////////////////////////////////
285 template <typename PointT> unsigned int
287 {
288  return (static_cast<unsigned int> (votes_->size ()));
289 }
290 
291 //////////////////////////////////////////////////////////////////////////////////////////////
293 
294 //////////////////////////////////////////////////////////////////////////////////////////////
296 {
297  reset ();
298 
303 
304  std::vector<float> vec;
305  vec.resize (this->number_of_clusters_, 0.0f);
306  this->statistical_weights_.resize (this->number_of_classes_, vec);
307  for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
308  for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
309  this->statistical_weights_[i_class][i_cluster] = copy.statistical_weights_[i_class][i_cluster];
310 
311  this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
312  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
313  this->learned_weights_[i_visual_word] = copy.learned_weights_[i_visual_word];
314 
315  this->classes_.resize (this->number_of_visual_words_, 0);
316  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
317  this->classes_[i_visual_word] = copy.classes_[i_visual_word];
318 
319  this->sigmas_.resize (this->number_of_classes_, 0.0f);
320  for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
321  this->sigmas_[i_class] = copy.sigmas_[i_class];
322 
323  this->directions_to_center_.resize (this->number_of_visual_words_, 3);
324  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
325  for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
326  this->directions_to_center_ (i_visual_word, i_dim) = copy.directions_to_center_ (i_visual_word, i_dim);
327 
328  this->clusters_centers_.resize (this->number_of_clusters_, 3);
329  for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
330  for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
331  this->clusters_centers_ (i_cluster, i_dim) = copy.clusters_centers_ (i_cluster, i_dim);
332 }
333 
334 //////////////////////////////////////////////////////////////////////////////////////////////
336 {
337  reset ();
338 }
339 
340 //////////////////////////////////////////////////////////////////////////////////////////////
341 bool
343 {
344  std::ofstream output_file (file_name.c_str (), std::ios::trunc);
345  if (!output_file)
346  {
347  output_file.close ();
348  return (false);
349  }
350 
351  output_file << number_of_classes_ << " ";
352  output_file << number_of_visual_words_ << " ";
353  output_file << number_of_clusters_ << " ";
354  output_file << descriptors_dimension_ << " ";
355 
356  //write statistical weights
357  for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
358  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
359  output_file << statistical_weights_[i_class][i_cluster] << " ";
360 
361  //write learned weights
362  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
363  output_file << learned_weights_[i_visual_word] << " ";
364 
365  //write classes
366  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
367  output_file << classes_[i_visual_word] << " ";
368 
369  //write sigmas
370  for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
371  output_file << sigmas_[i_class] << " ";
372 
373  //write directions to centers
374  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
375  for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
376  output_file << directions_to_center_ (i_visual_word, i_dim) << " ";
377 
378  //write clusters centers
379  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
380  for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
381  output_file << clusters_centers_ (i_cluster, i_dim) << " ";
382 
383  //write clusters
384  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
385  {
386  output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) << " ";
387  for (const unsigned int &visual_word : clusters_[i_cluster])
388  output_file << visual_word << " ";
389  }
390 
391  output_file.close ();
392  return (true);
393 }
394 
395 //////////////////////////////////////////////////////////////////////////////////////////////
396 bool
398 {
399  reset ();
400  std::ifstream input_file (file_name.c_str ());
401  if (!input_file)
402  {
403  input_file.close ();
404  return (false);
405  }
406 
407  char line[256];
408 
409  input_file.getline (line, 256, ' ');
410  number_of_classes_ = static_cast<unsigned int> (strtol (line, nullptr, 10));
411  input_file.getline (line, 256, ' '); number_of_visual_words_ = atoi (line);
412  input_file.getline (line, 256, ' '); number_of_clusters_ = atoi (line);
413  input_file.getline (line, 256, ' '); descriptors_dimension_ = atoi (line);
414 
415  //read statistical weights
416  std::vector<float> vec;
417  vec.resize (number_of_clusters_, 0.0f);
418  statistical_weights_.resize (number_of_classes_, vec);
419  for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
420  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
421  input_file >> statistical_weights_[i_class][i_cluster];
422 
423  //read learned weights
424  learned_weights_.resize (number_of_visual_words_, 0.0f);
425  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
426  input_file >> learned_weights_[i_visual_word];
427 
428  //read classes
429  classes_.resize (number_of_visual_words_, 0);
430  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
431  input_file >> classes_[i_visual_word];
432 
433  //read sigmas
434  sigmas_.resize (number_of_classes_, 0.0f);
435  for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
436  input_file >> sigmas_[i_class];
437 
438  //read directions to centers
439  directions_to_center_.resize (number_of_visual_words_, 3);
440  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
441  for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
442  input_file >> directions_to_center_ (i_visual_word, i_dim);
443 
444  //read clusters centers
445  clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
446  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
447  for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
448  input_file >> clusters_centers_ (i_cluster, i_dim);
449 
450  //read clusters
451  std::vector<unsigned int> vect;
452  clusters_.resize (number_of_clusters_, vect);
453  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
454  {
455  unsigned int size_of_current_cluster = 0;
456  input_file >> size_of_current_cluster;
457  clusters_[i_cluster].resize (size_of_current_cluster, 0);
458  for (unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
459  input_file >> clusters_[i_cluster][i_visual_word];
460  }
461 
462  input_file.close ();
463  return (true);
464 }
465 
466 //////////////////////////////////////////////////////////////////////////////////////////////
467 void
469 {
470  statistical_weights_.clear ();
471  learned_weights_.clear ();
472  classes_.clear ();
473  sigmas_.clear ();
474  directions_to_center_.resize (0, 0);
475  clusters_centers_.resize (0, 0);
476  clusters_.clear ();
477  number_of_classes_ = 0;
478  number_of_visual_words_ = 0;
479  number_of_clusters_ = 0;
480  descriptors_dimension_ = 0;
481 }
482 
483 //////////////////////////////////////////////////////////////////////////////////////////////
486 {
487  if (this != &other)
488  {
489  this->reset ();
490 
491  this->number_of_classes_ = other.number_of_classes_;
492  this->number_of_visual_words_ = other.number_of_visual_words_;
493  this->number_of_clusters_ = other.number_of_clusters_;
494  this->descriptors_dimension_ = other.descriptors_dimension_;
495 
496  std::vector<float> vec;
497  vec.resize (number_of_clusters_, 0.0f);
498  this->statistical_weights_.resize (this->number_of_classes_, vec);
499  for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
500  for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
501  this->statistical_weights_[i_class][i_cluster] = other.statistical_weights_[i_class][i_cluster];
502 
503  this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
504  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
505  this->learned_weights_[i_visual_word] = other.learned_weights_[i_visual_word];
506 
507  this->classes_.resize (this->number_of_visual_words_, 0);
508  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
509  this->classes_[i_visual_word] = other.classes_[i_visual_word];
510 
511  this->sigmas_.resize (this->number_of_classes_, 0.0f);
512  for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
513  this->sigmas_[i_class] = other.sigmas_[i_class];
514 
515  this->directions_to_center_.resize (this->number_of_visual_words_, 3);
516  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
517  for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
518  this->directions_to_center_ (i_visual_word, i_dim) = other.directions_to_center_ (i_visual_word, i_dim);
519 
520  this->clusters_centers_.resize (this->number_of_clusters_, 3);
521  for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
522  for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
523  this->clusters_centers_ (i_cluster, i_dim) = other.clusters_centers_ (i_cluster, i_dim);
524  }
525  return (*this);
526 }
527 
528 //////////////////////////////////////////////////////////////////////////////////////////////
529 template <int FeatureSize, typename PointT, typename NormalT>
531 
532 //////////////////////////////////////////////////////////////////////////////////////////////
533 template <int FeatureSize, typename PointT, typename NormalT>
535 {
536  training_clouds_.clear ();
537  training_classes_.clear ();
538  training_normals_.clear ();
539  training_sigmas_.clear ();
540  feature_estimator_.reset ();
541 }
542 
543 //////////////////////////////////////////////////////////////////////////////////////////////
544 template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
546 {
547  return (training_clouds_);
548 }
549 
550 //////////////////////////////////////////////////////////////////////////////////////////////
551 template <int FeatureSize, typename PointT, typename NormalT> void
553  const std::vector< typename pcl::PointCloud<PointT>::Ptr >& training_clouds)
554 {
555  training_clouds_.clear ();
556  std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
557  training_clouds_.swap (clouds);
558 }
559 
560 //////////////////////////////////////////////////////////////////////////////////////////////
561 template <int FeatureSize, typename PointT, typename NormalT> std::vector<unsigned int>
563 {
564  return (training_classes_);
565 }
566 
567 //////////////////////////////////////////////////////////////////////////////////////////////
568 template <int FeatureSize, typename PointT, typename NormalT> void
570 {
571  training_classes_.clear ();
572  std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
573  training_classes_.swap (classes);
574 }
575 
576 //////////////////////////////////////////////////////////////////////////////////////////////
577 template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
579 {
580  return (training_normals_);
581 }
582 
583 //////////////////////////////////////////////////////////////////////////////////////////////
584 template <int FeatureSize, typename PointT, typename NormalT> void
586  const std::vector< typename pcl::PointCloud<NormalT>::Ptr >& training_normals)
587 {
588  training_normals_.clear ();
589  std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
590  training_normals_.swap (normals);
591 }
592 
593 //////////////////////////////////////////////////////////////////////////////////////////////
594 template <int FeatureSize, typename PointT, typename NormalT> float
596 {
597  return (sampling_size_);
598 }
599 
600 //////////////////////////////////////////////////////////////////////////////////////////////
601 template <int FeatureSize, typename PointT, typename NormalT> void
603 {
604  if (sampling_size >= std::numeric_limits<float>::epsilon ())
605  sampling_size_ = sampling_size;
606 }
607 
608 //////////////////////////////////////////////////////////////////////////////////////////////
609 template <int FeatureSize, typename PointT, typename NormalT> typename pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::FeaturePtr
611 {
612  return (feature_estimator_);
613 }
614 
615 //////////////////////////////////////////////////////////////////////////////////////////////
616 template <int FeatureSize, typename PointT, typename NormalT> void
618 {
619  feature_estimator_ = feature;
620 }
621 
622 //////////////////////////////////////////////////////////////////////////////////////////////
623 template <int FeatureSize, typename PointT, typename NormalT> unsigned int
625 {
626  return (number_of_clusters_);
627 }
628 
629 //////////////////////////////////////////////////////////////////////////////////////////////
630 template <int FeatureSize, typename PointT, typename NormalT> void
632 {
633  if (num_of_clusters > 0)
634  number_of_clusters_ = num_of_clusters;
635 }
636 
637 //////////////////////////////////////////////////////////////////////////////////////////////
638 template <int FeatureSize, typename PointT, typename NormalT> std::vector<float>
640 {
641  return (training_sigmas_);
642 }
643 
644 //////////////////////////////////////////////////////////////////////////////////////////////
645 template <int FeatureSize, typename PointT, typename NormalT> void
647 {
648  training_sigmas_.clear ();
649  std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
650  training_sigmas_.swap (sigmas);
651 }
652 
653 //////////////////////////////////////////////////////////////////////////////////////////////
654 template <int FeatureSize, typename PointT, typename NormalT> bool
656 {
657  return (n_vot_ON_);
658 }
659 
660 //////////////////////////////////////////////////////////////////////////////////////////////
661 template <int FeatureSize, typename PointT, typename NormalT> void
663 {
664  n_vot_ON_ = state;
665 }
666 
667 //////////////////////////////////////////////////////////////////////////////////////////////
668 template <int FeatureSize, typename PointT, typename NormalT> bool
670 {
671  bool success = true;
672 
673  if (trained_model == nullptr)
674  return (false);
675  trained_model->reset ();
676 
677  std::vector<pcl::Histogram<FeatureSize> > histograms;
678  std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
679  success = extractDescriptors (histograms, locations);
680  if (!success)
681  return (false);
682 
683  Eigen::MatrixXi labels;
684  success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
685  if (!success)
686  return (false);
687 
688  std::vector<unsigned int> vec;
689  trained_model->clusters_.resize (number_of_clusters_, vec);
690  for (std::size_t i_label = 0; i_label < locations.size (); i_label++)
691  trained_model->clusters_[labels (i_label)].push_back (i_label);
692 
693  calculateSigmas (trained_model->sigmas_);
694 
695  calculateWeights(
696  locations,
697  labels,
698  trained_model->sigmas_,
699  trained_model->clusters_,
700  trained_model->statistical_weights_,
701  trained_model->learned_weights_);
702 
703  trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
704  trained_model->number_of_visual_words_ = static_cast<unsigned int> (histograms.size ());
705  trained_model->number_of_clusters_ = number_of_clusters_;
706  trained_model->descriptors_dimension_ = FeatureSize;
707 
708  trained_model->directions_to_center_.resize (locations.size (), 3);
709  trained_model->classes_.resize (locations.size ());
710  for (std::size_t i_dir = 0; i_dir < locations.size (); i_dir++)
711  {
712  trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
713  trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
714  trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
715  trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
716  }
717 
718  return (true);
719 }
720 
721 //////////////////////////////////////////////////////////////////////////////////////////////
722 template <int FeatureSize, typename PointT, typename NormalT> typename pcl::features::ISMVoteList<PointT>::Ptr
724  ISMModelPtr model,
725  typename pcl::PointCloud<PointT>::Ptr in_cloud,
726  typename pcl::PointCloud<Normal>::Ptr in_normals,
727  int in_class_of_interest)
728 {
730 
731  if (in_cloud->points.empty ())
732  return (out_votes);
733 
734  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
735  typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
736  simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
737  if (sampled_point_cloud->points.empty ())
738  return (out_votes);
739 
741  estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
742 
743  //find nearest cluster
744  const auto n_key_points = static_cast<unsigned int> (sampled_point_cloud->size ());
745  std::vector<int> min_dist_inds (n_key_points, -1);
746  for (unsigned int i_point = 0; i_point < n_key_points; i_point++)
747  {
748  Eigen::VectorXf curr_descriptor (FeatureSize);
749  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
750  curr_descriptor (i_dim) = (*feature_cloud)[i_point].histogram[i_dim];
751 
752  float descriptor_sum = curr_descriptor.sum ();
753  if (descriptor_sum < std::numeric_limits<float>::epsilon ())
754  continue;
755 
756  unsigned int min_dist_idx = 0;
757  Eigen::VectorXf clusters_center (FeatureSize);
758  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
759  clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
760 
761  float best_dist = computeDistance (curr_descriptor, clusters_center);
762  for (unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
763  {
764  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
765  clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
766  float curr_dist = computeDistance (clusters_center, curr_descriptor);
767  if (curr_dist < best_dist)
768  {
769  min_dist_idx = i_clust_cent;
770  best_dist = curr_dist;
771  }
772  }
773  min_dist_inds[i_point] = min_dist_idx;
774  }//next keypoint
775 
776  for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
777  {
778  int min_dist_idx = min_dist_inds[i_point];
779  if (min_dist_idx == -1)
780  continue;
781 
782  const auto n_words = static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
783  //compute coord system transform
784  Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
785  for (unsigned int i_word = 0; i_word < n_words; i_word++)
786  {
787  unsigned int index = model->clusters_[min_dist_idx][i_word];
788  unsigned int i_class = model->classes_[index];
789  if (static_cast<int> (i_class) != in_class_of_interest)
790  continue;//skip this class
791 
792  //rotate dir to center as needed
793  Eigen::Vector3f direction (
794  model->directions_to_center_(index, 0),
795  model->directions_to_center_(index, 1),
796  model->directions_to_center_(index, 2));
797  applyTransform (direction, transform.transpose ());
798 
799  pcl::InterestPoint vote;
800  Eigen::Vector3f vote_pos = (*sampled_point_cloud)[i_point].getVector3fMap () + direction;
801  vote.x = vote_pos[0];
802  vote.y = vote_pos[1];
803  vote.z = vote_pos[2];
804  float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
805  float learned_weight = model->learned_weights_[index];
806  float power = statistical_weight * learned_weight;
807  vote.strength = power;
808  if (vote.strength > std::numeric_limits<float>::epsilon ())
809  out_votes->addVote (vote, (*sampled_point_cloud)[i_point], i_class);
810  }
811  }//next point
812 
813  return (out_votes);
814 }
815 
816 //////////////////////////////////////////////////////////////////////////////////////////////
817 template <int FeatureSize, typename PointT, typename NormalT> bool
819  std::vector< pcl::Histogram<FeatureSize> >& histograms,
820  std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
821 {
822  histograms.clear ();
823  locations.clear ();
824 
825  if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ == nullptr)
826  return (false);
827 
828  for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
829  {
830  //compute the center of the training object
831  Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
832  const auto num_of_points = training_clouds_[i_cloud]->size ();
833  for (auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
834  models_center += point_j->getVector3fMap ();
835  models_center /= static_cast<float> (num_of_points);
836 
837  //downsample the cloud
838  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
839  typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
840  simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
841  if (sampled_point_cloud->points.empty ())
842  continue;
843 
844  shiftCloud (training_clouds_[i_cloud], models_center);
845  shiftCloud (sampled_point_cloud, models_center);
846 
848  estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
849 
850  int point_index = 0;
851  for (auto point_i = sampled_point_cloud->points.cbegin (); point_i != sampled_point_cloud->points.cend (); point_i++, point_index++)
852  {
853  float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
854  if (descriptor_sum < std::numeric_limits<float>::epsilon ())
855  continue;
856 
857  histograms.insert ( histograms.end (), feature_cloud->begin () + point_index, feature_cloud->begin () + point_index + 1 );
858 
859  int dist = static_cast<int> (std::distance (sampled_point_cloud->points.cbegin (), point_i));
860  Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
861  Eigen::Vector3f zero;
862  zero (0) = 0.0;
863  zero (1) = 0.0;
864  zero (2) = 0.0;
865  Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
866  applyTransform (new_dir, new_basis);
867 
868  PointT point (new_dir[0], new_dir[1], new_dir[2]);
869  LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
870  locations.insert(locations.end (), info);
871  }
872  }//next training cloud
873 
874  return (true);
875 }
876 
877 //////////////////////////////////////////////////////////////////////////////////////////////
878 template <int FeatureSize, typename PointT, typename NormalT> bool
880  std::vector< pcl::Histogram<FeatureSize> >& histograms,
881  Eigen::MatrixXi& labels,
882  Eigen::MatrixXf& clusters_centers)
883 {
884  Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
885 
886  for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
887  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
888  points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
889 
890  labels.resize (histograms.size(), 1);
891  computeKMeansClustering (
892  points_to_cluster,
893  number_of_clusters_,
894  labels,
895  TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),//1000
896  5,
897  PP_CENTERS,
898  clusters_centers);
899 
900  return (true);
901 }
902 
903 //////////////////////////////////////////////////////////////////////////////////////////////
904 template <int FeatureSize, typename PointT, typename NormalT> void
906 {
907  if (!training_sigmas_.empty ())
908  {
909  sigmas.resize (training_sigmas_.size (), 0.0f);
910  for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
911  sigmas[i_sigma] = training_sigmas_[i_sigma];
912  return;
913  }
914 
915  sigmas.clear ();
916  unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
917  sigmas.resize (number_of_classes, 0.0f);
918 
919  std::vector<float> vec;
920  std::vector<std::vector<float> > objects_sigmas;
921  objects_sigmas.resize (number_of_classes, vec);
922 
923  auto number_of_objects = static_cast<unsigned int> (training_clouds_.size ());
924  for (unsigned int i_object = 0; i_object < number_of_objects; i_object++)
925  {
926  float max_distance = 0.0f;
927  const auto number_of_points = training_clouds_[i_object]->size ();
928  for (unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
929  for (unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
930  {
931  float curr_distance = 0.0f;
932  curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
933  curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
934  curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
935  if (curr_distance > max_distance)
936  max_distance = curr_distance;
937  }
938  max_distance = static_cast<float> (std::sqrt (max_distance));
939  unsigned int i_class = training_classes_[i_object];
940  objects_sigmas[i_class].push_back (max_distance);
941  }
942 
943  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
944  {
945  float sig = 0.0f;
946  auto number_of_objects_in_class = static_cast<unsigned int> (objects_sigmas[i_class].size ());
947  for (unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
948  sig += objects_sigmas[i_class][i_object];
949  sig /= (static_cast<float> (number_of_objects_in_class) * 10.0f);
950  sigmas[i_class] = sig;
951  }
952 }
953 
954 //////////////////////////////////////////////////////////////////////////////////////////////
955 template <int FeatureSize, typename PointT, typename NormalT> void
957  const std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
958  const Eigen::MatrixXi &labels,
959  std::vector<float>& sigmas,
960  std::vector<std::vector<unsigned int> >& clusters,
961  std::vector<std::vector<float> >& statistical_weights,
962  std::vector<float>& learned_weights)
963 {
964  unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
965  //Temporary variable
966  std::vector<float> vec;
967  vec.resize (number_of_clusters_, 0.0f);
968  statistical_weights.clear ();
969  learned_weights.clear ();
970  statistical_weights.resize (number_of_classes, vec);
971  learned_weights.resize (locations.size (), 0.0f);
972 
973  //Temporary variable
974  std::vector<int> vect;
975  vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
976 
977  //Number of features from which c_i was learned
978  std::vector<int> n_ftr;
979 
980  //Total number of votes from visual word v_j
981  std::vector<int> n_vot;
982 
983  //Number of visual words that vote for class c_i
984  std::vector<int> n_vw;
985 
986  //Number of votes for class c_i from v_j
987  std::vector<std::vector<int> > n_vot_2;
988 
989  n_vot_2.resize (number_of_clusters_, vect);
990  n_vot.resize (number_of_clusters_, 0);
991  n_ftr.resize (number_of_classes, 0);
992  for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
993  {
994  int i_class = training_classes_[locations[i_location].model_num_];
995  int i_cluster = labels (i_location);
996  n_vot_2[i_cluster][i_class] += 1;
997  n_vot[i_cluster] += 1;
998  n_ftr[i_class] += 1;
999  }
1000 
1001  n_vw.resize (number_of_classes, 0);
1002  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1003  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1004  if (n_vot_2[i_cluster][i_class] > 0)
1005  n_vw[i_class] += 1;
1006 
1007  //computing learned weights
1008  learned_weights.resize (locations.size (), 0.0);
1009  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1010  {
1011  auto number_of_words_in_cluster = static_cast<unsigned int> (clusters[i_cluster].size ());
1012  for (unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1013  {
1014  unsigned int i_index = clusters[i_cluster][i_visual_word];
1015  int i_class = training_classes_[locations[i_index].model_num_];
1016  float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1017  if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1018  {
1019  std::vector<float> calculated_sigmas;
1020  calculateSigmas (calculated_sigmas);
1021  square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1022  if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1023  continue;
1024  }
1025  Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1026  Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1027  applyTransform (direction, transform);
1028  Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1029 
1030  //collect gaussian weighted distances
1031  std::vector<float> gauss_dists;
1032  for (unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1033  {
1034  unsigned int j_index = clusters[i_cluster][j_visual_word];
1035  int j_class = training_classes_[locations[j_index].model_num_];
1036  if (i_class != j_class)
1037  continue;
1038  //predict center
1039  Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1040  Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1041  applyTransform (direction_2, transform_2);
1042  Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1043  float residual = (predicted_center - actual_center).norm ();
1044  float value = -residual * residual / square_sigma_dist;
1045  gauss_dists.push_back (static_cast<float> (std::exp (value)));
1046  }//next word
1047  //find median gaussian weighted distance
1048  std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1049  std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1050  learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1051  }//next word
1052  }//next cluster
1053 
1054  //computing statistical weights
1055  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1056  {
1057  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1058  {
1059  if (n_vot_2[i_cluster][i_class] == 0)
1060  continue;//no votes per class of interest in this cluster
1061  if (n_vw[i_class] == 0)
1062  continue;//there were no objects of this class in the training dataset
1063  if (n_vot[i_cluster] == 0)
1064  continue;//this cluster has never been used
1065  if (n_ftr[i_class] == 0)
1066  continue;//there were no objects of this class in the training dataset
1067  float part_1 = static_cast<float> (n_vw[i_class]);
1068  float part_2 = static_cast<float> (n_vot[i_cluster]);
1069  float part_3 = static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1070  float part_4 = 0.0f;
1071 
1072  if (!n_vot_ON_)
1073  part_2 = 1.0f;
1074 
1075  for (unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1076  if (n_ftr[j_class] != 0)
1077  part_4 += static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1078 
1079  statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1080  }
1081  }//next cluster
1082 }
1083 
1084 //////////////////////////////////////////////////////////////////////////////////////////////
1085 template <int FeatureSize, typename PointT, typename NormalT> void
1087  typename pcl::PointCloud<PointT>::ConstPtr in_point_cloud,
1088  typename pcl::PointCloud<NormalT>::ConstPtr in_normal_cloud,
1089  typename pcl::PointCloud<PointT>::Ptr out_sampled_point_cloud,
1090  typename pcl::PointCloud<NormalT>::Ptr out_sampled_normal_cloud)
1091 {
1092  //create voxel grid
1094  grid.setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1095  grid.setSaveLeafLayout (true);
1096  grid.setInputCloud (in_point_cloud);
1097 
1098  pcl::PointCloud<PointT> temp_cloud;
1099  grid.filter (temp_cloud);
1100 
1101  //extract indices of points from source cloud which are closest to grid points
1102  constexpr float max_value = std::numeric_limits<float>::max ();
1103 
1104  const auto num_source_points = in_point_cloud->size ();
1105  const auto num_sample_points = temp_cloud.size ();
1106 
1107  std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1108  std::vector<int> sampling_indices (num_sample_points, -1);
1109 
1110  for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1111  {
1112  int index = grid.getCentroidIndex ((*in_point_cloud)[i_point]);
1113  if (index == -1)
1114  continue;
1115 
1116  PointT pt_1 = (*in_point_cloud)[i_point];
1117  PointT pt_2 = temp_cloud[index];
1118 
1119  float distance = (pt_1.x - pt_2.x) * (pt_1.x - pt_2.x) + (pt_1.y - pt_2.y) * (pt_1.y - pt_2.y) + (pt_1.z - pt_2.z) * (pt_1.z - pt_2.z);
1120  if (distance < dist_to_grid_center[index])
1121  {
1122  dist_to_grid_center[index] = distance;
1123  sampling_indices[index] = static_cast<int> (i_point);
1124  }
1125  }
1126 
1127  //extract source points
1128  pcl::PointIndices::Ptr final_inliers_indices (new pcl::PointIndices ());
1129  pcl::ExtractIndices<PointT> extract_points;
1130  pcl::ExtractIndices<NormalT> extract_normals;
1131 
1132  final_inliers_indices->indices.reserve (num_sample_points);
1133  for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1134  {
1135  if (sampling_indices[i_point] != -1)
1136  final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1137  }
1138 
1139  extract_points.setInputCloud (in_point_cloud);
1140  extract_points.setIndices (final_inliers_indices);
1141  extract_points.filter (*out_sampled_point_cloud);
1142 
1143  extract_normals.setInputCloud (in_normal_cloud);
1144  extract_normals.setIndices (final_inliers_indices);
1145  extract_normals.filter (*out_sampled_normal_cloud);
1146 }
1147 
1148 //////////////////////////////////////////////////////////////////////////////////////////////
1149 template <int FeatureSize, typename PointT, typename NormalT> void
1151  typename pcl::PointCloud<PointT>::Ptr in_cloud,
1152  Eigen::Vector3f shift_point)
1153 {
1154  for (auto point_it = in_cloud->points.begin (); point_it != in_cloud->points.end (); point_it++)
1155  {
1156  point_it->x -= shift_point.x ();
1157  point_it->y -= shift_point.y ();
1158  point_it->z -= shift_point.z ();
1159  }
1160 }
1161 
1162 //////////////////////////////////////////////////////////////////////////////////////////////
1163 template <int FeatureSize, typename PointT, typename NormalT> Eigen::Matrix3f
1165 {
1166  Eigen::Matrix3f result;
1167  Eigen::Matrix3f rotation_matrix_X;
1168  Eigen::Matrix3f rotation_matrix_Z;
1169 
1170  float A = 0.0f;
1171  float B = 0.0f;
1172  float sign = -1.0f;
1173 
1174  float denom_X = static_cast<float> (std::sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1175  A = in_normal.normal_y / denom_X;
1176  B = sign * in_normal.normal_z / denom_X;
1177  rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1178  0.0f, A, -B,
1179  0.0f, B, A;
1180 
1181  float denom_Z = static_cast<float> (std::sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1182  A = in_normal.normal_y / denom_Z;
1183  B = sign * in_normal.normal_x / denom_Z;
1184  rotation_matrix_Z << A, -B, 0.0f,
1185  B, A, 0.0f,
1186  0.0f, 0.0f, 1.0f;
1187 
1188  result = rotation_matrix_X * rotation_matrix_Z;
1189 
1190  return (result);
1191 }
1192 
1193 //////////////////////////////////////////////////////////////////////////////////////////////
1194 template <int FeatureSize, typename PointT, typename NormalT> void
1195 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform)
1196 {
1197  io_vec = in_transform * io_vec;
1198 }
1199 
1200 //////////////////////////////////////////////////////////////////////////////////////////////
1201 template <int FeatureSize, typename PointT, typename NormalT> void
1203  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud,
1204  typename pcl::PointCloud<NormalT>::Ptr normal_cloud,
1205  typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud)
1206 {
1208 // tree->setInputCloud (point_cloud);
1209 
1210  feature_estimator_->setInputCloud (sampled_point_cloud->makeShared ());
1211 // feature_estimator_->setSearchSurface (point_cloud->makeShared ());
1212  feature_estimator_->setSearchMethod (tree);
1213 
1214 // typename pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm =
1215 // dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1216 // feat_est_norm->setInputNormals (normal_cloud);
1217 
1219  dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1220  feat_est_norm->setInputNormals (normal_cloud);
1221 
1222  feature_estimator_->compute (*feature_cloud);
1223 }
1224 
1225 //////////////////////////////////////////////////////////////////////////////////////////////
1226 template <int FeatureSize, typename PointT, typename NormalT> double
1228  const Eigen::MatrixXf& points_to_cluster,
1229  int number_of_clusters,
1230  Eigen::MatrixXi& io_labels,
1231  TermCriteria criteria,
1232  int attempts,
1233  int flags,
1234  Eigen::MatrixXf& cluster_centers)
1235 {
1236  constexpr int spp_trials = 3;
1237  std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1238  int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1239 
1240  attempts = std::max (attempts, 1);
1241  srand (static_cast<unsigned int> (time (nullptr)));
1242 
1243  Eigen::MatrixXi labels (number_of_points, 1);
1244 
1245  if (flags & USE_INITIAL_LABELS)
1246  labels = io_labels;
1247  else
1248  labels.setZero ();
1249 
1250  Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1251  Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1252  std::vector<int> counters (number_of_clusters);
1253  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1254  Eigen::Vector2f* box = boxes.data();
1255 
1256  double best_compactness = std::numeric_limits<double>::max ();
1257  double compactness = 0.0;
1258 
1259  if (criteria.type_ & TermCriteria::EPS)
1260  criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f);
1261  else
1262  criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1263 
1264  criteria.epsilon_ *= criteria.epsilon_;
1265 
1266  if (criteria.type_ & TermCriteria::COUNT)
1267  criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100);
1268  else
1269  criteria.max_count_ = 100;
1270 
1271  if (number_of_clusters == 1)
1272  {
1273  attempts = 1;
1274  criteria.max_count_ = 2;
1275  }
1276 
1277  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1278  box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1279 
1280  for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1281  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1282  {
1283  float v = points_to_cluster (i_point, i_dim);
1284  box[i_dim] (0) = std::min (box[i_dim] (0), v);
1285  box[i_dim] (1) = std::max (box[i_dim] (1), v);
1286  }
1287 
1288  for (int i_attempt = 0; i_attempt < attempts; i_attempt++)
1289  {
1290  float max_center_shift = std::numeric_limits<float>::max ();
1291  for (int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++)
1292  {
1293  Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1294  temp = centers;
1295  centers = old_centers;
1296  old_centers = temp;
1297 
1298  if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1299  {
1300  if (flags & PP_CENTERS)
1301  generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1302  else
1303  {
1304  for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1305  {
1306  Eigen::VectorXf center (feature_dimension);
1307  generateRandomCenter (boxes, center);
1308  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1309  centers (i_cl_center, i_dim) = center (i_dim);
1310  }//generate center for next cluster
1311  }//end if-else random or PP centers
1312  }
1313  else
1314  {
1315  centers.setZero ();
1316  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1317  counters[i_cluster] = 0;
1318  for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1319  {
1320  int i_label = labels (i_point, 0);
1321  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1322  centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1323  counters[i_label]++;
1324  }
1325  if (iter > 0)
1326  max_center_shift = 0.0f;
1327  for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1328  {
1329  if (counters[i_cl_center] != 0)
1330  {
1331  float scale = 1.0f / static_cast<float> (counters[i_cl_center]);
1332  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1333  centers (i_cl_center, i_dim) *= scale;
1334  }
1335  else
1336  {
1337  Eigen::VectorXf center (feature_dimension);
1338  generateRandomCenter (boxes, center);
1339  for(int i_dim = 0; i_dim < feature_dimension; i_dim++)
1340  centers (i_cl_center, i_dim) = center (i_dim);
1341  }
1342 
1343  if (iter > 0)
1344  {
1345  float dist = 0.0f;
1346  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1347  {
1348  float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1349  dist += diff * diff;
1350  }
1351  max_center_shift = std::max (max_center_shift, dist);
1352  }
1353  }
1354  }
1355  compactness = 0.0f;
1356  for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1357  {
1358  Eigen::VectorXf sample (feature_dimension);
1359  sample = points_to_cluster.row (i_point);
1360 
1361  int k_best = 0;
1362  float min_dist = std::numeric_limits<float>::max ();
1363 
1364  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1365  {
1366  Eigen::VectorXf center (feature_dimension);
1367  center = centers.row (i_cluster);
1368  float dist = computeDistance (sample, center);
1369  if (min_dist > dist)
1370  {
1371  min_dist = dist;
1372  k_best = i_cluster;
1373  }
1374  }
1375  compactness += min_dist;
1376  labels (i_point, 0) = k_best;
1377  }
1378  }//next iteration
1379 
1380  if (compactness < best_compactness)
1381  {
1382  best_compactness = compactness;
1383  cluster_centers = centers;
1384  io_labels = labels;
1385  }
1386  }//next attempt
1387 
1388  return (best_compactness);
1389 }
1390 
1391 //////////////////////////////////////////////////////////////////////////////////////////////
1392 template <int FeatureSize, typename PointT, typename NormalT> void
1394  const Eigen::MatrixXf& data,
1395  Eigen::MatrixXf& out_centers,
1396  int number_of_clusters,
1397  int trials)
1398 {
1399  std::size_t dimension = data.cols ();
1400  auto number_of_points = static_cast<unsigned int> (data.rows ());
1401  std::vector<int> centers_vec (number_of_clusters);
1402  int* centers = centers_vec.data();
1403  std::vector<double> dist (number_of_points);
1404  std::vector<double> tdist (number_of_points);
1405  std::vector<double> tdist2 (number_of_points);
1406  double sum0 = 0.0;
1407 
1408  unsigned int random_unsigned = rand ();
1409  centers[0] = random_unsigned % number_of_points;
1410 
1411  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1412  {
1413  Eigen::VectorXf first (dimension);
1414  Eigen::VectorXf second (dimension);
1415  first = data.row (i_point);
1416  second = data.row (centers[0]);
1417  dist[i_point] = computeDistance (first, second);
1418  sum0 += dist[i_point];
1419  }
1420 
1421  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1422  {
1423  double best_sum = std::numeric_limits<double>::max ();
1424  int best_center = -1;
1425  for (int i_trials = 0; i_trials < trials; i_trials++)
1426  {
1427  unsigned int random_integer = rand () - 1;
1428  double random_double = static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1429  double p = random_double * sum0;
1430 
1431  unsigned int i_point;
1432  for (i_point = 0; i_point < number_of_points - 1; i_point++)
1433  if ( (p -= dist[i_point]) <= 0.0)
1434  break;
1435 
1436  int ci = i_point;
1437 
1438  double s = 0.0;
1439  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1440  {
1441  Eigen::VectorXf first (dimension);
1442  Eigen::VectorXf second (dimension);
1443  first = data.row (i_point);
1444  second = data.row (ci);
1445  tdist2[i_point] = std::min (static_cast<double> (computeDistance (first, second)), dist[i_point]);
1446  s += tdist2[i_point];
1447  }
1448 
1449  if (s <= best_sum)
1450  {
1451  best_sum = s;
1452  best_center = ci;
1453  std::swap (tdist, tdist2);
1454  }
1455  }
1456 
1457  centers[i_cluster] = best_center;
1458  sum0 = best_sum;
1459  std::swap (dist, tdist);
1460  }
1461 
1462  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1463  for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1464  out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1465 }
1466 
1467 //////////////////////////////////////////////////////////////////////////////////////////////
1468 template <int FeatureSize, typename PointT, typename NormalT> void
1469 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateRandomCenter (const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >& boxes,
1470  Eigen::VectorXf& center)
1471 {
1472  std::size_t dimension = boxes.size ();
1473  float margin = 1.0f / static_cast<float> (dimension);
1474 
1475  for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1476  {
1477  unsigned int random_integer = rand () - 1;
1478  float random_float = static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1479  center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1480  }
1481 }
1482 
1483 //////////////////////////////////////////////////////////////////////////////////////////////
1484 template <int FeatureSize, typename PointT, typename NormalT> float
1486 {
1487  std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1488  float distance = 0.0f;
1489  for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1490  {
1491  float diff = vec_1 (i_dim) - vec_2 (i_dim);
1492  distance += diff * diff;
1493  }
1494 
1495  return (distance);
1496 }
1497 
1498 #endif //#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_
A point structure representing normal coordinates and the surface curvature estimate.
void generateRandomCenter(const std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > &boxes, Eigen::VectorXf &center)
Generates random center for cluster.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
std::vector< typename pcl::PointCloud< PointT >::Ptr > getTrainingClouds()
This method simply returns the clouds that were set as the training clouds.
std::vector< float > learned_weights_
Stores learned weights.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
ISMVoteList()
Empty constructor with member variables initialization.
unsigned int getNumberOfVotes()
This method simply returns the number of votes.
void generateCentersPP(const Eigen::MatrixXf &data, Eigen::MatrixXf &out_centers, int number_of_clusters, int trials)
Generates centers for clusters as described in Arthur, David and Sergei Vassilvitski (2007) k-means++...
bool loadModelFromfile(std::string &file_name)
This method loads the trained model from file.
The assignment of this structure is to store the statistical/learned weights and other information of...
void setTrainingNormals(const std::vector< typename pcl::PointCloud< NormalT >::Ptr > &training_normals)
Allows to set normals for the training clouds that were passed through setTrainingClouds method...
void simplifyCloud(typename pcl::PointCloud< PointT >::ConstPtr in_point_cloud, typename pcl::PointCloud< NormalT >::ConstPtr in_normal_cloud, typename pcl::PointCloud< PointT >::Ptr out_sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr out_sampled_normal_cloud)
Simplifies the cloud using voxel grid principles.
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed, so avoid using this function on non-empty clouds.
Definition: point_cloud.h:898
std::vector< float > getSigmaDists()
Returns the array of sigma values.
virtual ~ISMVoteList()
virtual descriptor.
iterator begin() noexcept
Definition: point_cloud.h:429
unsigned int descriptors_dimension_
Stores descriptors dimension.
unsigned int number_of_classes_
Stores the number of classes.
Eigen::MatrixXf directions_to_center_
Stores the directions to objects center for each visual word.
This structure is used for determining the end of the k-means clustering process. ...
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
Definition: voxel_grid.h:343
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
pcl::features::ISMModel::Ptr ISMModelPtr
Eigen::Matrix3f alignYCoordWithNormal(const NormalT &in_normal)
This method simply computes the rotation matrix, so that the given normal would match the Y axis afte...
ISMModel()
Simple constructor that initializes the structure.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition: voxel_grid.h:304
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:121
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:209
void setNVotState(bool state)
Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
pcl::features::ISMVoteList< PointT >::Ptr findObjects(ISMModelPtr model, typename pcl::PointCloud< PointT >::Ptr in_cloud, typename pcl::PointCloud< Normal >::Ptr in_normals, int in_class_of_interest)
This function is searching for the class of interest in a given cloud and returns the list of votes...
void setSigmaDists(const std::vector< float > &training_sigmas)
This method allows to set the value of sigma used for calculating the learned weights for every singl...
virtual ~ISMModel()
Destructor that frees memory.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:13
A point structure representing an N-D histogram.
bool trainISM(ISMModelPtr &trained_model)
This method performs training and forms a visual vocabulary.
ImplicitShapeModelEstimation()
Simple constructor that initializes everything.
unsigned int number_of_clusters_
Stores the number of clusters.
double computeKMeansClustering(const Eigen::MatrixXf &points_to_cluster, int number_of_clusters, Eigen::MatrixXi &io_labels, TermCriteria criteria, int attempts, int flags, Eigen::MatrixXf &cluster_centers)
Performs K-means clustering.
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
A point structure representing Euclidean xyz coordinates.
shared_ptr< ISMVoteList< PointT > > Ptr
float computeDistance(Eigen::VectorXf &vec_1, Eigen::VectorXf &vec_2)
Computes the square distance between two vectors.
std::vector< std::vector< float > > statistical_weights_
Stores statistical weights.
A point structure representing an interest point with Euclidean xyz coordinates, and an interest valu...
void reset()
this method resets all variables and frees memory.
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
double getDensityAtPoint(const PointT &point, double sigma_dist)
Returns the density at the specified point.
double density
Density of this peak.
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 setFeatureEstimator(FeaturePtr feature)
Changes the feature estimator.
void calculateWeights(const std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations, const Eigen::MatrixXi &labels, std::vector< float > &sigmas, std::vector< std::vector< unsigned int > > &clusters, std::vector< std::vector< float > > &statistical_weights, std::vector< float > &learned_weights)
This function forms a visual vocabulary and evaluates weights described in [Knopp et al...
void applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
FeaturePtr getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the corresponding cloud of normals for every training point cloud.
std::vector< unsigned int > getTrainingClasses()
Returns the array of classes that indicates which class the corresponding training cloud belongs...
bool clusterDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, Eigen::MatrixXi &labels, Eigen::MatrixXf &clusters_centers)
This method performs descriptor clustering.
void estimateFeatures(typename pcl::PointCloud< PointT >::Ptr sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr normal_cloud, typename pcl::PointCloud< pcl::Histogram< FeatureSize > >::Ptr feature_cloud)
This method estimates features for the given point cloud.
ISMModel & operator=(const ISMModel &other)
Operator overloading for deep copy.
int class_id
Determines which class this peak belongs.
void findStrongestPeaks(std::vector< ISMPeak, Eigen::aligned_allocator< ISMPeak > > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma)
This method finds the strongest peaks (points were density has most higher values).
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
This struct is used for storing peak.
unsigned int number_of_visual_words_
Stores the number of visual words.
std::vector< unsigned int > classes_
Stores the class label for every direction.
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
void addVote(pcl::InterestPoint &in_vote, const PointT &vote_origin, int in_class)
This method simply adds another vote to the list.
std::vector< float > sigmas_
Stores the sigma value for each class.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool extractDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations)
Extracts the descriptors from the input clouds.
float getSamplingSize()
Returns the sampling size used for cloud simplification.
std::size_t size() const
Definition: point_cloud.h:443
int type_
Flag that determines when the k-means clustering must be stopped.
unsigned int getNumberOfClusters()
Returns the number of clusters used for descriptor clustering.
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
Definition: norms.h:54
ExtractIndices extracts a set of indices from a point cloud.
float epsilon_
Defines the accuracy for k-means clustering.
void validateTree()
this method is simply setting up the search tree.
This structure stores the information about the keypoint.
void setSamplingSize(float sampling_size)
Changes the sampling size used for cloud simplification.
void shiftCloud(typename pcl::PointCloud< PointT >::Ptr in_cloud, Eigen::Vector3f shift_point)
This method simply shifts the clouds points relative to the passed point.
int max_count_
Defines maximum number of iterations for k-means clustering.
void setTrainingClasses(const std::vector< unsigned int > &training_classes)
Allows to set the class labels for the corresponding training clouds.
void setTrainingClouds(const std::vector< typename pcl::PointCloud< PointT >::Ptr > &training_clouds)
Allows to set clouds for training the ISM model.
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud(typename pcl::PointCloud< PointT >::Ptr cloud=0)
Returns the colored cloud that consists of votes for center (blue points) and initial point cloud (if...
void calculateSigmas(std::vector< float > &sigmas)
This method calculates the value of sigma used for calculating the learned weights for every single c...
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition: voxel_grid.h:247
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm...