Point Cloud Library (PCL)  1.11.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 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 #include <pcl/memory.h> // for dynamic_pointer_cast
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT>
51  votes_ (new pcl::PointCloud<pcl::InterestPoint> ()),
52  tree_is_valid_ (false),
53  votes_origins_ (new pcl::PointCloud<PointT> ()),
54  votes_class_ (0),
55  k_ind_ (0),
56  k_sqr_dist_ (0)
57 {
58 }
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointT>
63 {
64  votes_class_.clear ();
65  votes_origins_.reset ();
66  votes_.reset ();
67  k_ind_.clear ();
68  k_sqr_dist_.clear ();
69  tree_.reset ();
70 }
71 
72 //////////////////////////////////////////////////////////////////////////////////////////////
73 template <typename PointT> void
75  pcl::InterestPoint& vote, const PointT &vote_origin, int votes_class)
76 {
77  tree_is_valid_ = false;
78  votes_->points.insert (votes_->points.end (), vote);// TODO: adjust height and width
79 
80  votes_origins_->points.push_back (vote_origin);
81  votes_class_.push_back (votes_class);
82 }
83 
84 //////////////////////////////////////////////////////////////////////////////////////////////
85 template <typename PointT> typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr
87 {
88  pcl::PointXYZRGB point;
90  colored_cloud->height = 0;
91  colored_cloud->width = 1;
92 
93  if (cloud != nullptr)
94  {
95  colored_cloud->height += cloud->size ();
96  point.r = 255;
97  point.g = 255;
98  point.b = 255;
99  for (const auto& i_point: *cloud)
100  {
101  point.x = i_point.x;
102  point.y = i_point.y;
103  point.z = i_point.z;
104  colored_cloud->points.push_back (point);
105  }
106  }
107 
108  point.r = 0;
109  point.g = 0;
110  point.b = 255;
111  for (const auto &i_vote : votes_->points)
112  {
113  point.x = i_vote.x;
114  point.y = i_vote.y;
115  point.z = i_vote.z;
116  colored_cloud->points.push_back (point);
117  }
118  colored_cloud->height += votes_->size ();
119 
120  return (colored_cloud);
121 }
122 
123 //////////////////////////////////////////////////////////////////////////////////////////////
124 template <typename PointT> void
126  std::vector<pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
127  int in_class_id,
128  double in_non_maxima_radius,
129  double in_sigma)
130 {
131  validateTree ();
132 
133  const std::size_t n_vote_classes = votes_class_.size ();
134  if (n_vote_classes == 0)
135  return;
136  for (std::size_t i = 0; i < n_vote_classes ; i++)
137  assert ( votes_class_[i] == in_class_id );
138 
139  // heuristic: start from NUM_INIT_PTS different locations selected uniformly
140  // on the votes. Intuitively, it is likely to get a good location in dense regions.
141  const int NUM_INIT_PTS = 100;
142  double SIGMA_DIST = in_sigma;// rule of thumb: 10% of the object radius
143  const double FINAL_EPS = SIGMA_DIST / 100;// another heuristic
144 
145  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
146  std::vector<double> peak_densities (NUM_INIT_PTS);
147  double max_density = -1.0;
148  for (int i = 0; i < NUM_INIT_PTS; i++)
149  {
150  Eigen::Vector3f old_center;
151  const auto idx = votes_->size() * i / NUM_INIT_PTS;
152  Eigen::Vector3f curr_center = (*votes_)[idx].getVector3fMap();
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)
230  tree_.reset (new pcl::KdTreeFLANN<pcl::InterestPoint>);
231  tree_->setInputCloud (votes_);
232  k_ind_.resize ( votes_->size (), -1 );
233  k_sqr_dist_.resize ( votes_->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  std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
252 
253  for (std::size_t j = 0; j < n_pts; j++)
254  {
255  double kernel = (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
256  Eigen::Vector3f vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[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 std::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  std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
283 
284  for (std::size_t j = 0; j < num_of_pts; j++)
285  sum_vote += (*votes_)[k_ind_[j]].strength * std::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_->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 
316  this->number_of_classes_ = copy.number_of_classes_;
317  this->number_of_visual_words_ = copy.number_of_visual_words_;
318  this->number_of_clusters_ = copy.number_of_clusters_;
319  this->descriptors_dimension_ = copy.descriptors_dimension_;
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);
435  statistical_weights_.resize (number_of_classes_, vec);
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
441  learned_weights_.resize (number_of_visual_words_, 0.0f);
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;
495  number_of_visual_words_ = 0;
496  number_of_clusters_ = 0;
497  descriptors_dimension_ = 0;
498 }
499 
500 //////////////////////////////////////////////////////////////////////////////////////////////
503 {
504  if (this != &other)
505  {
506  this->reset ();
507 
508  this->number_of_classes_ = other.number_of_classes_;
509  this->number_of_visual_words_ = other.number_of_visual_words_;
510  this->number_of_clusters_ = other.number_of_clusters_;
511  this->descriptors_dimension_ = other.descriptors_dimension_;
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 (std::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 
722  calculateWeights(
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 (std::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)[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 (std::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)[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)[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)[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 (std::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 auto num_of_points = training_clouds_[i_cloud]->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)[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)[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)[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 (std::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);
922  computeKMeansClustering (
923  points_to_cluster,
924  number_of_clusters_,
925  labels,
926  TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),//1000
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 (std::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  const auto number_of_points = training_clouds_[i_object]->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])[i_point].x * (*training_clouds_[i_object])[j_point].x;
964  curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
965  curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[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 (std::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> (std::exp (value)));
1077  }//next word
1078  //find median gaussian weighted distance
1079  std::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
1125  grid.setLeafSize (sampling_size_, sampling_size_, sampling_size_);
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 auto num_source_points = in_point_cloud->size ();
1136  const auto num_sample_points = temp_cloud.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 (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1142  {
1143  int index = grid.getCentroidIndex ((*in_point_cloud)[i_point]);
1144  if (index == -1)
1145  continue;
1146 
1147  PointT pt_1 = (*in_point_cloud)[i_point];
1148  PointT pt_2 = temp_cloud[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 (std::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 // dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1247 // feat_est_norm->setInputNormals (normal_cloud);
1248 
1250  dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
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  std::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 (std::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 (std::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 (std::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  std::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 (std::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  std::size_t dimension = boxes.size ();
1504  float margin = 1.0f / static_cast<float> (dimension);
1505 
1506  for (std::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  std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1519  float distance = 0.0f;
1520  for(std::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_
ExtractIndices extracts a set of indices from a point cloud.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: feature.h:345
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:124
void filter(std::vector< int > &indices)
Calls the filtering method and returns the filtered point cloud indices.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:181
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
std::size_t size() const
Definition: point_cloud.h:459
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
iterator begin() noexcept
Definition: point_cloud.h:445
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed,...
Definition: point_cloud.h:704
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
Definition: voxel_grid.h:178
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
Definition: voxel_grid.h:318
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition: voxel_grid.h:222
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
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm.
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
shared_ptr< ISMVoteList< PointT > > Ptr
void addVote(pcl::InterestPoint &in_vote, const PointT &vote_origin, int in_class)
This method simply adds another vote to the list.
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).
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...
unsigned int getNumberOfVotes()
This method simply returns the number of votes.
void validateTree()
this method is simply setting up the search tree.
double getDensityAtPoint(const PointT &point, double sigma_dist)
Returns the density at the specified point.
virtual ~ISMVoteList()
virtual descriptor.
ISMVoteList()
Empty constructor with member variables initialization.
bool trainISM(ISMModelPtr &trained_model)
This method performs training and forms a visual vocabulary.
void applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
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< typename pcl::PointCloud< PointT >::Ptr > getTrainingClouds()
This method simply returns the clouds that were set as the training clouds.
void generateRandomCenter(const std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > &boxes, Eigen::VectorXf &center)
Generates random center for cluster.
void setFeatureEstimator(FeaturePtr feature)
Changes the feature estimator.
void setTrainingClouds(const std::vector< typename pcl::PointCloud< PointT >::Ptr > &training_clouds)
Allows to set clouds for training the ISM model.
void setNVotState(bool state)
Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
float computeDistance(Eigen::VectorXf &vec_1, Eigen::VectorXf &vec_2)
Computes the square distance between two vectors.
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
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.
pcl::features::ISMModel::Ptr ISMModelPtr
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
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 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++...
ImplicitShapeModelEstimation()
Simple constructor that initializes everything.
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.
std::vector< unsigned int > getTrainingClasses()
Returns the array of classes that indicates which class the corresponding training cloud belongs.
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 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.
bool extractDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations)
Extracts the descriptors from the input clouds.
FeaturePtr getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
unsigned int getNumberOfClusters()
Returns the number of clusters used for descriptor clustering.
void calculateSigmas(std::vector< float > &sigmas)
This method calculates the value of sigma used for calculating the learned weights for every single c...
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
bool clusterDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, Eigen::MatrixXi &labels, Eigen::MatrixXf &clusters_centers)
This method performs descriptor clustering.
std::vector< float > getSigmaDists()
Returns the array of sigma values.
void setSamplingSize(float sampling_size)
Changes the sampling size used for cloud simplification.
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 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.
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...
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the corresponding cloud of normals for every training point cloud.
void setTrainingClasses(const std::vector< unsigned int > &training_classes)
Allows to set the class labels for the corresponding training clouds.
float getSamplingSize()
Returns the sampling size used for cloud simplification.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
@ B
Definition: norms.h:54
Defines functions, macros and traits for allocating and using memory.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
A point structure representing an N-D histogram.
This struct is used for storing peak.
double density
Density of this peak.
int class_id
Determines which class this peak belongs.
A point structure representing an interest point with Euclidean xyz coordinates, and an interest valu...
A point structure representing normal coordinates and the surface curvature estimate.
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:15
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.
The assignment of this structure is to store the statistical/learned weights and other information of...
unsigned int number_of_clusters_
Stores the number of clusters.
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
ISMModel()
Simple constructor that initializes the structure.
bool loadModelFromfile(std::string &file_name)
This method loads the trained model from file.
ISMModel & operator=(const ISMModel &other)
Operator overloading for deep copy.
virtual ~ISMModel()
Destructor that frees memory.
unsigned int descriptors_dimension_
Stores descriptors dimension.
std::vector< float > sigmas_
Stores the sigma value for each class.
std::vector< float > learned_weights_
Stores learned weights.
void reset()
this method resets all variables and frees memory.
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
std::vector< unsigned int > classes_
Stores the class label for every direction.
Eigen::MatrixXf directions_to_center_
Stores the directions to objects center for each visual word.
unsigned int number_of_classes_
Stores the number of classes.
std::vector< std::vector< float > > statistical_weights_
Stores statistical weights.
unsigned int number_of_visual_words_
Stores the number of visual words.
This structure stores the information about the keypoint.
This structure is used for determining the end of the k-means clustering process.
int type_
Flag that determines when the k-means clustering must be stopped.
float epsilon_
Defines the accuracy for k-means clustering.
int max_count_
Defines maximum number of iterations for k-means clustering.