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