Point Cloud Library (PCL)  1.12.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 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 auto 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 auto 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  if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ == nullptr)
855  return (false);
856 
857  for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
858  {
859  //compute the center of the training object
860  Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
861  const auto num_of_points = training_clouds_[i_cloud]->size ();
862  for (auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
863  models_center += point_j->getVector3fMap ();
864  models_center /= static_cast<float> (num_of_points);
865 
866  //downsample the cloud
867  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
868  typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
869  simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
870  if (sampled_point_cloud->points.empty ())
871  continue;
872 
873  shiftCloud (training_clouds_[i_cloud], models_center);
874  shiftCloud (sampled_point_cloud, models_center);
875 
877  estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
878 
879  int point_index = 0;
880  for (auto point_i = sampled_point_cloud->points.cbegin (); point_i != sampled_point_cloud->points.cend (); point_i++, point_index++)
881  {
882  float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
883  if (descriptor_sum < std::numeric_limits<float>::epsilon ())
884  continue;
885 
886  histograms.insert ( histograms.end (), feature_cloud->begin () + point_index, feature_cloud->begin () + point_index + 1 );
887 
888  int dist = static_cast<int> (std::distance (sampled_point_cloud->points.cbegin (), point_i));
889  Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
890  Eigen::Vector3f zero;
891  zero (0) = 0.0;
892  zero (1) = 0.0;
893  zero (2) = 0.0;
894  Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
895  applyTransform (new_dir, new_basis);
896 
897  PointT point (new_dir[0], new_dir[1], new_dir[2]);
898  LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
899  locations.insert(locations.end (), info);
900  }
901  }//next training cloud
902 
903  return (true);
904 }
905 
906 //////////////////////////////////////////////////////////////////////////////////////////////
907 template <int FeatureSize, typename PointT, typename NormalT> bool
909  std::vector< pcl::Histogram<FeatureSize> >& histograms,
910  Eigen::MatrixXi& labels,
911  Eigen::MatrixXf& clusters_centers)
912 {
913  Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
914 
915  for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
916  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
917  points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
918 
919  labels.resize (histograms.size(), 1);
920  computeKMeansClustering (
921  points_to_cluster,
922  number_of_clusters_,
923  labels,
924  TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),//1000
925  5,
926  PP_CENTERS,
927  clusters_centers);
928 
929  return (true);
930 }
931 
932 //////////////////////////////////////////////////////////////////////////////////////////////
933 template <int FeatureSize, typename PointT, typename NormalT> void
935 {
936  if (!training_sigmas_.empty ())
937  {
938  sigmas.resize (training_sigmas_.size (), 0.0f);
939  for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
940  sigmas[i_sigma] = training_sigmas_[i_sigma];
941  return;
942  }
943 
944  sigmas.clear ();
945  unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
946  sigmas.resize (number_of_classes, 0.0f);
947 
948  std::vector<float> vec;
949  std::vector<std::vector<float> > objects_sigmas;
950  objects_sigmas.resize (number_of_classes, vec);
951 
952  auto number_of_objects = static_cast<unsigned int> (training_clouds_.size ());
953  for (unsigned int i_object = 0; i_object < number_of_objects; i_object++)
954  {
955  float max_distance = 0.0f;
956  const auto number_of_points = training_clouds_[i_object]->size ();
957  for (unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
958  for (unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
959  {
960  float curr_distance = 0.0f;
961  curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
962  curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
963  curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
964  if (curr_distance > max_distance)
965  max_distance = curr_distance;
966  }
967  max_distance = static_cast<float> (std::sqrt (max_distance));
968  unsigned int i_class = training_classes_[i_object];
969  objects_sigmas[i_class].push_back (max_distance);
970  }
971 
972  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
973  {
974  float sig = 0.0f;
975  auto number_of_objects_in_class = static_cast<unsigned int> (objects_sigmas[i_class].size ());
976  for (unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
977  sig += objects_sigmas[i_class][i_object];
978  sig /= (static_cast<float> (number_of_objects_in_class) * 10.0f);
979  sigmas[i_class] = sig;
980  }
981 }
982 
983 //////////////////////////////////////////////////////////////////////////////////////////////
984 template <int FeatureSize, typename PointT, typename NormalT> void
986  const std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
987  const Eigen::MatrixXi &labels,
988  std::vector<float>& sigmas,
989  std::vector<std::vector<unsigned int> >& clusters,
990  std::vector<std::vector<float> >& statistical_weights,
991  std::vector<float>& learned_weights)
992 {
993  unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
994  //Temporary variable
995  std::vector<float> vec;
996  vec.resize (number_of_clusters_, 0.0f);
997  statistical_weights.clear ();
998  learned_weights.clear ();
999  statistical_weights.resize (number_of_classes, vec);
1000  learned_weights.resize (locations.size (), 0.0f);
1001 
1002  //Temporary variable
1003  std::vector<int> vect;
1004  vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
1005 
1006  //Number of features from which c_i was learned
1007  std::vector<int> n_ftr;
1008 
1009  //Total number of votes from visual word v_j
1010  std::vector<int> n_vot;
1011 
1012  //Number of visual words that vote for class c_i
1013  std::vector<int> n_vw;
1014 
1015  //Number of votes for class c_i from v_j
1016  std::vector<std::vector<int> > n_vot_2;
1017 
1018  n_vot_2.resize (number_of_clusters_, vect);
1019  n_vot.resize (number_of_clusters_, 0);
1020  n_ftr.resize (number_of_classes, 0);
1021  for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
1022  {
1023  int i_class = training_classes_[locations[i_location].model_num_];
1024  int i_cluster = labels (i_location);
1025  n_vot_2[i_cluster][i_class] += 1;
1026  n_vot[i_cluster] += 1;
1027  n_ftr[i_class] += 1;
1028  }
1029 
1030  n_vw.resize (number_of_classes, 0);
1031  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1032  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1033  if (n_vot_2[i_cluster][i_class] > 0)
1034  n_vw[i_class] += 1;
1035 
1036  //computing learned weights
1037  learned_weights.resize (locations.size (), 0.0);
1038  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1039  {
1040  auto number_of_words_in_cluster = static_cast<unsigned int> (clusters[i_cluster].size ());
1041  for (unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1042  {
1043  unsigned int i_index = clusters[i_cluster][i_visual_word];
1044  int i_class = training_classes_[locations[i_index].model_num_];
1045  float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1046  if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1047  {
1048  std::vector<float> calculated_sigmas;
1049  calculateSigmas (calculated_sigmas);
1050  square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1051  if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1052  continue;
1053  }
1054  Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1055  Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1056  applyTransform (direction, transform);
1057  Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1058 
1059  //collect gaussian weighted distances
1060  std::vector<float> gauss_dists;
1061  for (unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1062  {
1063  unsigned int j_index = clusters[i_cluster][j_visual_word];
1064  int j_class = training_classes_[locations[j_index].model_num_];
1065  if (i_class != j_class)
1066  continue;
1067  //predict center
1068  Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1069  Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1070  applyTransform (direction_2, transform_2);
1071  Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1072  float residual = (predicted_center - actual_center).norm ();
1073  float value = -residual * residual / square_sigma_dist;
1074  gauss_dists.push_back (static_cast<float> (std::exp (value)));
1075  }//next word
1076  //find median gaussian weighted distance
1077  std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1078  std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1079  learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1080  }//next word
1081  }//next cluster
1082 
1083  //computing statistical weights
1084  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1085  {
1086  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1087  {
1088  if (n_vot_2[i_cluster][i_class] == 0)
1089  continue;//no votes per class of interest in this cluster
1090  if (n_vw[i_class] == 0)
1091  continue;//there were no objects of this class in the training dataset
1092  if (n_vot[i_cluster] == 0)
1093  continue;//this cluster has never been used
1094  if (n_ftr[i_class] == 0)
1095  continue;//there were no objects of this class in the training dataset
1096  float part_1 = static_cast<float> (n_vw[i_class]);
1097  float part_2 = static_cast<float> (n_vot[i_cluster]);
1098  float part_3 = static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1099  float part_4 = 0.0f;
1100 
1101  if (!n_vot_ON_)
1102  part_2 = 1.0f;
1103 
1104  for (unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1105  if (n_ftr[j_class] != 0)
1106  part_4 += static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1107 
1108  statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1109  }
1110  }//next cluster
1111 }
1112 
1113 //////////////////////////////////////////////////////////////////////////////////////////////
1114 template <int FeatureSize, typename PointT, typename NormalT> void
1116  typename pcl::PointCloud<PointT>::ConstPtr in_point_cloud,
1117  typename pcl::PointCloud<NormalT>::ConstPtr in_normal_cloud,
1118  typename pcl::PointCloud<PointT>::Ptr out_sampled_point_cloud,
1119  typename pcl::PointCloud<NormalT>::Ptr out_sampled_normal_cloud)
1120 {
1121  //create voxel grid
1123  grid.setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1124  grid.setSaveLeafLayout (true);
1125  grid.setInputCloud (in_point_cloud);
1126 
1127  pcl::PointCloud<PointT> temp_cloud;
1128  grid.filter (temp_cloud);
1129 
1130  //extract indices of points from source cloud which are closest to grid points
1131  const float max_value = std::numeric_limits<float>::max ();
1132 
1133  const auto num_source_points = in_point_cloud->size ();
1134  const auto num_sample_points = temp_cloud.size ();
1135 
1136  std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1137  std::vector<int> sampling_indices (num_sample_points, -1);
1138 
1139  for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1140  {
1141  int index = grid.getCentroidIndex ((*in_point_cloud)[i_point]);
1142  if (index == -1)
1143  continue;
1144 
1145  PointT pt_1 = (*in_point_cloud)[i_point];
1146  PointT pt_2 = temp_cloud[index];
1147 
1148  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);
1149  if (distance < dist_to_grid_center[index])
1150  {
1151  dist_to_grid_center[index] = distance;
1152  sampling_indices[index] = static_cast<int> (i_point);
1153  }
1154  }
1155 
1156  //extract source points
1157  pcl::PointIndices::Ptr final_inliers_indices (new pcl::PointIndices ());
1158  pcl::ExtractIndices<PointT> extract_points;
1159  pcl::ExtractIndices<NormalT> extract_normals;
1160 
1161  final_inliers_indices->indices.reserve (num_sample_points);
1162  for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1163  {
1164  if (sampling_indices[i_point] != -1)
1165  final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1166  }
1167 
1168  extract_points.setInputCloud (in_point_cloud);
1169  extract_points.setIndices (final_inliers_indices);
1170  extract_points.filter (*out_sampled_point_cloud);
1171 
1172  extract_normals.setInputCloud (in_normal_cloud);
1173  extract_normals.setIndices (final_inliers_indices);
1174  extract_normals.filter (*out_sampled_normal_cloud);
1175 }
1176 
1177 //////////////////////////////////////////////////////////////////////////////////////////////
1178 template <int FeatureSize, typename PointT, typename NormalT> void
1180  typename pcl::PointCloud<PointT>::Ptr in_cloud,
1181  Eigen::Vector3f shift_point)
1182 {
1183  for (auto point_it = in_cloud->points.begin (); point_it != in_cloud->points.end (); point_it++)
1184  {
1185  point_it->x -= shift_point.x ();
1186  point_it->y -= shift_point.y ();
1187  point_it->z -= shift_point.z ();
1188  }
1189 }
1190 
1191 //////////////////////////////////////////////////////////////////////////////////////////////
1192 template <int FeatureSize, typename PointT, typename NormalT> Eigen::Matrix3f
1194 {
1195  Eigen::Matrix3f result;
1196  Eigen::Matrix3f rotation_matrix_X;
1197  Eigen::Matrix3f rotation_matrix_Z;
1198 
1199  float A = 0.0f;
1200  float B = 0.0f;
1201  float sign = -1.0f;
1202 
1203  float denom_X = static_cast<float> (std::sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1204  A = in_normal.normal_y / denom_X;
1205  B = sign * in_normal.normal_z / denom_X;
1206  rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1207  0.0f, A, -B,
1208  0.0f, B, A;
1209 
1210  float denom_Z = static_cast<float> (std::sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1211  A = in_normal.normal_y / denom_Z;
1212  B = sign * in_normal.normal_x / denom_Z;
1213  rotation_matrix_Z << A, -B, 0.0f,
1214  B, A, 0.0f,
1215  0.0f, 0.0f, 1.0f;
1216 
1217  result = rotation_matrix_X * rotation_matrix_Z;
1218 
1219  return (result);
1220 }
1221 
1222 //////////////////////////////////////////////////////////////////////////////////////////////
1223 template <int FeatureSize, typename PointT, typename NormalT> void
1224 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform)
1225 {
1226  io_vec = in_transform * io_vec;
1227 }
1228 
1229 //////////////////////////////////////////////////////////////////////////////////////////////
1230 template <int FeatureSize, typename PointT, typename NormalT> void
1232  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud,
1233  typename pcl::PointCloud<NormalT>::Ptr normal_cloud,
1234  typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud)
1235 {
1237 // tree->setInputCloud (point_cloud);
1238 
1239  feature_estimator_->setInputCloud (sampled_point_cloud->makeShared ());
1240 // feature_estimator_->setSearchSurface (point_cloud->makeShared ());
1241  feature_estimator_->setSearchMethod (tree);
1242 
1243 // typename pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm =
1244 // dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1245 // feat_est_norm->setInputNormals (normal_cloud);
1246 
1248  dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1249  feat_est_norm->setInputNormals (normal_cloud);
1250 
1251  feature_estimator_->compute (*feature_cloud);
1252 }
1253 
1254 //////////////////////////////////////////////////////////////////////////////////////////////
1255 template <int FeatureSize, typename PointT, typename NormalT> double
1257  const Eigen::MatrixXf& points_to_cluster,
1258  int number_of_clusters,
1259  Eigen::MatrixXi& io_labels,
1260  TermCriteria criteria,
1261  int attempts,
1262  int flags,
1263  Eigen::MatrixXf& cluster_centers)
1264 {
1265  const int spp_trials = 3;
1266  std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1267  int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1268 
1269  attempts = std::max (attempts, 1);
1270  srand (static_cast<unsigned int> (time (nullptr)));
1271 
1272  Eigen::MatrixXi labels (number_of_points, 1);
1273 
1274  if (flags & USE_INITIAL_LABELS)
1275  labels = io_labels;
1276  else
1277  labels.setZero ();
1278 
1279  Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1280  Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1281  std::vector<int> counters (number_of_clusters);
1282  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1283  Eigen::Vector2f* box = &boxes[0];
1284 
1285  double best_compactness = std::numeric_limits<double>::max ();
1286  double compactness = 0.0;
1287 
1288  if (criteria.type_ & TermCriteria::EPS)
1289  criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f);
1290  else
1291  criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1292 
1293  criteria.epsilon_ *= criteria.epsilon_;
1294 
1295  if (criteria.type_ & TermCriteria::COUNT)
1296  criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100);
1297  else
1298  criteria.max_count_ = 100;
1299 
1300  if (number_of_clusters == 1)
1301  {
1302  attempts = 1;
1303  criteria.max_count_ = 2;
1304  }
1305 
1306  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1307  box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1308 
1309  for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1310  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1311  {
1312  float v = points_to_cluster (i_point, i_dim);
1313  box[i_dim] (0) = std::min (box[i_dim] (0), v);
1314  box[i_dim] (1) = std::max (box[i_dim] (1), v);
1315  }
1316 
1317  for (int i_attempt = 0; i_attempt < attempts; i_attempt++)
1318  {
1319  float max_center_shift = std::numeric_limits<float>::max ();
1320  for (int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++)
1321  {
1322  Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1323  temp = centers;
1324  centers = old_centers;
1325  old_centers = temp;
1326 
1327  if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1328  {
1329  if (flags & PP_CENTERS)
1330  generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1331  else
1332  {
1333  for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1334  {
1335  Eigen::VectorXf center (feature_dimension);
1336  generateRandomCenter (boxes, center);
1337  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1338  centers (i_cl_center, i_dim) = center (i_dim);
1339  }//generate center for next cluster
1340  }//end if-else random or PP centers
1341  }
1342  else
1343  {
1344  centers.setZero ();
1345  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1346  counters[i_cluster] = 0;
1347  for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1348  {
1349  int i_label = labels (i_point, 0);
1350  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1351  centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1352  counters[i_label]++;
1353  }
1354  if (iter > 0)
1355  max_center_shift = 0.0f;
1356  for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1357  {
1358  if (counters[i_cl_center] != 0)
1359  {
1360  float scale = 1.0f / static_cast<float> (counters[i_cl_center]);
1361  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1362  centers (i_cl_center, i_dim) *= scale;
1363  }
1364  else
1365  {
1366  Eigen::VectorXf center (feature_dimension);
1367  generateRandomCenter (boxes, center);
1368  for(int i_dim = 0; i_dim < feature_dimension; i_dim++)
1369  centers (i_cl_center, i_dim) = center (i_dim);
1370  }
1371 
1372  if (iter > 0)
1373  {
1374  float dist = 0.0f;
1375  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1376  {
1377  float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1378  dist += diff * diff;
1379  }
1380  max_center_shift = std::max (max_center_shift, dist);
1381  }
1382  }
1383  }
1384  compactness = 0.0f;
1385  for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1386  {
1387  Eigen::VectorXf sample (feature_dimension);
1388  sample = points_to_cluster.row (i_point);
1389 
1390  int k_best = 0;
1391  float min_dist = std::numeric_limits<float>::max ();
1392 
1393  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1394  {
1395  Eigen::VectorXf center (feature_dimension);
1396  center = centers.row (i_cluster);
1397  float dist = computeDistance (sample, center);
1398  if (min_dist > dist)
1399  {
1400  min_dist = dist;
1401  k_best = i_cluster;
1402  }
1403  }
1404  compactness += min_dist;
1405  labels (i_point, 0) = k_best;
1406  }
1407  }//next iteration
1408 
1409  if (compactness < best_compactness)
1410  {
1411  best_compactness = compactness;
1412  cluster_centers = centers;
1413  io_labels = labels;
1414  }
1415  }//next attempt
1416 
1417  return (best_compactness);
1418 }
1419 
1420 //////////////////////////////////////////////////////////////////////////////////////////////
1421 template <int FeatureSize, typename PointT, typename NormalT> void
1423  const Eigen::MatrixXf& data,
1424  Eigen::MatrixXf& out_centers,
1425  int number_of_clusters,
1426  int trials)
1427 {
1428  std::size_t dimension = data.cols ();
1429  auto number_of_points = static_cast<unsigned int> (data.rows ());
1430  std::vector<int> centers_vec (number_of_clusters);
1431  int* centers = &centers_vec[0];
1432  std::vector<double> dist (number_of_points);
1433  std::vector<double> tdist (number_of_points);
1434  std::vector<double> tdist2 (number_of_points);
1435  double sum0 = 0.0;
1436 
1437  unsigned int random_unsigned = rand ();
1438  centers[0] = random_unsigned % number_of_points;
1439 
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 (centers[0]);
1446  dist[i_point] = computeDistance (first, second);
1447  sum0 += dist[i_point];
1448  }
1449 
1450  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1451  {
1452  double best_sum = std::numeric_limits<double>::max ();
1453  int best_center = -1;
1454  for (int i_trials = 0; i_trials < trials; i_trials++)
1455  {
1456  unsigned int random_integer = rand () - 1;
1457  double random_double = static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1458  double p = random_double * sum0;
1459 
1460  unsigned int i_point;
1461  for (i_point = 0; i_point < number_of_points - 1; i_point++)
1462  if ( (p -= dist[i_point]) <= 0.0)
1463  break;
1464 
1465  int ci = i_point;
1466 
1467  double s = 0.0;
1468  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1469  {
1470  Eigen::VectorXf first (dimension);
1471  Eigen::VectorXf second (dimension);
1472  first = data.row (i_point);
1473  second = data.row (ci);
1474  tdist2[i_point] = std::min (static_cast<double> (computeDistance (first, second)), dist[i_point]);
1475  s += tdist2[i_point];
1476  }
1477 
1478  if (s <= best_sum)
1479  {
1480  best_sum = s;
1481  best_center = ci;
1482  std::swap (tdist, tdist2);
1483  }
1484  }
1485 
1486  centers[i_cluster] = best_center;
1487  sum0 = best_sum;
1488  std::swap (dist, tdist);
1489  }
1490 
1491  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1492  for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1493  out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1494 }
1495 
1496 //////////////////////////////////////////////////////////////////////////////////////////////
1497 template <int FeatureSize, typename PointT, typename NormalT> void
1498 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateRandomCenter (const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >& boxes,
1499  Eigen::VectorXf& center)
1500 {
1501  std::size_t dimension = boxes.size ();
1502  float margin = 1.0f / static_cast<float> (dimension);
1503 
1504  for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1505  {
1506  unsigned int random_integer = rand () - 1;
1507  float random_float = static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1508  center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1509  }
1510 }
1511 
1512 //////////////////////////////////////////////////////////////////////////////////////////////
1513 template <int FeatureSize, typename PointT, typename NormalT> float
1515 {
1516  std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1517  float distance = 0.0f;
1518  for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1519  {
1520  float diff = vec_1 (i_dim) - vec_2 (i_dim);
1521  distance += diff * diff;
1522  }
1523 
1524  return (distance);
1525 }
1526 
1527 #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
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:898
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.