Point Cloud Library (PCL) 1.12.0
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
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,
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 {
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;
171
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
191 {
194 best_peak_ind = i;
195 }
196 ++peak_counter;
197 }
198
199 if( peak_counter == 0 )
200 break;// no peaks
201
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 ();
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)
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
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;
476 clusters_[i_cluster].resize (size_of_current_cluster, 0);
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
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);
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
757{
759
760 if (in_cloud->points.empty ())
761 return (out_votes);
762
766 if (sampled_point_cloud->points.empty ())
767 return (out_votes);
768
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 {
800 }
801 }
803 }//next keypoint
804
805 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
806 {
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
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];
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
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
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);
877
878 n_key_points += static_cast<int> (sampled_point_cloud->size ());
879
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//////////////////////////////////////////////////////////////////////////////////////////////
911template <int FeatureSize, typename PointT, typename NormalT> bool
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++)
922
923 labels.resize (histograms.size(), 1);
924 computeKMeansClustering (
926 number_of_clusters_,
927 labels,
928 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),//1000
929 5,
930 PP_CENTERS,
932
933 return (true);
934}
935
936//////////////////////////////////////////////////////////////////////////////////////////////
937template <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;
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;
970 }
971 max_distance = static_cast<float> (sqrt (max_distance));
972 unsigned int i_class = training_classes_[i_object];
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++)
982 sig /= (static_cast<float> (number_of_objects_in_class) * 10.0f);
983 sigmas[i_class] = sig;
984 }
985}
986
987//////////////////////////////////////////////////////////////////////////////////////////////
988template <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 ();
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 ());
1046 {
1047 unsigned int i_index = clusters[i_cluster][i_visual_word];
1048 int i_class = training_classes_[locations[i_index].model_num_];
1050 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1051 {
1052 std::vector<float> calculated_sigmas;
1053 calculateSigmas (calculated_sigmas);
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;
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 ());
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//////////////////////////////////////////////////////////////////////////////////////////////
1118template <int FeatureSize, typename PointT, typename NormalT> void
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
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
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);
1175
1176 extract_normals.setInputCloud (in_normal_cloud);
1179}
1180
1181//////////////////////////////////////////////////////////////////////////////////////////////
1182template <int FeatureSize, typename PointT, typename NormalT> void
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//////////////////////////////////////////////////////////////////////////////////////////////
1196template <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
1222
1223 return (result);
1224}
1225
1226//////////////////////////////////////////////////////////////////////////////////////////////
1227template <int FeatureSize, typename PointT, typename NormalT> void
1232
1233//////////////////////////////////////////////////////////////////////////////////////////////
1234template <int FeatureSize, typename PointT, typename NormalT> void
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
1253 feat_est_norm->setInputNormals (normal_cloud);
1254
1255 feature_estimator_->compute (*feature_cloud);
1256}
1257
1258//////////////////////////////////////////////////////////////////////////////////////////////
1259template <int FeatureSize, typename PointT, typename NormalT> double
1261 const Eigen::MatrixXf& points_to_cluster,
1263 Eigen::MatrixXi& io_labels,
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);
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;
1329 old_centers = temp;
1330
1331 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1332 {
1333 if (flags & PP_CENTERS)
1335 else
1336 {
1338 {
1339 Eigen::VectorXf center (feature_dimension);
1340 generateRandomCenter (boxes, center);
1341 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1343 }//generate center for next cluster
1344 }//end if-else random or PP centers
1345 }
1346 else
1347 {
1348 centers.setZero ();
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++)
1356 counters[i_label]++;
1357 }
1358 if (iter > 0)
1359 max_center_shift = 0.0f;
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++)
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 {
1382 dist += diff * diff;
1383 }
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
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 }
1409 labels (i_point, 0) = k_best;
1410 }
1411 }//next iteration
1412
1414 {
1417 io_labels = labels;
1418 }
1419 }//next attempt
1420
1421 return (best_compactness);
1422}
1423
1424//////////////////////////////////////////////////////////////////////////////////////////////
1425template <int FeatureSize, typename PointT, typename NormalT> void
1427 const Eigen::MatrixXf& data,
1428 Eigen::MatrixXf& out_centers,
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 ();
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
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
1491 sum0 = best_sum;
1492 std::swap (dist, tdist);
1493 }
1494
1496 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1498}
1499
1500//////////////////////////////////////////////////////////////////////////////////////////////
1501template <int FeatureSize, typename PointT, typename NormalT> void
1502pcl::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//////////////////////////////////////////////////////////////////////////////////////////////
1517template <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_
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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.
This struct is used for storing peak.
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.