Point Cloud Library (PCL)  1.8.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 
319  this->number_of_classes_ = copy.number_of_classes_;
320  this->number_of_visual_words_ = copy.number_of_visual_words_;
321  this->number_of_clusters_ = copy.number_of_clusters_;
322  this->descriptors_dimension_ = copy.descriptors_dimension_;
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);
438  statistical_weights_.resize (number_of_classes_, vec);
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
444  learned_weights_.resize (number_of_visual_words_, 0.0f);
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;
498  number_of_visual_words_ = 0;
499  number_of_clusters_ = 0;
500  descriptors_dimension_ = 0;
501 }
502 
503 //////////////////////////////////////////////////////////////////////////////////////////////
506 {
507  if (this != &other)
508  {
509  this->reset ();
510 
511  this->number_of_classes_ = other.number_of_classes_;
512  this->number_of_visual_words_ = other.number_of_visual_words_;
513  this->number_of_clusters_ = other.number_of_clusters_;
514  this->descriptors_dimension_ = other.descriptors_dimension_;
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> boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > >
641 {
642  return (feature_estimator_);
643 }
644 
645 //////////////////////////////////////////////////////////////////////////////////////////////
646 template <int FeatureSize, typename PointT, typename NormalT> void
648  boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > > feature)
649 {
650  feature_estimator_ = feature;
651 }
652 
653 //////////////////////////////////////////////////////////////////////////////////////////////
654 template <int FeatureSize, typename PointT, typename NormalT> unsigned int
656 {
657  return (number_of_clusters_);
658 }
659 
660 //////////////////////////////////////////////////////////////////////////////////////////////
661 template <int FeatureSize, typename PointT, typename NormalT> void
663 {
664  if (num_of_clusters > 0)
665  number_of_clusters_ = num_of_clusters;
666 }
667 
668 //////////////////////////////////////////////////////////////////////////////////////////////
669 template <int FeatureSize, typename PointT, typename NormalT> std::vector<float>
671 {
672  return (training_sigmas_);
673 }
674 
675 //////////////////////////////////////////////////////////////////////////////////////////////
676 template <int FeatureSize, typename PointT, typename NormalT> void
678 {
679  training_sigmas_.clear ();
680  std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
681  training_sigmas_.swap (sigmas);
682 }
683 
684 //////////////////////////////////////////////////////////////////////////////////////////////
685 template <int FeatureSize, typename PointT, typename NormalT> bool
687 {
688  return (n_vot_ON_);
689 }
690 
691 //////////////////////////////////////////////////////////////////////////////////////////////
692 template <int FeatureSize, typename PointT, typename NormalT> void
694 {
695  n_vot_ON_ = state;
696 }
697 
698 //////////////////////////////////////////////////////////////////////////////////////////////
699 template <int FeatureSize, typename PointT, typename NormalT> bool
701 {
702  bool success = true;
703 
704  if (trained_model == 0)
705  return (false);
706  trained_model->reset ();
707 
708  std::vector<pcl::Histogram<FeatureSize> > histograms;
709  std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
710  success = extractDescriptors (histograms, locations);
711  if (!success)
712  return (false);
713 
714  Eigen::MatrixXi labels;
715  success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
716  if (!success)
717  return (false);
718 
719  std::vector<unsigned int> vec;
720  trained_model->clusters_.resize (number_of_clusters_, vec);
721  for (size_t i_label = 0; i_label < locations.size (); i_label++)
722  trained_model->clusters_[labels (i_label)].push_back (i_label);
723 
724  calculateSigmas (trained_model->sigmas_);
725 
726  calculateWeights(
727  locations,
728  labels,
729  trained_model->sigmas_,
730  trained_model->clusters_,
731  trained_model->statistical_weights_,
732  trained_model->learned_weights_);
733 
734  trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
735  trained_model->number_of_visual_words_ = static_cast<unsigned int> (histograms.size ());
736  trained_model->number_of_clusters_ = number_of_clusters_;
737  trained_model->descriptors_dimension_ = FeatureSize;
738 
739  trained_model->directions_to_center_.resize (locations.size (), 3);
740  trained_model->classes_.resize (locations.size ());
741  for (size_t i_dir = 0; i_dir < locations.size (); i_dir++)
742  {
743  trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
744  trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
745  trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
746  trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
747  }
748 
749  return (true);
750 }
751 
752 //////////////////////////////////////////////////////////////////////////////////////////////
753 template <int FeatureSize, typename PointT, typename NormalT> boost::shared_ptr<pcl::features::ISMVoteList<PointT> >
755  ISMModelPtr model,
756  typename pcl::PointCloud<PointT>::Ptr in_cloud,
757  typename pcl::PointCloud<Normal>::Ptr in_normals,
758  int in_class_of_interest)
759 {
760  boost::shared_ptr<pcl::features::ISMVoteList<PointT> > out_votes (new pcl::features::ISMVoteList<PointT> ());
761 
762  if (in_cloud->points.size () == 0)
763  return (out_votes);
764 
765  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
766  typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
767  simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
768  if (sampled_point_cloud->points.size () == 0)
769  return (out_votes);
770 
772  estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
773 
774  //find nearest cluster
775  const unsigned int n_key_points = static_cast<unsigned int> (sampled_point_cloud->size ());
776  std::vector<int> min_dist_inds (n_key_points, -1);
777  for (unsigned int i_point = 0; i_point < n_key_points; i_point++)
778  {
779  Eigen::VectorXf curr_descriptor (FeatureSize);
780  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
781  curr_descriptor (i_dim) = feature_cloud->points[i_point].histogram[i_dim];
782 
783  float descriptor_sum = curr_descriptor.sum ();
784  if (descriptor_sum < std::numeric_limits<float>::epsilon ())
785  continue;
786 
787  unsigned int min_dist_idx = 0;
788  Eigen::VectorXf clusters_center (FeatureSize);
789  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
790  clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
791 
792  float best_dist = computeDistance (curr_descriptor, clusters_center);
793  for (unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
794  {
795  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
796  clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
797  float curr_dist = computeDistance (clusters_center, curr_descriptor);
798  if (curr_dist < best_dist)
799  {
800  min_dist_idx = i_clust_cent;
801  best_dist = curr_dist;
802  }
803  }
804  min_dist_inds[i_point] = min_dist_idx;
805  }//next keypoint
806 
807  for (size_t i_point = 0; i_point < n_key_points; i_point++)
808  {
809  int min_dist_idx = min_dist_inds[i_point];
810  if (min_dist_idx == -1)
811  continue;
812 
813  const unsigned int n_words = static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
814  //compute coord system transform
815  Eigen::Matrix3f transform = alignYCoordWithNormal (sampled_normal_cloud->points[i_point]);
816  for (unsigned int i_word = 0; i_word < n_words; i_word++)
817  {
818  unsigned int index = model->clusters_[min_dist_idx][i_word];
819  unsigned int i_class = model->classes_[index];
820  if (static_cast<int> (i_class) != in_class_of_interest)
821  continue;//skip this class
822 
823  //rotate dir to center as needed
824  Eigen::Vector3f direction (
825  model->directions_to_center_(index, 0),
826  model->directions_to_center_(index, 1),
827  model->directions_to_center_(index, 2));
828  applyTransform (direction, transform.transpose ());
829 
830  pcl::InterestPoint vote;
831  Eigen::Vector3f vote_pos = sampled_point_cloud->points[i_point].getVector3fMap () + direction;
832  vote.x = vote_pos[0];
833  vote.y = vote_pos[1];
834  vote.z = vote_pos[2];
835  float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
836  float learned_weight = model->learned_weights_[index];
837  float power = statistical_weight * learned_weight;
838  vote.strength = power;
839  if (vote.strength > std::numeric_limits<float>::epsilon ())
840  out_votes->addVote (vote, sampled_point_cloud->points[i_point], i_class);
841  }
842  }//next point
843 
844  return (out_votes);
845 }
846 
847 //////////////////////////////////////////////////////////////////////////////////////////////
848 template <int FeatureSize, typename PointT, typename NormalT> bool
850  std::vector< pcl::Histogram<FeatureSize> >& histograms,
851  std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
852 {
853  histograms.clear ();
854  locations.clear ();
855 
856  int n_key_points = 0;
857 
858  if (training_clouds_.size () == 0 || training_classes_.size () == 0 || feature_estimator_ == 0)
859  return (false);
860 
861  for (size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
862  {
863  //compute the center of the training object
864  Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
865  const size_t num_of_points = training_clouds_[i_cloud]->points.size ();
866  typename pcl::PointCloud<PointT>::iterator point_j;
867  for (point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
868  models_center += point_j->getVector3fMap ();
869  models_center /= static_cast<float> (num_of_points);
870 
871  //downsample the cloud
872  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
873  typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
874  simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
875  if (sampled_point_cloud->points.size () == 0)
876  continue;
877 
878  shiftCloud (training_clouds_[i_cloud], models_center);
879  shiftCloud (sampled_point_cloud, models_center);
880 
881  n_key_points += static_cast<int> (sampled_point_cloud->size ());
882 
884  estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
885 
886  int point_index = 0;
887  typename pcl::PointCloud<PointT>::iterator point_i;
888  for (point_i = sampled_point_cloud->points.begin (); point_i != sampled_point_cloud->points.end (); point_i++, point_index++)
889  {
890  float descriptor_sum = Eigen::VectorXf::Map (feature_cloud->points[point_index].histogram, FeatureSize).sum ();
891  if (descriptor_sum < std::numeric_limits<float>::epsilon ())
892  continue;
893 
894  histograms.insert ( histograms.end (), feature_cloud->begin () + point_index, feature_cloud->begin () + point_index + 1 );
895 
896  int dist = static_cast<int> (std::distance (sampled_point_cloud->points.begin (), point_i));
897  Eigen::Matrix3f new_basis = alignYCoordWithNormal (sampled_normal_cloud->points[dist]);
898  Eigen::Vector3f zero;
899  zero (0) = 0.0;
900  zero (1) = 0.0;
901  zero (2) = 0.0;
902  Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
903  applyTransform (new_dir, new_basis);
904 
905  PointT point (new_dir[0], new_dir[1], new_dir[2]);
906  LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, sampled_normal_cloud->points[dist]);
907  locations.insert(locations.end (), info);
908  }
909  }//next training cloud
910 
911  return (true);
912 }
913 
914 //////////////////////////////////////////////////////////////////////////////////////////////
915 template <int FeatureSize, typename PointT, typename NormalT> bool
917  std::vector< pcl::Histogram<FeatureSize> >& histograms,
918  Eigen::MatrixXi& labels,
919  Eigen::MatrixXf& clusters_centers)
920 {
921  Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
922 
923  for (unsigned int i_feature = 0; i_feature < histograms.size (); i_feature++)
924  for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
925  points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
926 
927  labels.resize (histograms.size(), 1);
928  computeKMeansClustering (
929  points_to_cluster,
930  number_of_clusters_,
931  labels,
932  TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),//1000
933  5,
934  PP_CENTERS,
935  clusters_centers);
936 
937  return (true);
938 }
939 
940 //////////////////////////////////////////////////////////////////////////////////////////////
941 template <int FeatureSize, typename PointT, typename NormalT> void
943 {
944  if (training_sigmas_.size () != 0)
945  {
946  sigmas.resize (training_sigmas_.size (), 0.0f);
947  for (unsigned int i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
948  sigmas[i_sigma] = training_sigmas_[i_sigma];
949  return;
950  }
951 
952  sigmas.clear ();
953  unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
954  sigmas.resize (number_of_classes, 0.0f);
955 
956  std::vector<float> vec;
957  std::vector<std::vector<float> > objects_sigmas;
958  objects_sigmas.resize (number_of_classes, vec);
959 
960  unsigned int number_of_objects = static_cast<unsigned int> (training_clouds_.size ());
961  for (unsigned int i_object = 0; i_object < number_of_objects; i_object++)
962  {
963  float max_distance = 0.0f;
964  unsigned int number_of_points = static_cast<unsigned int> (training_clouds_[i_object]->points.size ());
965  for (unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
966  for (unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
967  {
968  float curr_distance = 0.0f;
969  curr_distance += training_clouds_[i_object]->points[i_point].x * training_clouds_[i_object]->points[j_point].x;
970  curr_distance += training_clouds_[i_object]->points[i_point].y * training_clouds_[i_object]->points[j_point].y;
971  curr_distance += training_clouds_[i_object]->points[i_point].z * training_clouds_[i_object]->points[j_point].z;
972  if (curr_distance > max_distance)
973  max_distance = curr_distance;
974  }
975  max_distance = static_cast<float> (sqrt (max_distance));
976  unsigned int i_class = training_classes_[i_object];
977  objects_sigmas[i_class].push_back (max_distance);
978  }
979 
980  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
981  {
982  float sig = 0.0f;
983  unsigned int number_of_objects_in_class = static_cast<unsigned int> (objects_sigmas[i_class].size ());
984  for (unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
985  sig += objects_sigmas[i_class][i_object];
986  sig /= (static_cast<float> (number_of_objects_in_class) * 10.0f);
987  sigmas[i_class] = sig;
988  }
989 }
990 
991 //////////////////////////////////////////////////////////////////////////////////////////////
992 template <int FeatureSize, typename PointT, typename NormalT> void
994  const std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
995  const Eigen::MatrixXi &labels,
996  std::vector<float>& sigmas,
997  std::vector<std::vector<unsigned int> >& clusters,
998  std::vector<std::vector<float> >& statistical_weights,
999  std::vector<float>& learned_weights)
1000 {
1001  unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
1002  //Temporary variable
1003  std::vector<float> vec;
1004  vec.resize (number_of_clusters_, 0.0f);
1005  statistical_weights.clear ();
1006  learned_weights.clear ();
1007  statistical_weights.resize (number_of_classes, vec);
1008  learned_weights.resize (locations.size (), 0.0f);
1009 
1010  //Temporary variable
1011  std::vector<int> vect;
1012  vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
1013 
1014  //Number of features from which c_i was learned
1015  std::vector<int> n_ftr;
1016 
1017  //Total number of votes from visual word v_j
1018  std::vector<int> n_vot;
1019 
1020  //Number of visual words that vote for class c_i
1021  std::vector<int> n_vw;
1022 
1023  //Number of votes for class c_i from v_j
1024  std::vector<std::vector<int> > n_vot_2;
1025 
1026  n_vot_2.resize (number_of_clusters_, vect);
1027  n_vot.resize (number_of_clusters_, 0);
1028  n_ftr.resize (number_of_classes, 0);
1029  for (size_t i_location = 0; i_location < locations.size (); i_location++)
1030  {
1031  int i_class = training_classes_[locations[i_location].model_num_];
1032  int i_cluster = labels (i_location);
1033  n_vot_2[i_cluster][i_class] += 1;
1034  n_vot[i_cluster] += 1;
1035  n_ftr[i_class] += 1;
1036  }
1037 
1038  n_vw.resize (number_of_classes, 0);
1039  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1040  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1041  if (n_vot_2[i_cluster][i_class] > 0)
1042  n_vw[i_class] += 1;
1043 
1044  //computing learned weights
1045  learned_weights.resize (locations.size (), 0.0);
1046  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1047  {
1048  unsigned int number_of_words_in_cluster = static_cast<unsigned int> (clusters[i_cluster].size ());
1049  for (unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1050  {
1051  unsigned int i_index = clusters[i_cluster][i_visual_word];
1052  int i_class = training_classes_[locations[i_index].model_num_];
1053  float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1054  if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1055  {
1056  std::vector<float> calculated_sigmas;
1057  calculateSigmas (calculated_sigmas);
1058  square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1059  if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1060  continue;
1061  }
1062  Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1063  Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1064  applyTransform (direction, transform);
1065  Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1066 
1067  //collect gaussian weighted distances
1068  std::vector<float> gauss_dists;
1069  for (unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1070  {
1071  unsigned int j_index = clusters[i_cluster][j_visual_word];
1072  int j_class = training_classes_[locations[j_index].model_num_];
1073  if (i_class != j_class)
1074  continue;
1075  //predict center
1076  Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1077  Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1078  applyTransform (direction_2, transform_2);
1079  Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1080  float residual = (predicted_center - actual_center).norm ();
1081  float value = -residual * residual / square_sigma_dist;
1082  gauss_dists.push_back (static_cast<float> (exp (value)));
1083  }//next word
1084  //find median gaussian weighted distance
1085  size_t mid_elem = (gauss_dists.size () - 1) / 2;
1086  std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1087  learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1088  }//next word
1089  }//next cluster
1090 
1091  //computing statistical weights
1092  for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1093  {
1094  for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1095  {
1096  if (n_vot_2[i_cluster][i_class] == 0)
1097  continue;//no votes per class of interest in this cluster
1098  if (n_vw[i_class] == 0)
1099  continue;//there were no objects of this class in the training dataset
1100  if (n_vot[i_cluster] == 0)
1101  continue;//this cluster has never been used
1102  if (n_ftr[i_class] == 0)
1103  continue;//there were no objects of this class in the training dataset
1104  float part_1 = static_cast<float> (n_vw[i_class]);
1105  float part_2 = static_cast<float> (n_vot[i_cluster]);
1106  float part_3 = static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1107  float part_4 = 0.0f;
1108 
1109  if (!n_vot_ON_)
1110  part_2 = 1.0f;
1111 
1112  for (unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1113  if (n_ftr[j_class] != 0)
1114  part_4 += static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1115 
1116  statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1117  }
1118  }//next cluster
1119 }
1120 
1121 //////////////////////////////////////////////////////////////////////////////////////////////
1122 template <int FeatureSize, typename PointT, typename NormalT> void
1124  typename pcl::PointCloud<PointT>::ConstPtr in_point_cloud,
1125  typename pcl::PointCloud<NormalT>::ConstPtr in_normal_cloud,
1126  typename pcl::PointCloud<PointT>::Ptr out_sampled_point_cloud,
1127  typename pcl::PointCloud<NormalT>::Ptr out_sampled_normal_cloud)
1128 {
1129  //create voxel grid
1131  grid.setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1132  grid.setSaveLeafLayout (true);
1133  grid.setInputCloud (in_point_cloud);
1134 
1135  pcl::PointCloud<PointT> temp_cloud;
1136  grid.filter (temp_cloud);
1137 
1138  //extract indices of points from source cloud which are closest to grid points
1139  const float max_value = std::numeric_limits<float>::max ();
1140 
1141  const size_t num_source_points = in_point_cloud->points.size ();
1142  const size_t num_sample_points = temp_cloud.points.size ();
1143 
1144  std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1145  std::vector<int> sampling_indices (num_sample_points, -1);
1146 
1147  for (size_t i_point = 0; i_point < num_source_points; i_point++)
1148  {
1149  int index = grid.getCentroidIndex (in_point_cloud->points[i_point]);
1150  if (index == -1)
1151  continue;
1152 
1153  PointT pt_1 = in_point_cloud->points[i_point];
1154  PointT pt_2 = temp_cloud.points[index];
1155 
1156  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);
1157  if (distance < dist_to_grid_center[index])
1158  {
1159  dist_to_grid_center[index] = distance;
1160  sampling_indices[index] = static_cast<int> (i_point);
1161  }
1162  }
1163 
1164  //extract source points
1165  pcl::PointIndices::Ptr final_inliers_indices (new pcl::PointIndices ());
1166  pcl::ExtractIndices<PointT> extract_points;
1167  pcl::ExtractIndices<NormalT> extract_normals;
1168 
1169  final_inliers_indices->indices.reserve (num_sample_points);
1170  for (size_t i_point = 0; i_point < num_sample_points; i_point++)
1171  {
1172  if (sampling_indices[i_point] != -1)
1173  final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1174  }
1175 
1176  extract_points.setInputCloud (in_point_cloud);
1177  extract_points.setIndices (final_inliers_indices);
1178  extract_points.filter (*out_sampled_point_cloud);
1179 
1180  extract_normals.setInputCloud (in_normal_cloud);
1181  extract_normals.setIndices (final_inliers_indices);
1182  extract_normals.filter (*out_sampled_normal_cloud);
1183 }
1184 
1185 //////////////////////////////////////////////////////////////////////////////////////////////
1186 template <int FeatureSize, typename PointT, typename NormalT> void
1188  typename pcl::PointCloud<PointT>::Ptr in_cloud,
1189  Eigen::Vector3f shift_point)
1190 {
1191  typename pcl::PointCloud<PointT>::iterator point_it;
1192  for (point_it = in_cloud->points.begin (); point_it != in_cloud->points.end (); point_it++)
1193  {
1194  point_it->x -= shift_point.x ();
1195  point_it->y -= shift_point.y ();
1196  point_it->z -= shift_point.z ();
1197  }
1198 }
1199 
1200 //////////////////////////////////////////////////////////////////////////////////////////////
1201 template <int FeatureSize, typename PointT, typename NormalT> Eigen::Matrix3f
1203 {
1204  Eigen::Matrix3f result;
1205  Eigen::Matrix3f rotation_matrix_X;
1206  Eigen::Matrix3f rotation_matrix_Z;
1207 
1208  float A = 0.0f;
1209  float B = 0.0f;
1210  float sign = -1.0f;
1211 
1212  float denom_X = static_cast<float> (sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1213  A = in_normal.normal_y / denom_X;
1214  B = sign * in_normal.normal_z / denom_X;
1215  rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1216  0.0f, A, -B,
1217  0.0f, B, A;
1218 
1219  float denom_Z = static_cast<float> (sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1220  A = in_normal.normal_y / denom_Z;
1221  B = sign * in_normal.normal_x / denom_Z;
1222  rotation_matrix_Z << A, -B, 0.0f,
1223  B, A, 0.0f,
1224  0.0f, 0.0f, 1.0f;
1225 
1226  result = rotation_matrix_X * rotation_matrix_Z;
1227 
1228  return (result);
1229 }
1230 
1231 //////////////////////////////////////////////////////////////////////////////////////////////
1232 template <int FeatureSize, typename PointT, typename NormalT> void
1233 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform)
1234 {
1235  io_vec = in_transform * io_vec;
1236 }
1237 
1238 //////////////////////////////////////////////////////////////////////////////////////////////
1239 template <int FeatureSize, typename PointT, typename NormalT> void
1241  typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud,
1242  typename pcl::PointCloud<NormalT>::Ptr normal_cloud,
1243  typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud)
1244 {
1245  typename pcl::search::Search<PointT>::Ptr tree = boost::shared_ptr<pcl::search::Search<PointT> > (new pcl::search::KdTree<PointT>);
1246 // tree->setInputCloud (point_cloud);
1247 
1248  feature_estimator_->setInputCloud (sampled_point_cloud->makeShared ());
1249 // feature_estimator_->setSearchSurface (point_cloud->makeShared ());
1250  feature_estimator_->setSearchMethod (tree);
1251 
1252 // typename pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm =
1253 // boost::dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1254 // feat_est_norm->setInputNormals (normal_cloud);
1255 
1257  boost::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1258  feat_est_norm->setInputNormals (normal_cloud);
1259 
1260  feature_estimator_->compute (*feature_cloud);
1261 }
1262 
1263 //////////////////////////////////////////////////////////////////////////////////////////////
1264 template <int FeatureSize, typename PointT, typename NormalT> double
1266  const Eigen::MatrixXf& points_to_cluster,
1267  int number_of_clusters,
1268  Eigen::MatrixXi& io_labels,
1269  TermCriteria criteria,
1270  int attempts,
1271  int flags,
1272  Eigen::MatrixXf& cluster_centers)
1273 {
1274  const int spp_trials = 3;
1275  size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1276  int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1277 
1278  attempts = std::max (attempts, 1);
1279  srand (static_cast<unsigned int> (time (0)));
1280 
1281  Eigen::MatrixXi labels (number_of_points, 1);
1282 
1283  if (flags & USE_INITIAL_LABELS)
1284  labels = io_labels;
1285  else
1286  labels.setZero ();
1287 
1288  Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1289  Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1290  std::vector<int> counters (number_of_clusters);
1291  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1292  Eigen::Vector2f* box = &boxes[0];
1293 
1294  double best_compactness = std::numeric_limits<double>::max ();
1295  double compactness = 0.0;
1296 
1297  if (criteria.type_ & TermCriteria::EPS)
1298  criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f);
1299  else
1300  criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1301 
1302  criteria.epsilon_ *= criteria.epsilon_;
1303 
1304  if (criteria.type_ & TermCriteria::COUNT)
1305  criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100);
1306  else
1307  criteria.max_count_ = 100;
1308 
1309  if (number_of_clusters == 1)
1310  {
1311  attempts = 1;
1312  criteria.max_count_ = 2;
1313  }
1314 
1315  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1316  box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1317 
1318  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1319  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1320  {
1321  float v = points_to_cluster (i_point, i_dim);
1322  box[i_dim] (0) = std::min (box[i_dim] (0), v);
1323  box[i_dim] (1) = std::max (box[i_dim] (1), v);
1324  }
1325 
1326  for (int i_attempt = 0; i_attempt < attempts; i_attempt++)
1327  {
1328  float max_center_shift = std::numeric_limits<float>::max ();
1329  for (int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++)
1330  {
1331  Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1332  temp = centers;
1333  centers = old_centers;
1334  old_centers = temp;
1335 
1336  if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1337  {
1338  if (flags & PP_CENTERS)
1339  generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1340  else
1341  {
1342  for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1343  {
1344  Eigen::VectorXf center (feature_dimension);
1345  generateRandomCenter (boxes, center);
1346  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1347  centers (i_cl_center, i_dim) = center (i_dim);
1348  }//generate center for next cluster
1349  }//end if-else random or PP centers
1350  }
1351  else
1352  {
1353  centers.setZero ();
1354  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1355  counters[i_cluster] = 0;
1356  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1357  {
1358  int i_label = labels (i_point, 0);
1359  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1360  centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1361  counters[i_label]++;
1362  }
1363  if (iter > 0)
1364  max_center_shift = 0.0f;
1365  for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1366  {
1367  if (counters[i_cl_center] != 0)
1368  {
1369  float scale = 1.0f / static_cast<float> (counters[i_cl_center]);
1370  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1371  centers (i_cl_center, i_dim) *= scale;
1372  }
1373  else
1374  {
1375  Eigen::VectorXf center (feature_dimension);
1376  generateRandomCenter (boxes, center);
1377  for(int i_dim = 0; i_dim < feature_dimension; i_dim++)
1378  centers (i_cl_center, i_dim) = center (i_dim);
1379  }
1380 
1381  if (iter > 0)
1382  {
1383  float dist = 0.0f;
1384  for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1385  {
1386  float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1387  dist += diff * diff;
1388  }
1389  max_center_shift = std::max (max_center_shift, dist);
1390  }
1391  }
1392  }
1393  compactness = 0.0f;
1394  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1395  {
1396  Eigen::VectorXf sample (feature_dimension);
1397  sample = points_to_cluster.row (i_point);
1398 
1399  int k_best = 0;
1400  float min_dist = std::numeric_limits<float>::max ();
1401 
1402  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1403  {
1404  Eigen::VectorXf center (feature_dimension);
1405  center = centers.row (i_cluster);
1406  float dist = computeDistance (sample, center);
1407  if (min_dist > dist)
1408  {
1409  min_dist = dist;
1410  k_best = i_cluster;
1411  }
1412  }
1413  compactness += min_dist;
1414  labels (i_point, 0) = k_best;
1415  }
1416  }//next iteration
1417 
1418  if (compactness < best_compactness)
1419  {
1420  best_compactness = compactness;
1421  cluster_centers = centers;
1422  io_labels = labels;
1423  }
1424  }//next attempt
1425 
1426  return (best_compactness);
1427 }
1428 
1429 //////////////////////////////////////////////////////////////////////////////////////////////
1430 template <int FeatureSize, typename PointT, typename NormalT> void
1432  const Eigen::MatrixXf& data,
1433  Eigen::MatrixXf& out_centers,
1434  int number_of_clusters,
1435  int trials)
1436 {
1437  size_t dimension = data.cols ();
1438  unsigned int number_of_points = static_cast<unsigned int> (data.rows ());
1439  std::vector<int> centers_vec (number_of_clusters);
1440  int* centers = &centers_vec[0];
1441  std::vector<double> dist (number_of_points);
1442  std::vector<double> tdist (number_of_points);
1443  std::vector<double> tdist2 (number_of_points);
1444  double sum0 = 0.0;
1445 
1446  unsigned int random_unsigned = rand ();
1447  centers[0] = random_unsigned % number_of_points;
1448 
1449  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1450  {
1451  Eigen::VectorXf first (dimension);
1452  Eigen::VectorXf second (dimension);
1453  first = data.row (i_point);
1454  second = data.row (centers[0]);
1455  dist[i_point] = computeDistance (first, second);
1456  sum0 += dist[i_point];
1457  }
1458 
1459  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1460  {
1461  double best_sum = std::numeric_limits<double>::max ();
1462  int best_center = -1;
1463  for (int i_trials = 0; i_trials < trials; i_trials++)
1464  {
1465  unsigned int random_integer = rand () - 1;
1466  double random_double = static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1467  double p = random_double * sum0;
1468 
1469  unsigned int i_point;
1470  for (i_point = 0; i_point < number_of_points - 1; i_point++)
1471  if ( (p -= dist[i_point]) <= 0.0)
1472  break;
1473 
1474  int ci = i_point;
1475 
1476  double s = 0.0;
1477  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1478  {
1479  Eigen::VectorXf first (dimension);
1480  Eigen::VectorXf second (dimension);
1481  first = data.row (i_point);
1482  second = data.row (ci);
1483  tdist2[i_point] = std::min (static_cast<double> (computeDistance (first, second)), dist[i_point]);
1484  s += tdist2[i_point];
1485  }
1486 
1487  if (s <= best_sum)
1488  {
1489  best_sum = s;
1490  best_center = ci;
1491  std::swap (tdist, tdist2);
1492  }
1493  }
1494 
1495  centers[i_cluster] = best_center;
1496  sum0 = best_sum;
1497  std::swap (dist, tdist);
1498  }
1499 
1500  for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1501  for (unsigned int i_dim = 0; i_dim < dimension; i_dim++)
1502  out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1503 }
1504 
1505 //////////////////////////////////////////////////////////////////////////////////////////////
1506 template <int FeatureSize, typename PointT, typename NormalT> void
1507 pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateRandomCenter (const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >& boxes,
1508  Eigen::VectorXf& center)
1509 {
1510  size_t dimension = boxes.size ();
1511  float margin = 1.0f / static_cast<float> (dimension);
1512 
1513  for (unsigned int i_dim = 0; i_dim < dimension; i_dim++)
1514  {
1515  unsigned int random_integer = rand () - 1;
1516  float random_float = static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1517  center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1518  }
1519 }
1520 
1521 //////////////////////////////////////////////////////////////////////////////////////////////
1522 template <int FeatureSize, typename PointT, typename NormalT> float
1524 {
1525  size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1526  float distance = 0.0f;
1527  for(unsigned int i_dim = 0; i_dim < dimension; i_dim++)
1528  {
1529  float diff = vec_1 (i_dim) - vec_2 (i_dim);
1530  distance += diff * diff;
1531  }
1532 
1533  return (distance);
1534 }
1535 
1536 #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:69
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.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
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...
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. ...
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
iterator begin()
Definition: point_cloud.h:442
std::vector< float > getSigmaDists()
Returns the array of sigma values.
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.
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
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...
ISMModel()
Simple constructor that initializes the structure.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void setFeatureEstimator(boost::shared_ptr< pcl::Feature< PointT, pcl::Histogram< FeatureSize > > > feature)
Changes the feature estimator.
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:132
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:178
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.
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< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
virtual ~ISMModel()
Destructor that frees memory.
A point structure representing an N-D histogram.
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:81
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 beetween two vectors.
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
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...
int getCentroidIndex(const PointT &p)
Returns the index in the resulting downsampled cloud of the specified point.
Definition: voxel_grid.h:319
VectorType::iterator iterator
Definition: point_cloud.h:440
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...
size_t size() const
Definition: point_cloud.h:448
void reset()
this method resets all variables and frees memory.
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 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...
iterator end()
Definition: point_cloud.h:443
void applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
void filter(PointCloud &output)
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the coresponding 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...
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.
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).
boost::shared_ptr< pcl::Feature< PointT, pcl::Histogram< FeatureSize > > > getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
This struct is used for storing peak.
unsigned int number_of_visual_words_
Stores the number of visual words.
std::vector< unsigned int > classes_
Stores the class label for every direction.
Feature represents the base feature class.
Definition: feature.h:105
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
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:344
Definition: norms.h:55
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...
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...
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415