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