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