Point Cloud Library (PCL)  1.7.1
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> 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 (int 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 (int 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 (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 (int 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> 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
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_
float getSamplingSize()
Returns the sampling size used for cloud simplification.
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...
void generateRandomCenter(const std::vector< Eigen::Vector2f > &boxes, Eigen::VectorXf &center)
Generates random center for cluster.
std::vector< typename pcl::PointCloud< PointT >::Ptr > getTrainingClouds()
This method simply returns the clouds that were set as the training clouds.
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.
iterator end()
Definition: point_cloud.h:435
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the coresponding cloud of normals for every training point cloud.
bool loadModelFromfile(std::string &file_name)
This method loads the trained model from file.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:80
This structure stores the information about the keypoint.
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
VectorType::iterator iterator
Definition: point_cloud.h:432
A point structure representing an interest point with Euclidean xyz coordinates, and an interest valu...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
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).
bool extractDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations)
Extracts the descriptors from the input clouds.
void setSamplingSize(float sampling_size)
Changes the sampling size used for cloud simplification.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
void filter(PointCloud &output)
unsigned int descriptors_dimension_
Stores descriptors dimension.
void setFeatureEstimator(boost::shared_ptr< pcl::Feature< PointT, pcl::Histogram< FeatureSize > > > feature)
Changes the feature estimator.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void setTrainingClouds(const std::vector< typename pcl::PointCloud< PointT >::Ptr > &training_clouds)
Allows to set clouds for training the ISM model.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition: voxel_grid.h:268
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...
unsigned int getNumberOfClusters()
Returns the number of clusters used for descriptor clustering.
bool trainISM(ISMModelPtr &trained_model)
This method performs training and forms a visual vocabulary.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:178
boost::shared_ptr< pcl::Feature< PointT, pcl::Histogram< FeatureSize > > > getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
A point structure representing an N-D histogram.
boost::shared_ptr< pcl::features::ISMModel > ISMModelPtr
unsigned int number_of_visual_words_
Stores the number of visual words.
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm...
void simplifyCloud(typename pcl::PointCloud< PointT >::ConstPtr in_point_cloud, typename pcl::PointCloud< NormalT >::ConstPtr in_normal_cloud, typename pcl::PointCloud< PointT >::Ptr out_sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr out_sampled_normal_cloud)
Simplifies the cloud using voxel grid principles.
void 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 calculateSigmas(std::vector< float > &sigmas)
This method calculates the value of sigma used for calculating the learned weights for every single c...
ISMModel()
Simple constructor that initializes the structure.
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
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++...
void setTrainingClasses(const std::vector< unsigned int > &training_classes)
Allows to set the class labels for the corresponding training clouds.
void addVote(pcl::InterestPoint &in_vote, const PointT &vote_origin, int in_class)
This method simply adds another vote to the list.
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
void reset()
this method resets all variables and frees memory.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:67
double density
Density of this peak.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition: voxel_grid.h:222
A point structure representing normal coordinates and the surface curvature estimate.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:131
ISMModel & operator=(const ISMModel &other)
Operator overloading for deep copy.
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:580
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< float > learned_weights_
Stores learned weights.
float computeDistance(Eigen::VectorXf &vec_1, Eigen::VectorXf &vec_2)
Computes the square distance beetween two vectors.
std::vector< float > getSigmaDists()
Returns the array of sigma values.
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.
boost::shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
iterator begin()
Definition: point_cloud.h:434
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
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...
virtual ~ISMModel()
Destructor that frees memory.
int getCentroidIndex(const PointT &p)
Returns the index in the resulting downsampled cloud of the specified point.
Definition: voxel_grid.h:307
Eigen::MatrixXf directions_to_center_
Stores the directions to objects center for each visual word.
std::vector< std::vector< float > > statistical_weights_
Stores statistical weights.
std::vector< unsigned int > getTrainingClasses()
Returns the array of classes that indicates which class the corresponding training cloud belongs...
unsigned int number_of_clusters_
Stores the number of clusters.
This struct is used for storing peak.
struct PCL_EXPORTS pcl::ism::ImplicitShapeModelEstimation::TC TermCriteria
This structure is used for determining the end of the k-means clustering process. ...
Feature represents the base feature class.
Definition: feature.h:105
std::vector< float > sigmas_
Stores the sigma value for each class.
unsigned int number_of_classes_
Stores the number of classes.
A point structure representing Euclidean xyz coordinates, and the RGB color.
int class_id
Determines which class this peak belongs.
double getDensityAtPoint(const PointT &point, double sigma_dist)
Returns the density at the specified point.
ImplicitShapeModelEstimation()
Simple constructor that initializes everything.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:73
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
unsigned int getNumberOfVotes()
This method simply returns the number of votes.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void setNVotState(bool state)
Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
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.
A point structure representing Euclidean xyz coordinates.
virtual ~ISMVoteList()
virtual descriptor.
std::vector< unsigned int > classes_
Stores the class label for every direction.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
The assignment of this structure is to store the statistical/learned weights and other information of...
void validateTree()
this method is simply setting up the search tree.
size_t size() const
Definition: point_cloud.h:440
bool clusterDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, Eigen::MatrixXi &labels, Eigen::MatrixXf &clusters_centers)
This method performs descriptor clustering.
ISMVoteList()
Empty constructor with member variables initialization.
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
ExtractIndices extracts a set of indices from a point cloud.
void applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
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...
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62