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