Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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//////////////////////////////////////////////////////////////////////////////////////////////
51template <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//////////////////////////////////////////////////////////////////////////////////////////////
63template <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//////////////////////////////////////////////////////////////////////////////////////////////
75template <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//////////////////////////////////////////////////////////////////////////////////////////////
87template <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//////////////////////////////////////////////////////////////////////////////////////////////
126template <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 constexpr 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//////////////////////////////////////////////////////////////////////////////////////////////
226template <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//////////////////////////////////////////////////////////////////////////////////////////////
241template <typename PointT> Eigen::Vector3f
242pcl::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//////////////////////////////////////////////////////////////////////////////////////////////
268template <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//////////////////////////////////////////////////////////////////////////////////////////////
293template <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//////////////////////////////////////////////////////////////////////////////////////////////
360bool
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//////////////////////////////////////////////////////////////////////////////////////////////
415bool
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//////////////////////////////////////////////////////////////////////////////////////////////
486void
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//////////////////////////////////////////////////////////////////////////////////////////////
548template <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//////////////////////////////////////////////////////////////////////////////////////////////
562template <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//////////////////////////////////////////////////////////////////////////////////////////////
573template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
578
579//////////////////////////////////////////////////////////////////////////////////////////////
580template <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//////////////////////////////////////////////////////////////////////////////////////////////
590template <int FeatureSize, typename PointT, typename NormalT> std::vector<unsigned int>
595
596//////////////////////////////////////////////////////////////////////////////////////////////
597template <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//////////////////////////////////////////////////////////////////////////////////////////////
606template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
611
612//////////////////////////////////////////////////////////////////////////////////////////////
613template <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//////////////////////////////////////////////////////////////////////////////////////////////
623template <int FeatureSize, typename PointT, typename NormalT> float
628
629//////////////////////////////////////////////////////////////////////////////////////////////
630template <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//////////////////////////////////////////////////////////////////////////////////////////////
638template <int FeatureSize, typename PointT, typename NormalT> typename pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::FeaturePtr
643
644//////////////////////////////////////////////////////////////////////////////////////////////
645template <int FeatureSize, typename PointT, typename NormalT> void
650
651//////////////////////////////////////////////////////////////////////////////////////////////
652template <int FeatureSize, typename PointT, typename NormalT> unsigned int
657
658//////////////////////////////////////////////////////////////////////////////////////////////
659template <int FeatureSize, typename PointT, typename NormalT> void
661{
662 if (num_of_clusters > 0)
663 number_of_clusters_ = num_of_clusters;
664}
665
666//////////////////////////////////////////////////////////////////////////////////////////////
667template <int FeatureSize, typename PointT, typename NormalT> std::vector<float>
672
673//////////////////////////////////////////////////////////////////////////////////////////////
674template <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//////////////////////////////////////////////////////////////////////////////////////////////
683template <int FeatureSize, typename PointT, typename NormalT> bool
688
689//////////////////////////////////////////////////////////////////////////////////////////////
690template <int FeatureSize, typename PointT, typename NormalT> void
695
696//////////////////////////////////////////////////////////////////////////////////////////////
697template <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//////////////////////////////////////////////////////////////////////////////////////////////
751template <int FeatureSize, typename PointT, typename NormalT> typename pcl::features::ISMVoteList<PointT>::Ptr
753 ISMModelPtr model,
754 typename pcl::PointCloud<PointT>::Ptr in_cloud,
755 typename pcl::PointCloud<Normal>::Ptr in_normals,
756 int in_class_of_interest)
757{
759
760 if (in_cloud->points.empty ())
761 return (out_votes);
762
763 typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
764 typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
765 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
766 if (sampled_point_cloud->points.empty ())
767 return (out_votes);
768
770 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
771
772 //find nearest cluster
773 const auto n_key_points = static_cast<unsigned int> (sampled_point_cloud->size ());
774 std::vector<int> min_dist_inds (n_key_points, -1);
775 for (unsigned int i_point = 0; i_point < n_key_points; i_point++)
776 {
777 Eigen::VectorXf curr_descriptor (FeatureSize);
778 for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
779 curr_descriptor (i_dim) = (*feature_cloud)[i_point].histogram[i_dim];
780
781 float descriptor_sum = curr_descriptor.sum ();
782 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
783 continue;
784
785 unsigned int min_dist_idx = 0;
786 Eigen::VectorXf clusters_center (FeatureSize);
787 for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
788 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
789
790 float best_dist = computeDistance (curr_descriptor, clusters_center);
791 for (unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
792 {
793 for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
794 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
795 float curr_dist = computeDistance (clusters_center, curr_descriptor);
796 if (curr_dist < best_dist)
797 {
798 min_dist_idx = i_clust_cent;
799 best_dist = curr_dist;
800 }
801 }
802 min_dist_inds[i_point] = min_dist_idx;
803 }//next keypoint
804
805 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
806 {
807 int min_dist_idx = min_dist_inds[i_point];
808 if (min_dist_idx == -1)
809 continue;
810
811 const auto n_words = static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
812 //compute coord system transform
813 Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
814 for (unsigned int i_word = 0; i_word < n_words; i_word++)
815 {
816 unsigned int index = model->clusters_[min_dist_idx][i_word];
817 unsigned int i_class = model->classes_[index];
818 if (static_cast<int> (i_class) != in_class_of_interest)
819 continue;//skip this class
820
821 //rotate dir to center as needed
822 Eigen::Vector3f direction (
823 model->directions_to_center_(index, 0),
824 model->directions_to_center_(index, 1),
825 model->directions_to_center_(index, 2));
826 applyTransform (direction, transform.transpose ());
827
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//////////////////////////////////////////////////////////////////////////////////////////////
846template <int FeatureSize, typename PointT, typename NormalT> bool
848 std::vector< pcl::Histogram<FeatureSize> >& histograms,
849 std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
850{
851 histograms.clear ();
852 locations.clear ();
853
854 if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ == nullptr)
855 return (false);
856
857 for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
858 {
859 //compute the center of the training object
860 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
861 const auto num_of_points = training_clouds_[i_cloud]->size ();
862 for (auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
863 models_center += point_j->getVector3fMap ();
864 models_center /= static_cast<float> (num_of_points);
865
866 //downsample the cloud
867 typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
868 typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
869 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
870 if (sampled_point_cloud->points.empty ())
871 continue;
872
873 shiftCloud (training_clouds_[i_cloud], models_center);
874 shiftCloud (sampled_point_cloud, models_center);
875
877 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
878
879 int point_index = 0;
880 for (auto point_i = sampled_point_cloud->points.cbegin (); point_i != sampled_point_cloud->points.cend (); point_i++, point_index++)
881 {
882 float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
883 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
884 continue;
885
886 histograms.insert ( histograms.end (), feature_cloud->begin () + point_index, feature_cloud->begin () + point_index + 1 );
887
888 int dist = static_cast<int> (std::distance (sampled_point_cloud->points.cbegin (), point_i));
889 Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
890 Eigen::Vector3f zero;
891 zero (0) = 0.0;
892 zero (1) = 0.0;
893 zero (2) = 0.0;
894 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
895 applyTransform (new_dir, new_basis);
896
897 PointT point (new_dir[0], new_dir[1], new_dir[2]);
898 LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
899 locations.insert(locations.end (), info);
900 }
901 }//next training cloud
902
903 return (true);
904}
905
906//////////////////////////////////////////////////////////////////////////////////////////////
907template <int FeatureSize, typename PointT, typename NormalT> bool
909 std::vector< pcl::Histogram<FeatureSize> >& histograms,
910 Eigen::MatrixXi& labels,
911 Eigen::MatrixXf& clusters_centers)
912{
913 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
914
915 for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
916 for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
917 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
918
919 labels.resize (histograms.size(), 1);
920 computeKMeansClustering (
921 points_to_cluster,
922 number_of_clusters_,
923 labels,
924 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),//1000
925 5,
926 PP_CENTERS,
927 clusters_centers);
928
929 return (true);
930}
931
932//////////////////////////////////////////////////////////////////////////////////////////////
933template <int FeatureSize, typename PointT, typename NormalT> void
935{
936 if (!training_sigmas_.empty ())
937 {
938 sigmas.resize (training_sigmas_.size (), 0.0f);
939 for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
940 sigmas[i_sigma] = training_sigmas_[i_sigma];
941 return;
942 }
943
944 sigmas.clear ();
945 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
946 sigmas.resize (number_of_classes, 0.0f);
947
948 std::vector<float> vec;
949 std::vector<std::vector<float> > objects_sigmas;
950 objects_sigmas.resize (number_of_classes, vec);
951
952 auto number_of_objects = static_cast<unsigned int> (training_clouds_.size ());
953 for (unsigned int i_object = 0; i_object < number_of_objects; i_object++)
954 {
955 float max_distance = 0.0f;
956 const auto number_of_points = training_clouds_[i_object]->size ();
957 for (unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
958 for (unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
959 {
960 float curr_distance = 0.0f;
961 curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
962 curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
963 curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
964 if (curr_distance > max_distance)
965 max_distance = curr_distance;
966 }
967 max_distance = static_cast<float> (std::sqrt (max_distance));
968 unsigned int i_class = training_classes_[i_object];
969 objects_sigmas[i_class].push_back (max_distance);
970 }
971
972 for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
973 {
974 float sig = 0.0f;
975 auto number_of_objects_in_class = static_cast<unsigned int> (objects_sigmas[i_class].size ());
976 for (unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
977 sig += objects_sigmas[i_class][i_object];
978 sig /= (static_cast<float> (number_of_objects_in_class) * 10.0f);
979 sigmas[i_class] = sig;
980 }
981}
982
983//////////////////////////////////////////////////////////////////////////////////////////////
984template <int FeatureSize, typename PointT, typename NormalT> void
986 const std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
987 const Eigen::MatrixXi &labels,
988 std::vector<float>& sigmas,
989 std::vector<std::vector<unsigned int> >& clusters,
990 std::vector<std::vector<float> >& statistical_weights,
991 std::vector<float>& learned_weights)
992{
993 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
994 //Temporary variable
995 std::vector<float> vec;
996 vec.resize (number_of_clusters_, 0.0f);
997 statistical_weights.clear ();
998 learned_weights.clear ();
999 statistical_weights.resize (number_of_classes, vec);
1000 learned_weights.resize (locations.size (), 0.0f);
1001
1002 //Temporary variable
1003 std::vector<int> vect;
1004 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
1005
1006 //Number of features from which c_i was learned
1007 std::vector<int> n_ftr;
1008
1009 //Total number of votes from visual word v_j
1010 std::vector<int> n_vot;
1011
1012 //Number of visual words that vote for class c_i
1013 std::vector<int> n_vw;
1014
1015 //Number of votes for class c_i from v_j
1016 std::vector<std::vector<int> > n_vot_2;
1017
1018 n_vot_2.resize (number_of_clusters_, vect);
1019 n_vot.resize (number_of_clusters_, 0);
1020 n_ftr.resize (number_of_classes, 0);
1021 for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
1022 {
1023 int i_class = training_classes_[locations[i_location].model_num_];
1024 int i_cluster = labels (i_location);
1025 n_vot_2[i_cluster][i_class] += 1;
1026 n_vot[i_cluster] += 1;
1027 n_ftr[i_class] += 1;
1028 }
1029
1030 n_vw.resize (number_of_classes, 0);
1031 for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1032 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1033 if (n_vot_2[i_cluster][i_class] > 0)
1034 n_vw[i_class] += 1;
1035
1036 //computing learned weights
1037 learned_weights.resize (locations.size (), 0.0);
1038 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1039 {
1040 auto number_of_words_in_cluster = static_cast<unsigned int> (clusters[i_cluster].size ());
1041 for (unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1042 {
1043 unsigned int i_index = clusters[i_cluster][i_visual_word];
1044 int i_class = training_classes_[locations[i_index].model_num_];
1045 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1046 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1047 {
1048 std::vector<float> calculated_sigmas;
1049 calculateSigmas (calculated_sigmas);
1050 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1051 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1052 continue;
1053 }
1054 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1055 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1056 applyTransform (direction, transform);
1057 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1058
1059 //collect gaussian weighted distances
1060 std::vector<float> gauss_dists;
1061 for (unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1062 {
1063 unsigned int j_index = clusters[i_cluster][j_visual_word];
1064 int j_class = training_classes_[locations[j_index].model_num_];
1065 if (i_class != j_class)
1066 continue;
1067 //predict center
1068 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1069 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1070 applyTransform (direction_2, transform_2);
1071 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1072 float residual = (predicted_center - actual_center).norm ();
1073 float value = -residual * residual / square_sigma_dist;
1074 gauss_dists.push_back (static_cast<float> (std::exp (value)));
1075 }//next word
1076 //find median gaussian weighted distance
1077 std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1078 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1079 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1080 }//next word
1081 }//next cluster
1082
1083 //computing statistical weights
1084 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1085 {
1086 for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1087 {
1088 if (n_vot_2[i_cluster][i_class] == 0)
1089 continue;//no votes per class of interest in this cluster
1090 if (n_vw[i_class] == 0)
1091 continue;//there were no objects of this class in the training dataset
1092 if (n_vot[i_cluster] == 0)
1093 continue;//this cluster has never been used
1094 if (n_ftr[i_class] == 0)
1095 continue;//there were no objects of this class in the training dataset
1096 float part_1 = static_cast<float> (n_vw[i_class]);
1097 float part_2 = static_cast<float> (n_vot[i_cluster]);
1098 float part_3 = static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1099 float part_4 = 0.0f;
1100
1101 if (!n_vot_ON_)
1102 part_2 = 1.0f;
1103
1104 for (unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1105 if (n_ftr[j_class] != 0)
1106 part_4 += static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1107
1108 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1109 }
1110 }//next cluster
1111}
1112
1113//////////////////////////////////////////////////////////////////////////////////////////////
1114template <int FeatureSize, typename PointT, typename NormalT> void
1116 typename pcl::PointCloud<PointT>::ConstPtr in_point_cloud,
1117 typename pcl::PointCloud<NormalT>::ConstPtr in_normal_cloud,
1118 typename pcl::PointCloud<PointT>::Ptr out_sampled_point_cloud,
1119 typename pcl::PointCloud<NormalT>::Ptr out_sampled_normal_cloud)
1120{
1121 //create voxel grid
1123 grid.setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1124 grid.setSaveLeafLayout (true);
1125 grid.setInputCloud (in_point_cloud);
1126
1127 pcl::PointCloud<PointT> temp_cloud;
1128 grid.filter (temp_cloud);
1129
1130 //extract indices of points from source cloud which are closest to grid points
1131 constexpr float max_value = std::numeric_limits<float>::max ();
1132
1133 const auto num_source_points = in_point_cloud->size ();
1134 const auto num_sample_points = temp_cloud.size ();
1135
1136 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1137 std::vector<int> sampling_indices (num_sample_points, -1);
1138
1139 for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1140 {
1141 int index = grid.getCentroidIndex ((*in_point_cloud)[i_point]);
1142 if (index == -1)
1143 continue;
1144
1145 PointT pt_1 = (*in_point_cloud)[i_point];
1146 PointT pt_2 = temp_cloud[index];
1147
1148 float distance = (pt_1.x - pt_2.x) * (pt_1.x - pt_2.x) + (pt_1.y - pt_2.y) * (pt_1.y - pt_2.y) + (pt_1.z - pt_2.z) * (pt_1.z - pt_2.z);
1149 if (distance < dist_to_grid_center[index])
1150 {
1151 dist_to_grid_center[index] = distance;
1152 sampling_indices[index] = static_cast<int> (i_point);
1153 }
1154 }
1155
1156 //extract source points
1157 pcl::PointIndices::Ptr final_inliers_indices (new pcl::PointIndices ());
1158 pcl::ExtractIndices<PointT> extract_points;
1159 pcl::ExtractIndices<NormalT> extract_normals;
1160
1161 final_inliers_indices->indices.reserve (num_sample_points);
1162 for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1163 {
1164 if (sampling_indices[i_point] != -1)
1165 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1166 }
1167
1168 extract_points.setInputCloud (in_point_cloud);
1169 extract_points.setIndices (final_inliers_indices);
1170 extract_points.filter (*out_sampled_point_cloud);
1171
1172 extract_normals.setInputCloud (in_normal_cloud);
1173 extract_normals.setIndices (final_inliers_indices);
1174 extract_normals.filter (*out_sampled_normal_cloud);
1175}
1176
1177//////////////////////////////////////////////////////////////////////////////////////////////
1178template <int FeatureSize, typename PointT, typename NormalT> void
1180 typename pcl::PointCloud<PointT>::Ptr in_cloud,
1181 Eigen::Vector3f shift_point)
1182{
1183 for (auto point_it = in_cloud->points.begin (); point_it != in_cloud->points.end (); point_it++)
1184 {
1185 point_it->x -= shift_point.x ();
1186 point_it->y -= shift_point.y ();
1187 point_it->z -= shift_point.z ();
1188 }
1189}
1190
1191//////////////////////////////////////////////////////////////////////////////////////////////
1192template <int FeatureSize, typename PointT, typename NormalT> Eigen::Matrix3f
1194{
1195 Eigen::Matrix3f result;
1196 Eigen::Matrix3f rotation_matrix_X;
1197 Eigen::Matrix3f rotation_matrix_Z;
1198
1199 float A = 0.0f;
1200 float B = 0.0f;
1201 float sign = -1.0f;
1202
1203 float denom_X = static_cast<float> (std::sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1204 A = in_normal.normal_y / denom_X;
1205 B = sign * in_normal.normal_z / denom_X;
1206 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1207 0.0f, A, -B,
1208 0.0f, B, A;
1209
1210 float denom_Z = static_cast<float> (std::sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1211 A = in_normal.normal_y / denom_Z;
1212 B = sign * in_normal.normal_x / denom_Z;
1213 rotation_matrix_Z << A, -B, 0.0f,
1214 B, A, 0.0f,
1215 0.0f, 0.0f, 1.0f;
1216
1217 result = rotation_matrix_X * rotation_matrix_Z;
1218
1219 return (result);
1220}
1221
1222//////////////////////////////////////////////////////////////////////////////////////////////
1223template <int FeatureSize, typename PointT, typename NormalT> void
1224pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform)
1225{
1226 io_vec = in_transform * io_vec;
1227}
1228
1229//////////////////////////////////////////////////////////////////////////////////////////////
1230template <int FeatureSize, typename PointT, typename NormalT> void
1232 typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud,
1233 typename pcl::PointCloud<NormalT>::Ptr normal_cloud,
1234 typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud)
1235{
1237// tree->setInputCloud (point_cloud);
1238
1239 feature_estimator_->setInputCloud (sampled_point_cloud->makeShared ());
1240// feature_estimator_->setSearchSurface (point_cloud->makeShared ());
1241 feature_estimator_->setSearchMethod (tree);
1242
1243// typename pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm =
1244// dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1245// feat_est_norm->setInputNormals (normal_cloud);
1246
1248 dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1249 feat_est_norm->setInputNormals (normal_cloud);
1250
1251 feature_estimator_->compute (*feature_cloud);
1252}
1253
1254//////////////////////////////////////////////////////////////////////////////////////////////
1255template <int FeatureSize, typename PointT, typename NormalT> double
1257 const Eigen::MatrixXf& points_to_cluster,
1258 int number_of_clusters,
1259 Eigen::MatrixXi& io_labels,
1260 TermCriteria criteria,
1261 int attempts,
1262 int flags,
1263 Eigen::MatrixXf& cluster_centers)
1264{
1265 constexpr int spp_trials = 3;
1266 std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1267 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1268
1269 attempts = std::max (attempts, 1);
1270 srand (static_cast<unsigned int> (time (nullptr)));
1271
1272 Eigen::MatrixXi labels (number_of_points, 1);
1273
1274 if (flags & USE_INITIAL_LABELS)
1275 labels = io_labels;
1276 else
1277 labels.setZero ();
1278
1279 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1280 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1281 std::vector<int> counters (number_of_clusters);
1282 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1283 Eigen::Vector2f* box = boxes.data();
1284
1285 double best_compactness = std::numeric_limits<double>::max ();
1286 double compactness = 0.0;
1287
1288 if (criteria.type_ & TermCriteria::EPS)
1289 criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f);
1290 else
1291 criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1292
1293 criteria.epsilon_ *= criteria.epsilon_;
1294
1295 if (criteria.type_ & TermCriteria::COUNT)
1296 criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100);
1297 else
1298 criteria.max_count_ = 100;
1299
1300 if (number_of_clusters == 1)
1301 {
1302 attempts = 1;
1303 criteria.max_count_ = 2;
1304 }
1305
1306 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1307 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1308
1309 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1310 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1311 {
1312 float v = points_to_cluster (i_point, i_dim);
1313 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1314 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1315 }
1316
1317 for (int i_attempt = 0; i_attempt < attempts; i_attempt++)
1318 {
1319 float max_center_shift = std::numeric_limits<float>::max ();
1320 for (int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++)
1321 {
1322 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1323 temp = centers;
1324 centers = old_centers;
1325 old_centers = temp;
1326
1327 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1328 {
1329 if (flags & PP_CENTERS)
1330 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1331 else
1332 {
1333 for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1334 {
1335 Eigen::VectorXf center (feature_dimension);
1336 generateRandomCenter (boxes, center);
1337 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1338 centers (i_cl_center, i_dim) = center (i_dim);
1339 }//generate center for next cluster
1340 }//end if-else random or PP centers
1341 }
1342 else
1343 {
1344 centers.setZero ();
1345 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1346 counters[i_cluster] = 0;
1347 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1348 {
1349 int i_label = labels (i_point, 0);
1350 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1351 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1352 counters[i_label]++;
1353 }
1354 if (iter > 0)
1355 max_center_shift = 0.0f;
1356 for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1357 {
1358 if (counters[i_cl_center] != 0)
1359 {
1360 float scale = 1.0f / static_cast<float> (counters[i_cl_center]);
1361 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1362 centers (i_cl_center, i_dim) *= scale;
1363 }
1364 else
1365 {
1366 Eigen::VectorXf center (feature_dimension);
1367 generateRandomCenter (boxes, center);
1368 for(int i_dim = 0; i_dim < feature_dimension; i_dim++)
1369 centers (i_cl_center, i_dim) = center (i_dim);
1370 }
1371
1372 if (iter > 0)
1373 {
1374 float dist = 0.0f;
1375 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1376 {
1377 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1378 dist += diff * diff;
1379 }
1380 max_center_shift = std::max (max_center_shift, dist);
1381 }
1382 }
1383 }
1384 compactness = 0.0f;
1385 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1386 {
1387 Eigen::VectorXf sample (feature_dimension);
1388 sample = points_to_cluster.row (i_point);
1389
1390 int k_best = 0;
1391 float min_dist = std::numeric_limits<float>::max ();
1392
1393 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1394 {
1395 Eigen::VectorXf center (feature_dimension);
1396 center = centers.row (i_cluster);
1397 float dist = computeDistance (sample, center);
1398 if (min_dist > dist)
1399 {
1400 min_dist = dist;
1401 k_best = i_cluster;
1402 }
1403 }
1404 compactness += min_dist;
1405 labels (i_point, 0) = k_best;
1406 }
1407 }//next iteration
1408
1409 if (compactness < best_compactness)
1410 {
1411 best_compactness = compactness;
1412 cluster_centers = centers;
1413 io_labels = labels;
1414 }
1415 }//next attempt
1416
1417 return (best_compactness);
1418}
1419
1420//////////////////////////////////////////////////////////////////////////////////////////////
1421template <int FeatureSize, typename PointT, typename NormalT> void
1423 const Eigen::MatrixXf& data,
1424 Eigen::MatrixXf& out_centers,
1425 int number_of_clusters,
1426 int trials)
1427{
1428 std::size_t dimension = data.cols ();
1429 auto number_of_points = static_cast<unsigned int> (data.rows ());
1430 std::vector<int> centers_vec (number_of_clusters);
1431 int* centers = centers_vec.data();
1432 std::vector<double> dist (number_of_points);
1433 std::vector<double> tdist (number_of_points);
1434 std::vector<double> tdist2 (number_of_points);
1435 double sum0 = 0.0;
1436
1437 unsigned int random_unsigned = rand ();
1438 centers[0] = random_unsigned % number_of_points;
1439
1440 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1441 {
1442 Eigen::VectorXf first (dimension);
1443 Eigen::VectorXf second (dimension);
1444 first = data.row (i_point);
1445 second = data.row (centers[0]);
1446 dist[i_point] = computeDistance (first, second);
1447 sum0 += dist[i_point];
1448 }
1449
1450 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1451 {
1452 double best_sum = std::numeric_limits<double>::max ();
1453 int best_center = -1;
1454 for (int i_trials = 0; i_trials < trials; i_trials++)
1455 {
1456 unsigned int random_integer = rand () - 1;
1457 double random_double = static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1458 double p = random_double * sum0;
1459
1460 unsigned int i_point;
1461 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1462 if ( (p -= dist[i_point]) <= 0.0)
1463 break;
1464
1465 int ci = i_point;
1466
1467 double s = 0.0;
1468 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1469 {
1470 Eigen::VectorXf first (dimension);
1471 Eigen::VectorXf second (dimension);
1472 first = data.row (i_point);
1473 second = data.row (ci);
1474 tdist2[i_point] = std::min (static_cast<double> (computeDistance (first, second)), dist[i_point]);
1475 s += tdist2[i_point];
1476 }
1477
1478 if (s <= best_sum)
1479 {
1480 best_sum = s;
1481 best_center = ci;
1482 std::swap (tdist, tdist2);
1483 }
1484 }
1485
1486 centers[i_cluster] = best_center;
1487 sum0 = best_sum;
1488 std::swap (dist, tdist);
1489 }
1490
1491 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1492 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1493 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1494}
1495
1496//////////////////////////////////////////////////////////////////////////////////////////////
1497template <int FeatureSize, typename PointT, typename NormalT> void
1498pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateRandomCenter (const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >& boxes,
1499 Eigen::VectorXf& center)
1500{
1501 std::size_t dimension = boxes.size ();
1502 float margin = 1.0f / static_cast<float> (dimension);
1503
1504 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1505 {
1506 unsigned int random_integer = rand () - 1;
1507 float random_float = static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1508 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1509 }
1510}
1511
1512//////////////////////////////////////////////////////////////////////////////////////////////
1513template <int FeatureSize, typename PointT, typename NormalT> float
1515{
1516 std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1517 float distance = 0.0f;
1518 for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1519 {
1520 float diff = vec_1 (i_dim) - vec_2 (i_dim);
1521 distance += diff * diff;
1522 }
1523
1524 return (distance);
1525}
1526
1527#endif //#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_
ExtractIndices extracts a set of indices from a point cloud.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition feature.h:339
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition filter.h:121
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition pcl_base.hpp:72
PointCloud represents the base class in PCL for storing collections of 3D points.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t size() const
shared_ptr< PointCloud< PointT > > Ptr
iterator begin() noexcept
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
shared_ptr< const PointCloud< PointT > > ConstPtr
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed,...
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
Definition voxel_grid.h:177
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
Definition voxel_grid.h:317
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:221
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition voxel_grid.h:278
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm.
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
shared_ptr< ISMVoteList< PointT > > Ptr
void addVote(pcl::InterestPoint &in_vote, const PointT &vote_origin, int in_class)
This method simply adds another vote to the list.
void findStrongestPeaks(std::vector< ISMPeak, Eigen::aligned_allocator< ISMPeak > > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma)
This method finds the strongest peaks (points were density has most higher values).
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud(typename pcl::PointCloud< PointT >::Ptr cloud=0)
Returns the colored cloud that consists of votes for center (blue points) and initial point cloud (if...
unsigned int getNumberOfVotes()
This method simply returns the number of votes.
void validateTree()
this method is simply setting up the search tree.
double getDensityAtPoint(const PointT &point, double sigma_dist)
Returns the density at the specified point.
virtual ~ISMVoteList()
virtual descriptor.
ISMVoteList()
Empty constructor with member variables initialization.
bool trainISM(ISMModelPtr &trained_model)
This method performs training and forms a visual vocabulary.
void applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
Eigen::Matrix3f alignYCoordWithNormal(const NormalT &in_normal)
This method simply computes the rotation matrix, so that the given normal would match the Y axis afte...
std::vector< typename pcl::PointCloud< PointT >::Ptr > getTrainingClouds()
This method simply returns the clouds that were set as the training clouds.
void generateRandomCenter(const std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > &boxes, Eigen::VectorXf &center)
Generates random center for cluster.
void setFeatureEstimator(FeaturePtr feature)
Changes the feature estimator.
void setTrainingClouds(const std::vector< typename pcl::PointCloud< PointT >::Ptr > &training_clouds)
Allows to set clouds for training the ISM model.
void setNVotState(bool state)
Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
float computeDistance(Eigen::VectorXf &vec_1, Eigen::VectorXf &vec_2)
Computes the square distance between two vectors.
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
void shiftCloud(typename pcl::PointCloud< PointT >::Ptr in_cloud, Eigen::Vector3f shift_point)
This method simply shifts the clouds points relative to the passed point.
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
void setTrainingNormals(const std::vector< typename pcl::PointCloud< NormalT >::Ptr > &training_normals)
Allows to set normals for the training clouds that were passed through setTrainingClouds method.
void generateCentersPP(const Eigen::MatrixXf &data, Eigen::MatrixXf &out_centers, int number_of_clusters, int trials)
Generates centers for clusters as described in Arthur, David and Sergei Vassilvitski (2007) k-means++...
ImplicitShapeModelEstimation()
Simple constructor that initializes everything.
double computeKMeansClustering(const Eigen::MatrixXf &points_to_cluster, int number_of_clusters, Eigen::MatrixXi &io_labels, TermCriteria criteria, int attempts, int flags, Eigen::MatrixXf &cluster_centers)
Performs K-means clustering.
std::vector< unsigned int > getTrainingClasses()
Returns the array of classes that indicates which class the corresponding training cloud belongs.
void calculateWeights(const std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations, const Eigen::MatrixXi &labels, std::vector< float > &sigmas, std::vector< std::vector< unsigned int > > &clusters, std::vector< std::vector< float > > &statistical_weights, std::vector< float > &learned_weights)
This function forms a visual vocabulary and evaluates weights described in [Knopp et al....
void estimateFeatures(typename pcl::PointCloud< PointT >::Ptr sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr normal_cloud, typename pcl::PointCloud< pcl::Histogram< FeatureSize > >::Ptr feature_cloud)
This method estimates features for the given point cloud.
bool extractDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations)
Extracts the descriptors from the input clouds.
FeaturePtr getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
unsigned int getNumberOfClusters()
Returns the number of clusters used for descriptor clustering.
void calculateSigmas(std::vector< float > &sigmas)
This method calculates the value of sigma used for calculating the learned weights for every single c...
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
bool clusterDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, Eigen::MatrixXi &labels, Eigen::MatrixXf &clusters_centers)
This method performs descriptor clustering.
std::vector< float > getSigmaDists()
Returns the array of sigma values.
void setSamplingSize(float sampling_size)
Changes the sampling size used for cloud simplification.
pcl::features::ISMVoteList< PointT >::Ptr findObjects(ISMModelPtr model, typename pcl::PointCloud< PointT >::Ptr in_cloud, typename pcl::PointCloud< Normal >::Ptr in_normals, int in_class_of_interest)
This function is searching for the class of interest in a given cloud and returns the list of votes.
void simplifyCloud(typename pcl::PointCloud< PointT >::ConstPtr in_point_cloud, typename pcl::PointCloud< NormalT >::ConstPtr in_normal_cloud, typename pcl::PointCloud< PointT >::Ptr out_sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr out_sampled_normal_cloud)
Simplifies the cloud using voxel grid principles.
void setSigmaDists(const std::vector< float > &training_sigmas)
This method allows to set the value of sigma used for calculating the learned weights for every singl...
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the corresponding cloud of normals for every training point cloud.
void setTrainingClasses(const std::vector< unsigned int > &training_classes)
Allows to set the class labels for the corresponding training clouds.
float getSamplingSize()
Returns the sampling size used for cloud simplification.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition search.h:81
@ B
Definition norms.h:54
Defines functions, macros and traits for allocating and using memory.
A point structure representing an N-D histogram.
This struct is used for storing peak.
double density
Density of this peak.
int class_id
Determines which class this peak belongs.
A point structure representing an interest point with Euclidean xyz coordinates, and an interest valu...
A point structure representing normal coordinates and the surface curvature estimate.
shared_ptr< ::pcl::PointIndices > Ptr
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.
The assignment of this structure is to store the statistical/learned weights and other information of...
unsigned int number_of_clusters_
Stores the number of clusters.
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
ISMModel()
Simple constructor that initializes the structure.
bool loadModelFromfile(std::string &file_name)
This method loads the trained model from file.
ISMModel & operator=(const ISMModel &other)
Operator overloading for deep copy.
virtual ~ISMModel()
Destructor that frees memory.
unsigned int descriptors_dimension_
Stores descriptors dimension.
std::vector< float > sigmas_
Stores the sigma value for each class.
std::vector< float > learned_weights_
Stores learned weights.
void reset()
this method resets all variables and frees memory.
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
std::vector< unsigned int > classes_
Stores the class label for every direction.
Eigen::MatrixXf directions_to_center_
Stores the directions to objects center for each visual word.
unsigned int number_of_classes_
Stores the number of classes.
std::vector< std::vector< float > > statistical_weights_
Stores statistical weights.
unsigned int number_of_visual_words_
Stores the number of visual words.
This structure stores the information about the keypoint.
This structure is used for determining the end of the k-means clustering process.
int type_
Flag that determines when the k-means clustering must be stopped.
float epsilon_
Defines the accuracy for k-means clustering.
int max_count_
Defines maximum number of iterations for k-means clustering.