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