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