Point Cloud Library (PCL)  1.9.1-dev
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 classication"
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 
46 //////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT>
49  votes_ (new pcl::PointCloud<pcl::InterestPoint> ()),
50  tree_is_valid_ (false),
51  votes_origins_ (new pcl::PointCloud<PointT> ()),
52  votes_class_ (0),
53  k_ind_ (0),
54  k_sqr_dist_ (0)
55 {
56 }
57 
58 //////////////////////////////////////////////////////////////////////////////////////////////
59 template <typename PointT>
61 {
62  votes_class_.clear ();
63  votes_origins_.reset ();
64  votes_.reset ();
65  k_ind_.clear ();
66  k_sqr_dist_.clear ();
67  tree_.reset ();
68 }
69 
70 //////////////////////////////////////////////////////////////////////////////////////////////
71 template <typename PointT> void
73  pcl::InterestPoint& vote, const PointT &vote_origin, int votes_class)
74 {
75  tree_is_valid_ = false;
76  votes_->points.insert (votes_->points.end (), vote);// TODO: adjust height and width
77 
78  votes_origins_->points.push_back (vote_origin);
79  votes_class_.push_back (votes_class);
80 }
81 
82 //////////////////////////////////////////////////////////////////////////////////////////////
83 template <typename PointT> typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr
85 {
86  pcl::PointXYZRGB point;
88  colored_cloud->height = 0;
89  colored_cloud->width = 1;
90 
91  if (cloud != nullptr)
92  {
93  colored_cloud->height += static_cast<uint32_t> (cloud->points.size ());
94  point.r = 255;
95  point.g = 255;
96  point.b = 255;
97  for (size_t i_point = 0; i_point < cloud->points.size (); i_point++)
98  {
99  point.x = cloud->points[i_point].x;
100  point.y = cloud->points[i_point].y;
101  point.z = cloud->points[i_point].z;
102  colored_cloud->points.push_back (point);
103  }
104  }
105 
106  point.r = 0;
107  point.g = 0;
108  point.b = 255;
109  for (const auto &i_vote : votes_->points)
110  {
111  point.x = i_vote.x;
112  point.y = i_vote.y;
113  point.z = i_vote.z;
114  colored_cloud->points.push_back (point);
115  }
116  colored_cloud->height += static_cast<uint32_t> (votes_->points.size ());
117 
118  return (colored_cloud);
119 }
120 
121 //////////////////////////////////////////////////////////////////////////////////////////////
122 template <typename PointT> void
124  std::vector<pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
125  int in_class_id,
126  double in_non_maxima_radius,
127  double in_sigma)
128 {
129  validateTree ();
130 
131  const size_t n_vote_classes = votes_class_.size ();
132  if (n_vote_classes == 0)
133  return;
134  for (size_t i = 0; i < n_vote_classes ; i++)
135  assert ( votes_class_[i] == in_class_id );
136 
137  // heuristic: start from NUM_INIT_PTS different locations selected uniformly
138  // on the votes. Intuitively, it is likely to get a good location in dense regions.
139  const int NUM_INIT_PTS = 100;
140  double SIGMA_DIST = in_sigma;// rule of thumb: 10% of the object radius
141  const double FINAL_EPS = SIGMA_DIST / 100;// another heuristic
142 
143  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
144  std::vector<double> peak_densities (NUM_INIT_PTS);
145  double max_density = -1.0;
146  for (int i = 0; i < NUM_INIT_PTS; i++)
147  {
148  Eigen::Vector3f old_center;
149  Eigen::Vector3f curr_center;
150  curr_center (0) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].x;
151  curr_center (1) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].y;
152  curr_center (2) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].z;
153 
154  do
155  {
156  old_center = curr_center;
157  curr_center = shiftMean (old_center, SIGMA_DIST);
158  } while ((old_center - curr_center).norm () > FINAL_EPS);
159 
160  pcl::PointXYZ point;
161  point.x = curr_center (0);
162  point.y = curr_center (1);
163  point.z = curr_center (2);
164  double curr_density = getDensityAtPoint (point, SIGMA_DIST);
165  assert (curr_density >= 0.0);
166 
167  peaks[i] = curr_center;
168  peak_densities[i] = curr_density;
169 
170  if ( max_density < curr_density )
171  max_density = curr_density;
172  }
173 
174  //extract peaks
175  std::vector<bool> peak_flag (NUM_INIT_PTS, true);
176  for (int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
177  {
178  // find best peak with taking into consideration peak flags
179  double best_density = -1.0;
180  Eigen::Vector3f strongest_peak;
181  int best_peak_ind (-1);
182  int peak_counter (0);
183  for (int i = 0; i < NUM_INIT_PTS; i++)
184  {
185  if ( !peak_flag[i] )
186  continue;
187 
188  if ( peak_densities[i] > best_density)
189  {
190  best_density = peak_densities[i];
191  strongest_peak = peaks[i];
192  best_peak_ind = i;
193  }
194  ++peak_counter;
195  }
196 
197  if( peak_counter == 0 )
198  break;// no peaks
199 
200  pcl::ISMPeak peak;
201  peak.x = strongest_peak(0);
202  peak.y = strongest_peak(1);
203  peak.z = strongest_peak(2);
204  peak.density = best_density;
205  peak.class_id = in_class_id;
206  out_peaks.push_back ( peak );
207 
208  // mark best peaks and all its neighbors
209  peak_flag[best_peak_ind] = false;
210  for (int i = 0; i < NUM_INIT_PTS; i++)
211  {
212  // compute distance between best peak and all unmarked peaks
213  if ( !peak_flag[i] )
214  continue;
215 
216  double dist = (strongest_peak - peaks[i]).norm ();
217  if ( dist < in_non_maxima_radius )
218  peak_flag[i] = false;
219  }
220  }
221 }
222 
223 //////////////////////////////////////////////////////////////////////////////////////////////
224 template <typename PointT> void
226 {
227  if (!tree_is_valid_)
228  {
229  if (tree_ == nullptr)
231  tree_->setInputCloud (votes_);
232  k_ind_.resize ( votes_->points.size (), -1 );
233  k_sqr_dist_.resize ( votes_->points.size (), 0.0f );
234  tree_is_valid_ = true;
235  }
236 }
237 
238 //////////////////////////////////////////////////////////////////////////////////////////////
239 template <typename PointT> Eigen::Vector3f
240 pcl::features::ISMVoteList<PointT>::shiftMean (const Eigen::Vector3f& snap_pt, const double in_sigma_dist)
241 {
242  validateTree ();
243 
244  Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
245  double denom = 0.0;
246 
248  pt.x = snap_pt[0];
249  pt.y = snap_pt[1];
250  pt.z = snap_pt[2];
251  size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
252 
253  for (size_t j = 0; j < n_pts; j++)
254  {
255  double kernel = votes_->points[k_ind_[j]].strength * exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
256  Eigen::Vector3f vote_vec (votes_->points[k_ind_[j]].x, votes_->points[k_ind_[j]].y, votes_->points[k_ind_[j]].z);
257  wgh_sum += vote_vec * static_cast<float> (kernel);
258  denom += kernel;
259  }
260  assert (denom > 0.0); // at least one point is close. In fact, this case should be handled too
261 
262  return (wgh_sum / static_cast<float> (denom));
263 }
264 
265 //////////////////////////////////////////////////////////////////////////////////////////////
266 template <typename PointT> double
268  const PointT &point, double sigma_dist)
269 {
270  validateTree ();
271 
272  const size_t n_vote_classes = votes_class_.size ();
273  if (n_vote_classes == 0)
274  return (0.0);
275 
276  double sum_vote = 0.0;
277 
279  pt.x = point.x;
280  pt.y = point.y;
281  pt.z = point.z;
282  size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
283 
284  for (size_t j = 0; j < num_of_pts; j++)
285  sum_vote += votes_->points[k_ind_[j]].strength * exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
286 
287  return (sum_vote);
288 }
289 
290 //////////////////////////////////////////////////////////////////////////////////////////////
291 template <typename PointT> unsigned int
293 {
294  return (static_cast<unsigned int> (votes_->points.size ()));
295 }
296 
297 //////////////////////////////////////////////////////////////////////////////////////////////
299  statistical_weights_ (0),
300  learned_weights_ (0),
301  classes_ (0),
302  sigmas_ (0),
303  clusters_ (0),
304  number_of_classes_ (0),
305  number_of_visual_words_ (0),
306  number_of_clusters_ (0),
307  descriptors_dimension_ (0)
308 {
309 }
310 
311 //////////////////////////////////////////////////////////////////////////////////////////////
313 {
314  reset ();
315 
320 
321  std::vector<float> vec;
322  vec.resize (this->number_of_clusters_, 0.0f);
323  this->statistical_weights_.resize (this->number_of_classes_, vec);
324  for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
325  for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
326  this->statistical_weights_[i_class][i_cluster] = copy.statistical_weights_[i_class][i_cluster];
327 
328  this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
329  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
330  this->learned_weights_[i_visual_word] = copy.learned_weights_[i_visual_word];
331 
332  this->classes_.resize (this->number_of_visual_words_, 0);
333  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
334  this->classes_[i_visual_word] = copy.classes_[i_visual_word];
335 
336  this->sigmas_.resize (this->number_of_classes_, 0.0f);
337  for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
338  this->sigmas_[i_class] = copy.sigmas_[i_class];
339 
340  this->directions_to_center_.resize (this->number_of_visual_words_, 3);
341  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
342  for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
343  this->directions_to_center_ (i_visual_word, i_dim) = copy.directions_to_center_ (i_visual_word, i_dim);
344 
345  this->clusters_centers_.resize (this->number_of_clusters_, 3);
346  for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
347  for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
348  this->clusters_centers_ (i_cluster, i_dim) = copy.clusters_centers_ (i_cluster, i_dim);
349 }
350 
351 //////////////////////////////////////////////////////////////////////////////////////////////
353 {
354  reset ();
355 }
356 
357 //////////////////////////////////////////////////////////////////////////////////////////////
358 bool
360 {
361  std::ofstream output_file (file_name.c_str (), std::ios::trunc);
362  if (!output_file)
363  {
364  output_file.close ();
365  return (false);
366  }
367 
368  output_file << number_of_classes_ << " ";
369  output_file << number_of_visual_words_ << " ";
370  output_file << number_of_clusters_ << " ";
371  output_file << descriptors_dimension_ << " ";
372 
373  //write statistical weights
374  for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
375  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
376  output_file << statistical_weights_[i_class][i_cluster] << " ";
377 
378  //write learned weights
379  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
380  output_file << learned_weights_[i_visual_word] << " ";
381 
382  //write classes
383  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
384  output_file << classes_[i_visual_word] << " ";
385 
386  //write sigmas
387  for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
388  output_file << sigmas_[i_class] << " ";
389 
390  //write directions to centers
391  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
392  for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
393  output_file << directions_to_center_ (i_visual_word, i_dim) << " ";
394 
395  //write clusters centers
396  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
397  for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
398  output_file << clusters_centers_ (i_cluster, i_dim) << " ";
399 
400  //write clusters
401  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
402  {
403  output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) << " ";
404  for (const unsigned int &visual_word : clusters_[i_cluster])
405  output_file << visual_word << " ";
406  }
407 
408  output_file.close ();
409  return (true);
410 }
411 
412 //////////////////////////////////////////////////////////////////////////////////////////////
413 bool
415 {
416  reset ();
417  std::ifstream input_file (file_name.c_str ());
418  if (!input_file)
419  {
420  input_file.close ();
421  return (false);
422  }
423 
424  char line[256];
425 
426  input_file.getline (line, 256, ' ');
427  number_of_classes_ = static_cast<unsigned int> (strtol (line, nullptr, 10));
428  input_file.getline (line, 256, ' '); number_of_visual_words_ = atoi (line);
429  input_file.getline (line, 256, ' '); number_of_clusters_ = atoi (line);
430  input_file.getline (line, 256, ' '); descriptors_dimension_ = atoi (line);
431 
432  //read statistical weights
433  std::vector<float> vec;
434  vec.resize (number_of_clusters_, 0.0f);
436  for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
437  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
438  input_file >> statistical_weights_[i_class][i_cluster];
439 
440  //read learned weights
442  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
443  input_file >> learned_weights_[i_visual_word];
444 
445  //read classes
446  classes_.resize (number_of_visual_words_, 0);
447  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
448  input_file >> classes_[i_visual_word];
449 
450  //read sigmas
451  sigmas_.resize (number_of_classes_, 0.0f);
452  for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
453  input_file >> sigmas_[i_class];
454 
455  //read directions to centers
456  directions_to_center_.resize (number_of_visual_words_, 3);
457  for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
458  for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
459  input_file >> directions_to_center_ (i_visual_word, i_dim);
460 
461  //read clusters centers
462  clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
463  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
464  for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
465  input_file >> clusters_centers_ (i_cluster, i_dim);
466 
467  //read clusters
468  std::vector<unsigned int> vect;
469  clusters_.resize (number_of_clusters_, vect);
470  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
471  {
472  unsigned int size_of_current_cluster = 0;
473  input_file >> size_of_current_cluster;
474  clusters_[i_cluster].resize (size_of_current_cluster, 0);
475  for (unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
476  input_file >> clusters_[i_cluster][i_visual_word];
477  }
478 
479  input_file.close ();
480  return (true);
481 }
482 
483 //////////////////////////////////////////////////////////////////////////////////////////////
484 void
486 {
487  statistical_weights_.clear ();
488  learned_weights_.clear ();
489  classes_.clear ();
490  sigmas_.clear ();
491  directions_to_center_.resize (0, 0);
492  clusters_centers_.resize (0, 0);
493  clusters_.clear ();
494  number_of_classes_ = 0;
498 }
499 
500 //////////////////////////////////////////////////////////////////////////////////////////////
503 {
504  if (this != &other)
505  {
506  this->reset ();
507 
512 
513  std::vector<float> vec;
514  vec.resize (number_of_clusters_, 0.0f);
515  this->statistical_weights_.resize (this->number_of_classes_, vec);
516  for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
517  for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
518  this->statistical_weights_[i_class][i_cluster] = other.statistical_weights_[i_class][i_cluster];
519 
520  this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
521  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
522  this->learned_weights_[i_visual_word] = other.learned_weights_[i_visual_word];
523 
524  this->classes_.resize (this->number_of_visual_words_, 0);
525  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
526  this->classes_[i_visual_word] = other.classes_[i_visual_word];
527 
528  this->sigmas_.resize (this->number_of_classes_, 0.0f);
529  for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
530  this->sigmas_[i_class] = other.sigmas_[i_class];
531 
532  this->directions_to_center_.resize (this->number_of_visual_words_, 3);
533  for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
534  for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
535  this->directions_to_center_ (i_visual_word, i_dim) = other.directions_to_center_ (i_visual_word, i_dim);
536 
537  this->clusters_centers_.resize (this->number_of_clusters_, 3);
538  for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
539  for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
540  this->clusters_centers_ (i_cluster, i_dim) = other.clusters_centers_ (i_cluster, i_dim);
541  }
542  return (*this);
543 }
544 
545 //////////////////////////////////////////////////////////////////////////////////////////////
546 template <int FeatureSize, typename PointT, typename NormalT>
548  training_clouds_ (0),
549  training_classes_ (0),
550  training_normals_ (0),
551  training_sigmas_ (0),
552  sampling_size_ (0.1f),
553  feature_estimator_ (),
554  number_of_clusters_ (184),
555  n_vot_ON_ (true)
556 {
557 }
558 
559 //////////////////////////////////////////////////////////////////////////////////////////////
560 template <int FeatureSize, typename PointT, typename NormalT>
562 {
563  training_clouds_.clear ();
564  training_classes_.clear ();
565  training_normals_.clear ();
566  training_sigmas_.clear ();
567  feature_estimator_.reset ();
568 }
569 
570 //////////////////////////////////////////////////////////////////////////////////////////////
571 template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
573 {
574  return (training_clouds_);
575 }
576 
577 //////////////////////////////////////////////////////////////////////////////////////////////
578 template <int FeatureSize, typename PointT, typename NormalT> void
580  const std::vector< typename pcl::PointCloud<PointT>::Ptr >& training_clouds)
581 {
582  training_clouds_.clear ();
583  std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
584  training_clouds_.swap (clouds);
585 }
586 
587 //////////////////////////////////////////////////////////////////////////////////////////////
588 template <int FeatureSize, typename PointT, typename NormalT> std::vector<unsigned int>
590 {
591  return (training_classes_);
592 }
593 
594 //////////////////////////////////////////////////////////////////////////////////////////////
595 template <int FeatureSize, typename PointT, typename NormalT> void
597 {
598  training_classes_.clear ();
599  std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
600  training_classes_.swap (classes);
601 }
602 
603 //////////////////////////////////////////////////////////////////////////////////////////////
604 template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
606 {
607  return (training_normals_);
608 }
609 
610 //////////////////////////////////////////////////////////////////////////////////////////////
611 template <int FeatureSize, typename PointT, typename NormalT> void
613  const std::vector< typename pcl::PointCloud<NormalT>::Ptr >& training_normals)
614 {
615  training_normals_.clear ();
616  std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
617  training_normals_.swap (normals);
618 }
619 
620 //////////////////////////////////////////////////////////////////////////////////////////////
621 template <int FeatureSize, typename PointT, typename NormalT> float
623 {
624  return (sampling_size_);
625 }
626 
627 //////////////////////////////////////////////////////////////////////////////////////////////
628 template <int FeatureSize, typename PointT, typename NormalT> void
630 {
631  if (sampling_size >= std::numeric_limits<float>::epsilon ())
632  sampling_size_ = sampling_size;
633 }
634 
635 //////////////////////////////////////////////////////////////////////////////////////////////
636 template <int FeatureSize, typename PointT, typename NormalT> typename pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::FeaturePtr
638 {
639  return (feature_estimator_);
640 }
641 
642 //////////////////////////////////////////////////////////////////////////////////////////////
643 template <int FeatureSize, typename PointT, typename NormalT> void
645 {
646  feature_estimator_ = feature;
647 }
648 
649 //////////////////////////////////////////////////////////////////////////////////////////////
650 template <int FeatureSize, typename PointT, typename NormalT> unsigned int
652 {
653  return (number_of_clusters_);
654 }
655 
656 //////////////////////////////////////////////////////////////////////////////////////////////
657 template <int FeatureSize, typename PointT, typename NormalT> void
659 {
660  if (num_of_clusters > 0)
661  number_of_clusters_ = num_of_clusters;
662 }
663 
664 //////////////////////////////////////////////////////////////////////////////////////////////
665 template <int FeatureSize, typename PointT, typename NormalT> std::vector<float>
667 {
668  return (training_sigmas_);
669 }
670 
671 //////////////////////////////////////////////////////////////////////////////////////////////
672 template <int FeatureSize, typename PointT, typename NormalT> void
674 {
675  training_sigmas_.clear ();
676  std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
677  training_sigmas_.swap (sigmas);
678 }
679 
680 //////////////////////////////////////////////////////////////////////////////////////////////
681 template <int FeatureSize, typename PointT, typename NormalT> bool
683 {
684  return (n_vot_ON_);
685 }
686 
687 //////////////////////////////////////////////////////////////////////////////////////////////
688 template <int FeatureSize, typename PointT, typename NormalT> void
690 {
691  n_vot_ON_ = state;
692 }
693 
694 //////////////////////////////////////////////////////////////////////////////////////////////
695 template <int FeatureSize, typename PointT, typename NormalT> bool
697 {
698  bool success = true;
699 
700  if (trained_model == nullptr)
701  return (false);
702  trained_model->reset ();
703 
704  std::vector<pcl::Histogram<FeatureSize> > histograms;
705  std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
706  success = extractDescriptors (histograms, locations);
707  if (!success)
708  return (false);
709 
710  Eigen::MatrixXi labels;
711  success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
712  if (!success)
713  return (false);
714 
715  std::vector<unsigned int> vec;
716  trained_model->clusters_.resize (number_of_clusters_, vec);
717  for (size_t i_label = 0; i_label < locations.size (); i_label++)
718  trained_model->clusters_[labels (i_label)].push_back (i_label);
719 
720  calculateSigmas (trained_model->sigmas_);
721 
723  locations,
724  labels,
725  trained_model->sigmas_,
726  trained_model->clusters_,
727  trained_model->statistical_weights_,
728  trained_model->learned_weights_);
729 
730  trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
731  trained_model->number_of_visual_words_ = static_cast<unsigned int> (histograms.size ());
732  trained_model->number_of_clusters_ = number_of_clusters_;
733  trained_model->descriptors_dimension_ = FeatureSize;
734 
735  trained_model->directions_to_center_.resize (locations.size (), 3);
736  trained_model->classes_.resize (locations.size ());
737  for (size_t i_dir = 0; i_dir < locations.size (); i_dir++)
738  {
739  trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
740  trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
741  trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
742  trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
743  }
744 
745  return (true);
746 }
747 
748 //////////////////////////////////////////////////////////////////////////////////////////////
749 template <int FeatureSize, typename PointT, typename NormalT> typename pcl::features::ISMVoteList<PointT>::Ptr
751  ISMModelPtr model,
752  typename pcl::PointCloud<PointT>::Ptr in_cloud,
753  typename pcl::PointCloud<Normal>::Ptr in_normals,
754  int in_class_of_interest)
755 {
757 
758  if (in_cloud->points.empty ())
759  return (out_votes);
760 
761  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
762  typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
763  simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
764  if (sampled_point_cloud->points.empty ())
765  return (out_votes);
766 
768  estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
769 
770  //find nearest cluster
771  const unsigned int n_key_points = static_cast<unsigned int> (sampled_point_cloud->size ());
772  std::vector<int> min_dist_inds (n_key_points, -1);
773  for (unsigned int i_point = 0; i_point < n_key_points; i_point++)
774  {
775  Eigen::VectorXf curr_descriptor (FeatureSize);
776  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
777  curr_descriptor (i_dim) = feature_cloud->points[i_point].histogram[i_dim];
778 
779  float descriptor_sum = curr_descriptor.sum ();
780  if (descriptor_sum < std::numeric_limits<float>::epsilon ())
781  continue;
782 
783  unsigned int min_dist_idx = 0;
784  Eigen::VectorXf clusters_center (FeatureSize);
785  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
786  clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
787 
788  float best_dist = computeDistance (curr_descriptor, clusters_center);
789  for (unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
790  {
791  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
792  clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
793  float curr_dist = computeDistance (clusters_center, curr_descriptor);
794  if (curr_dist < best_dist)
795  {
796  min_dist_idx = i_clust_cent;
797  best_dist = curr_dist;
798  }
799  }
800  min_dist_inds[i_point] = min_dist_idx;
801  }//next keypoint
802 
803  for (size_t i_point = 0; i_point < n_key_points; i_point++)
804  {
805  int min_dist_idx = min_dist_inds[i_point];
806  if (min_dist_idx == -1)
807  continue;
808 
809  const unsigned int n_words = static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
810  //compute coord system transform
811  Eigen::Matrix3f transform = alignYCoordWithNormal (sampled_normal_cloud->points[i_point]);
812  for (unsigned int i_word = 0; i_word < n_words; i_word++)
813  {
814  unsigned int index = model->clusters_[min_dist_idx][i_word];
815  unsigned int i_class = model->classes_[index];
816  if (static_cast<int> (i_class) != in_class_of_interest)
817  continue;//skip this class
818 
819  //rotate dir to center as needed
820  Eigen::Vector3f direction (
821  model->directions_to_center_(index, 0),
822  model->directions_to_center_(index, 1),
823  model->directions_to_center_(index, 2));
824  applyTransform (direction, transform.transpose ());
825 
826  pcl::InterestPoint vote;
827  Eigen::Vector3f vote_pos = sampled_point_cloud->points[i_point].getVector3fMap () + direction;
828  vote.x = vote_pos[0];
829  vote.y = vote_pos[1];
830  vote.z = vote_pos[2];
831  float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
832  float learned_weight = model->learned_weights_[index];
833  float power = statistical_weight * learned_weight;
834  vote.strength = power;
835  if (vote.strength > std::numeric_limits<float>::epsilon ())
836  out_votes->addVote (vote, sampled_point_cloud->points[i_point], i_class);
837  }
838  }//next point
839 
840  return (out_votes);
841 }
842 
843 //////////////////////////////////////////////////////////////////////////////////////////////
844 template <int FeatureSize, typename PointT, typename NormalT> bool
846  std::vector< pcl::Histogram<FeatureSize> >& histograms,
847  std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
848 {
849  histograms.clear ();
850  locations.clear ();
851 
852  int n_key_points = 0;
853 
854  if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ == nullptr)
855  return (false);
856 
857  for (size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
858  {
859  //compute the center of the training object
860  Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
861  const size_t num_of_points = training_clouds_[i_cloud]->points.size ();
862  for (auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
863  models_center += point_j->getVector3fMap ();
864  models_center /= static_cast<float> (num_of_points);
865 
866  //downsample the cloud
867  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
868  typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
869  simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
870  if (sampled_point_cloud->points.empty ())
871  continue;
872 
873  shiftCloud (training_clouds_[i_cloud], models_center);
874  shiftCloud (sampled_point_cloud, models_center);
875 
876  n_key_points += static_cast<int> (sampled_point_cloud->size ());
877 
879  estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
880 
881  int point_index = 0;
882  for (auto point_i = sampled_point_cloud->points.cbegin (); point_i != sampled_point_cloud->points.cend (); point_i++, point_index++)
883  {
884  float descriptor_sum = Eigen::VectorXf::Map (feature_cloud->points[point_index].histogram, FeatureSize).sum ();
885  if (descriptor_sum < std::numeric_limits<float>::epsilon ())
886  continue;
887 
888  histograms.insert ( histograms.end (), feature_cloud->begin () + point_index, feature_cloud->begin () + point_index + 1 );
889 
890  int dist = static_cast<int> (std::distance (sampled_point_cloud->points.cbegin (), point_i));
891  Eigen::Matrix3f new_basis = alignYCoordWithNormal (sampled_normal_cloud->points[dist]);
892  Eigen::Vector3f zero;
893  zero (0) = 0.0;
894  zero (1) = 0.0;
895  zero (2) = 0.0;
896  Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
897  applyTransform (new_dir, new_basis);
898 
899  PointT point (new_dir[0], new_dir[1], new_dir[2]);
900  LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, sampled_normal_cloud->points[dist]);
901  locations.insert(locations.end (), info);
902  }
903  }//next training cloud
904 
905  return (true);
906 }
907 
908 //////////////////////////////////////////////////////////////////////////////////////////////
909 template <int FeatureSize, typename PointT, typename NormalT> bool
911  std::vector< pcl::Histogram<FeatureSize> >& histograms,
912  Eigen::MatrixXi& labels,
913  Eigen::MatrixXf& clusters_centers)
914 {
915  Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
916 
917  for (size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
918  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
919  points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
920 
921  labels.resize (histograms.size(), 1);
923  points_to_cluster,
925  labels,
927  5,
928  PP_CENTERS,
929  clusters_centers);
930 
931  return (true);
932 }
933 
934 //////////////////////////////////////////////////////////////////////////////////////////////
935 template <int FeatureSize, typename PointT, typename NormalT> void
937 {
938  if (!training_sigmas_.empty ())
939  {
940  sigmas.resize (training_sigmas_.size (), 0.0f);
941  for (size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
942  sigmas[i_sigma] = training_sigmas_[i_sigma];
943  return;
944  }
945 
946  sigmas.clear ();
947  unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
948  sigmas.resize (number_of_classes, 0.0f);
949 
950  std::vector<float> vec;
951  std::vector<std::vector<float> > objects_sigmas;
952  objects_sigmas.resize (number_of_classes, vec);
953 
954  unsigned int number_of_objects = static_cast<unsigned int> (training_clouds_.size ());
955  for (unsigned int i_object = 0; i_object < number_of_objects; i_object++)
956  {
957  float max_distance = 0.0f;
958  unsigned int number_of_points = static_cast<unsigned int> (training_clouds_[i_object]->points.size ());
959  for (unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
960  for (unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
961  {
962  float curr_distance = 0.0f;
963  curr_distance += training_clouds_[i_object]->points[i_point].x * training_clouds_[i_object]->points[j_point].x;
964  curr_distance += training_clouds_[i_object]->points[i_point].y * training_clouds_[i_object]->points[j_point].y;
965  curr_distance += training_clouds_[i_object]->points[i_point].z * training_clouds_[i_object]->points[j_point].z;
966  if (curr_distance > max_distance)
967  max_distance = curr_distance;
968  }
969  max_distance = static_cast<float> (sqrt (max_distance));
970  unsigned int i_class = training_classes_[i_object];
971  objects_sigmas[i_class].push_back (max_distance);
972  }
973 
974  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
975  {
976  float sig = 0.0f;
977  unsigned int number_of_objects_in_class = static_cast<unsigned int> (objects_sigmas[i_class].size ());
978  for (unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
979  sig += objects_sigmas[i_class][i_object];
980  sig /= (static_cast<float> (number_of_objects_in_class) * 10.0f);
981  sigmas[i_class] = sig;
982  }
983 }
984 
985 //////////////////////////////////////////////////////////////////////////////////////////////
986 template <int FeatureSize, typename PointT, typename NormalT> void
988  const std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
989  const Eigen::MatrixXi &labels,
990  std::vector<float>& sigmas,
991  std::vector<std::vector<unsigned int> >& clusters,
992  std::vector<std::vector<float> >& statistical_weights,
993  std::vector<float>& learned_weights)
994 {
995  unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
996  //Temporary variable
997  std::vector<float> vec;
998  vec.resize (number_of_clusters_, 0.0f);
999  statistical_weights.clear ();
1000  learned_weights.clear ();
1001  statistical_weights.resize (number_of_classes, vec);
1002  learned_weights.resize (locations.size (), 0.0f);
1003 
1004  //Temporary variable
1005  std::vector<int> vect;
1006  vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
1007 
1008  //Number of features from which c_i was learned
1009  std::vector<int> n_ftr;
1010 
1011  //Total number of votes from visual word v_j
1012  std::vector<int> n_vot;
1013 
1014  //Number of visual words that vote for class c_i
1015  std::vector<int> n_vw;
1016 
1017  //Number of votes for class c_i from v_j
1018  std::vector<std::vector<int> > n_vot_2;
1019 
1020  n_vot_2.resize (number_of_clusters_, vect);
1021  n_vot.resize (number_of_clusters_, 0);
1022  n_ftr.resize (number_of_classes, 0);
1023  for (size_t i_location = 0; i_location < locations.size (); i_location++)
1024  {
1025  int i_class = training_classes_[locations[i_location].model_num_];
1026  int i_cluster = labels (i_location);
1027  n_vot_2[i_cluster][i_class] += 1;
1028  n_vot[i_cluster] += 1;
1029  n_ftr[i_class] += 1;
1030  }
1031 
1032  n_vw.resize (number_of_classes, 0);
1033  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1034  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1035  if (n_vot_2[i_cluster][i_class] > 0)
1036  n_vw[i_class] += 1;
1037 
1038  //computing learned weights
1039  learned_weights.resize (locations.size (), 0.0);
1040  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1041  {
1042  unsigned int number_of_words_in_cluster = static_cast<unsigned int> (clusters[i_cluster].size ());
1043  for (unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1044  {
1045  unsigned int i_index = clusters[i_cluster][i_visual_word];
1046  int i_class = training_classes_[locations[i_index].model_num_];
1047  float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1048  if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1049  {
1050  std::vector<float> calculated_sigmas;
1051  calculateSigmas (calculated_sigmas);
1052  square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1053  if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1054  continue;
1055  }
1056  Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1057  Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1058  applyTransform (direction, transform);
1059  Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1060 
1061  //collect gaussian weighted distances
1062  std::vector<float> gauss_dists;
1063  for (unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1064  {
1065  unsigned int j_index = clusters[i_cluster][j_visual_word];
1066  int j_class = training_classes_[locations[j_index].model_num_];
1067  if (i_class != j_class)
1068  continue;
1069  //predict center
1070  Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1071  Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1072  applyTransform (direction_2, transform_2);
1073  Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1074  float residual = (predicted_center - actual_center).norm ();
1075  float value = -residual * residual / square_sigma_dist;
1076  gauss_dists.push_back (static_cast<float> (exp (value)));
1077  }//next word
1078  //find median gaussian weighted distance
1079  size_t mid_elem = (gauss_dists.size () - 1) / 2;
1080  std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1081  learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1082  }//next word
1083  }//next cluster
1084 
1085  //computing statistical weights
1086  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1087  {
1088  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1089  {
1090  if (n_vot_2[i_cluster][i_class] == 0)
1091  continue;//no votes per class of interest in this cluster
1092  if (n_vw[i_class] == 0)
1093  continue;//there were no objects of this class in the training dataset
1094  if (n_vot[i_cluster] == 0)
1095  continue;//this cluster has never been used
1096  if (n_ftr[i_class] == 0)
1097  continue;//there were no objects of this class in the training dataset
1098  float part_1 = static_cast<float> (n_vw[i_class]);
1099  float part_2 = static_cast<float> (n_vot[i_cluster]);
1100  float part_3 = static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1101  float part_4 = 0.0f;
1102 
1103  if (!n_vot_ON_)
1104  part_2 = 1.0f;
1105 
1106  for (unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1107  if (n_ftr[j_class] != 0)
1108  part_4 += static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1109 
1110  statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1111  }
1112  }//next cluster
1113 }
1114 
1115 //////////////////////////////////////////////////////////////////////////////////////////////
1116 template <int FeatureSize, typename PointT, typename NormalT> void
1118  typename pcl::PointCloud<PointT>::ConstPtr in_point_cloud,
1119  typename pcl::PointCloud<NormalT>::ConstPtr in_normal_cloud,
1120  typename pcl::PointCloud<PointT>::Ptr out_sampled_point_cloud,
1121  typename pcl::PointCloud<NormalT>::Ptr out_sampled_normal_cloud)
1122 {
1123  //create voxel grid
1126  grid.setSaveLeafLayout (true);
1127  grid.setInputCloud (in_point_cloud);
1128 
1129  pcl::PointCloud<PointT> temp_cloud;
1130  grid.filter (temp_cloud);
1131 
1132  //extract indices of points from source cloud which are closest to grid points
1133  const float max_value = std::numeric_limits<float>::max ();
1134 
1135  const size_t num_source_points = in_point_cloud->points.size ();
1136  const size_t num_sample_points = temp_cloud.points.size ();
1137 
1138  std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1139  std::vector<int> sampling_indices (num_sample_points, -1);
1140 
1141  for (size_t i_point = 0; i_point < num_source_points; i_point++)
1142  {
1143  int index = grid.getCentroidIndex (in_point_cloud->points[i_point]);
1144  if (index == -1)
1145  continue;
1146 
1147  PointT pt_1 = in_point_cloud->points[i_point];
1148  PointT pt_2 = temp_cloud.points[index];
1149 
1150  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);
1151  if (distance < dist_to_grid_center[index])
1152  {
1153  dist_to_grid_center[index] = distance;
1154  sampling_indices[index] = static_cast<int> (i_point);
1155  }
1156  }
1157 
1158  //extract source points
1159  pcl::PointIndices::Ptr final_inliers_indices (new pcl::PointIndices ());
1160  pcl::ExtractIndices<PointT> extract_points;
1161  pcl::ExtractIndices<NormalT> extract_normals;
1162 
1163  final_inliers_indices->indices.reserve (num_sample_points);
1164  for (size_t i_point = 0; i_point < num_sample_points; i_point++)
1165  {
1166  if (sampling_indices[i_point] != -1)
1167  final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1168  }
1169 
1170  extract_points.setInputCloud (in_point_cloud);
1171  extract_points.setIndices (final_inliers_indices);
1172  extract_points.filter (*out_sampled_point_cloud);
1173 
1174  extract_normals.setInputCloud (in_normal_cloud);
1175  extract_normals.setIndices (final_inliers_indices);
1176  extract_normals.filter (*out_sampled_normal_cloud);
1177 }
1178 
1179 //////////////////////////////////////////////////////////////////////////////////////////////
1180 template <int FeatureSize, typename PointT, typename NormalT> void
1182  typename pcl::PointCloud<PointT>::Ptr in_cloud,
1183  Eigen::Vector3f shift_point)
1184 {
1185  for (auto point_it = in_cloud->points.begin (); point_it != in_cloud->points.end (); point_it++)
1186  {
1187  point_it->x -= shift_point.x ();
1188  point_it->y -= shift_point.y ();
1189  point_it->z -= shift_point.z ();
1190  }
1191 }
1192 
1193 //////////////////////////////////////////////////////////////////////////////////////////////
1194 template <int FeatureSize, typename PointT, typename NormalT> Eigen::Matrix3f
1196 {
1197  Eigen::Matrix3f result;
1198  Eigen::Matrix3f rotation_matrix_X;
1199  Eigen::Matrix3f rotation_matrix_Z;
1200 
1201  float A = 0.0f;
1202  float B = 0.0f;
1203  float sign = -1.0f;
1204 
1205  float denom_X = static_cast<float> (sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1206  A = in_normal.normal_y / denom_X;
1207  B = sign * in_normal.normal_z / denom_X;
1208  rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1209  0.0f, A, -B,
1210  0.0f, B, A;
1211 
1212  float denom_Z = static_cast<float> (sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1213  A = in_normal.normal_y / denom_Z;
1214  B = sign * in_normal.normal_x / denom_Z;
1215  rotation_matrix_Z << A, -B, 0.0f,
1216  B, A, 0.0f,
1217  0.0f, 0.0f, 1.0f;
1218 
1219  result = rotation_matrix_X * rotation_matrix_Z;
1220 
1221  return (result);
1222 }
1223 
1224 //////////////////////////////////////////////////////////////////////////////////////////////
1225 template <int FeatureSize, typename PointT, typename NormalT> void
1226 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform)
1227 {
1228  io_vec = in_transform * io_vec;
1229 }
1230 
1231 //////////////////////////////////////////////////////////////////////////////////////////////
1232 template <int FeatureSize, typename PointT, typename NormalT> void
1234  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud,
1235  typename pcl::PointCloud<NormalT>::Ptr normal_cloud,
1236  typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud)
1237 {
1239 // tree->setInputCloud (point_cloud);
1240 
1241  feature_estimator_->setInputCloud (sampled_point_cloud->makeShared ());
1242 // feature_estimator_->setSearchSurface (point_cloud->makeShared ());
1243  feature_estimator_->setSearchMethod (tree);
1244 
1245 // typename pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm =
1246 // boost::dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1247 // feat_est_norm->setInputNormals (normal_cloud);
1248 
1251  feat_est_norm->setInputNormals (normal_cloud);
1252 
1253  feature_estimator_->compute (*feature_cloud);
1254 }
1255 
1256 //////////////////////////////////////////////////////////////////////////////////////////////
1257 template <int FeatureSize, typename PointT, typename NormalT> double
1259  const Eigen::MatrixXf& points_to_cluster,
1260  int number_of_clusters,
1261  Eigen::MatrixXi& io_labels,
1262  TermCriteria criteria,
1263  int attempts,
1264  int flags,
1265  Eigen::MatrixXf& cluster_centers)
1266 {
1267  const int spp_trials = 3;
1268  size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1269  int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1270 
1271  attempts = std::max (attempts, 1);
1272  srand (static_cast<unsigned int> (time (nullptr)));
1273 
1274  Eigen::MatrixXi labels (number_of_points, 1);
1275 
1276  if (flags & USE_INITIAL_LABELS)
1277  labels = io_labels;
1278  else
1279  labels.setZero ();
1280 
1281  Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1282  Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1283  std::vector<int> counters (number_of_clusters);
1284  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1285  Eigen::Vector2f* box = &boxes[0];
1286 
1287  double best_compactness = std::numeric_limits<double>::max ();
1288  double compactness = 0.0;
1289 
1290  if (criteria.type_ & TermCriteria::EPS)
1291  criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f);
1292  else
1293  criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1294 
1295  criteria.epsilon_ *= criteria.epsilon_;
1296 
1297  if (criteria.type_ & TermCriteria::COUNT)
1298  criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100);
1299  else
1300  criteria.max_count_ = 100;
1301 
1302  if (number_of_clusters == 1)
1303  {
1304  attempts = 1;
1305  criteria.max_count_ = 2;
1306  }
1307 
1308  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1309  box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1310 
1311  for (size_t i_point = 0; i_point < number_of_points; i_point++)
1312  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1313  {
1314  float v = points_to_cluster (i_point, i_dim);
1315  box[i_dim] (0) = std::min (box[i_dim] (0), v);
1316  box[i_dim] (1) = std::max (box[i_dim] (1), v);
1317  }
1318 
1319  for (int i_attempt = 0; i_attempt < attempts; i_attempt++)
1320  {
1321  float max_center_shift = std::numeric_limits<float>::max ();
1322  for (int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++)
1323  {
1324  Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1325  temp = centers;
1326  centers = old_centers;
1327  old_centers = temp;
1328 
1329  if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1330  {
1331  if (flags & PP_CENTERS)
1332  generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1333  else
1334  {
1335  for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
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  }//generate center for next cluster
1342  }//end if-else random or PP centers
1343  }
1344  else
1345  {
1346  centers.setZero ();
1347  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1348  counters[i_cluster] = 0;
1349  for (size_t i_point = 0; i_point < number_of_points; i_point++)
1350  {
1351  int i_label = labels (i_point, 0);
1352  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1353  centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1354  counters[i_label]++;
1355  }
1356  if (iter > 0)
1357  max_center_shift = 0.0f;
1358  for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1359  {
1360  if (counters[i_cl_center] != 0)
1361  {
1362  float scale = 1.0f / static_cast<float> (counters[i_cl_center]);
1363  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1364  centers (i_cl_center, i_dim) *= scale;
1365  }
1366  else
1367  {
1368  Eigen::VectorXf center (feature_dimension);
1369  generateRandomCenter (boxes, center);
1370  for(int i_dim = 0; i_dim < feature_dimension; i_dim++)
1371  centers (i_cl_center, i_dim) = center (i_dim);
1372  }
1373 
1374  if (iter > 0)
1375  {
1376  float dist = 0.0f;
1377  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1378  {
1379  float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1380  dist += diff * diff;
1381  }
1382  max_center_shift = std::max (max_center_shift, dist);
1383  }
1384  }
1385  }
1386  compactness = 0.0f;
1387  for (size_t i_point = 0; i_point < number_of_points; i_point++)
1388  {
1389  Eigen::VectorXf sample (feature_dimension);
1390  sample = points_to_cluster.row (i_point);
1391 
1392  int k_best = 0;
1393  float min_dist = std::numeric_limits<float>::max ();
1394 
1395  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1396  {
1397  Eigen::VectorXf center (feature_dimension);
1398  center = centers.row (i_cluster);
1399  float dist = computeDistance (sample, center);
1400  if (min_dist > dist)
1401  {
1402  min_dist = dist;
1403  k_best = i_cluster;
1404  }
1405  }
1406  compactness += min_dist;
1407  labels (i_point, 0) = k_best;
1408  }
1409  }//next iteration
1410 
1411  if (compactness < best_compactness)
1412  {
1413  best_compactness = compactness;
1414  cluster_centers = centers;
1415  io_labels = labels;
1416  }
1417  }//next attempt
1418 
1419  return (best_compactness);
1420 }
1421 
1422 //////////////////////////////////////////////////////////////////////////////////////////////
1423 template <int FeatureSize, typename PointT, typename NormalT> void
1425  const Eigen::MatrixXf& data,
1426  Eigen::MatrixXf& out_centers,
1427  int number_of_clusters,
1428  int trials)
1429 {
1430  size_t dimension = data.cols ();
1431  unsigned int number_of_points = static_cast<unsigned int> (data.rows ());
1432  std::vector<int> centers_vec (number_of_clusters);
1433  int* centers = &centers_vec[0];
1434  std::vector<double> dist (number_of_points);
1435  std::vector<double> tdist (number_of_points);
1436  std::vector<double> tdist2 (number_of_points);
1437  double sum0 = 0.0;
1438 
1439  unsigned int random_unsigned = rand ();
1440  centers[0] = random_unsigned % number_of_points;
1441 
1442  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1443  {
1444  Eigen::VectorXf first (dimension);
1445  Eigen::VectorXf second (dimension);
1446  first = data.row (i_point);
1447  second = data.row (centers[0]);
1448  dist[i_point] = computeDistance (first, second);
1449  sum0 += dist[i_point];
1450  }
1451 
1452  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1453  {
1454  double best_sum = std::numeric_limits<double>::max ();
1455  int best_center = -1;
1456  for (int i_trials = 0; i_trials < trials; i_trials++)
1457  {
1458  unsigned int random_integer = rand () - 1;
1459  double random_double = static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1460  double p = random_double * sum0;
1461 
1462  unsigned int i_point;
1463  for (i_point = 0; i_point < number_of_points - 1; i_point++)
1464  if ( (p -= dist[i_point]) <= 0.0)
1465  break;
1466 
1467  int ci = i_point;
1468 
1469  double s = 0.0;
1470  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1471  {
1472  Eigen::VectorXf first (dimension);
1473  Eigen::VectorXf second (dimension);
1474  first = data.row (i_point);
1475  second = data.row (ci);
1476  tdist2[i_point] = std::min (static_cast<double> (computeDistance (first, second)), dist[i_point]);
1477  s += tdist2[i_point];
1478  }
1479 
1480  if (s <= best_sum)
1481  {
1482  best_sum = s;
1483  best_center = ci;
1484  std::swap (tdist, tdist2);
1485  }
1486  }
1487 
1488  centers[i_cluster] = best_center;
1489  sum0 = best_sum;
1490  std::swap (dist, tdist);
1491  }
1492 
1493  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1494  for (size_t i_dim = 0; i_dim < dimension; i_dim++)
1495  out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1496 }
1497 
1498 //////////////////////////////////////////////////////////////////////////////////////////////
1499 template <int FeatureSize, typename PointT, typename NormalT> void
1500 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateRandomCenter (const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >& boxes,
1501  Eigen::VectorXf& center)
1502 {
1503  size_t dimension = boxes.size ();
1504  float margin = 1.0f / static_cast<float> (dimension);
1505 
1506  for (size_t i_dim = 0; i_dim < dimension; i_dim++)
1507  {
1508  unsigned int random_integer = rand () - 1;
1509  float random_float = static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1510  center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1511  }
1512 }
1513 
1514 //////////////////////////////////////////////////////////////////////////////////////////////
1515 template <int FeatureSize, typename PointT, typename NormalT> float
1517 {
1518  size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1519  float distance = 0.0f;
1520  for(size_t i_dim = 0; i_dim < dimension; i_dim++)
1521  {
1522  float diff = vec_1 (i_dim) - vec_2 (i_dim);
1523  distance += diff * diff;
1524  }
1525 
1526  return (distance);
1527 }
1528 
1529 #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:67
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.
size_t size() const
Definition: point_cloud.h:449
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:411
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...
std::vector< int > k_ind_
Stores neighbours indices.
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:619
static const int PP_CENTERS
This const value is used for indicating that for k-means clustering centers must be generated as desc...
std::vector< float > getSigmaDists()
Returns the array of sigma values.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
virtual ~ISMVoteList()
virtual descriptor.
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. ...
float sampling_size_
This value is used for the simplification.
pcl::PointCloud< PointT >::Ptr votes_origins_
Stores the origins of the votes.
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
Definition: voxel_grid.h:318
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
pcl::PointCloud< pcl::InterestPoint >::Ptr votes_
Stores all votes.
std::vector< int > votes_class_
Stores classes for which every single vote was cast.
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
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...
std::vector< float > training_sigmas_
This array stores the sigma values for each training class.
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:279
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:130
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:177
bool n_vot_ON_
If set to false then Nvot coeff from [Knopp et al., 2010, (4)] is equal 1.0.
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.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
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.
boost::shared_ptr< pcl::features::ISMModel > ISMModelPtr
A point structure representing an N-D histogram.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
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.
boost::shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
A point structure representing Euclidean xyz coordinates.
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...
std::vector< std::vector< unsigned int > > clusters_
This is an array of clusters.
void reset()
this method resets all variables and frees memory.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
static const int USE_INITIAL_LABELS
This const value is used for indicating that input labels must be taken as the initial approximation ...
std::vector< float > k_sqr_dist_
Stores square distances to the corresponding neighbours.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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...
boost::shared_ptr< ISMVoteList< PointT > > Ptr
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.
void filter(PointCloud &output)
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...
pcl::KdTreeFLANN< pcl::InterestPoint >::Ptr tree_
Stores the search tree.
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.
unsigned int number_of_clusters_
Number of clusters, is used for clustering descriptors during the training.
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).
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
std::vector< typename pcl::PointCloud< PointT >::Ptr > training_clouds_
Stores the clouds used for training.
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
This struct is used for storing peak.
Feature::Ptr feature_estimator_
Stores the feature estimator.
unsigned int number_of_visual_words_
Stores the number of visual words.
bool tree_is_valid_
Signalizes if the tree is valid.
iterator begin()
Definition: point_cloud.h:443
std::vector< unsigned int > classes_
Stores the class label for every direction.
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.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:80
std::vector< typename pcl::PointCloud< NormalT >::Ptr > training_normals_
Stores the normals for each training cloud.
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:344
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...
std::vector< unsigned int > training_classes_
Stores the class number for each cloud from training_clouds_.
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:222
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm...