Point Cloud Library (PCL) 1.15.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 classification"
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
54//////////////////////////////////////////////////////////////////////////////////////////////
55template <typename PointT>
57{
58 votes_class_.clear ();
59 votes_origins_.reset ();
60 votes_.reset ();
61 k_ind_.clear ();
62 k_sqr_dist_.clear ();
63 tree_.reset ();
64}
65
66//////////////////////////////////////////////////////////////////////////////////////////////
67template <typename PointT> void
69 pcl::InterestPoint& vote, const PointT &vote_origin, int votes_class)
70{
71 tree_is_valid_ = false;
72 votes_->points.insert (votes_->points.end (), vote);// TODO: adjust height and width
73
74 votes_origins_->points.push_back (vote_origin);
75 votes_class_.push_back (votes_class);
76}
77
78//////////////////////////////////////////////////////////////////////////////////////////////
79template <typename PointT> typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr
81{
82 pcl::PointXYZRGB point;
84 colored_cloud->height = 0;
85 colored_cloud->width = 1;
86
87 if (cloud != nullptr)
88 {
89 colored_cloud->height += cloud->size ();
90 point.r = 255;
91 point.g = 255;
92 point.b = 255;
93 for (const auto& i_point: *cloud)
94 {
95 point.x = i_point.x;
96 point.y = i_point.y;
97 point.z = i_point.z;
98 colored_cloud->points.push_back (point);
99 }
100 }
101
102 point.r = 0;
103 point.g = 0;
104 point.b = 255;
105 for (const auto &i_vote : votes_->points)
106 {
107 point.x = i_vote.x;
108 point.y = i_vote.y;
109 point.z = i_vote.z;
110 colored_cloud->points.push_back (point);
111 }
112 colored_cloud->height += votes_->size ();
113
114 return (colored_cloud);
115}
116
117//////////////////////////////////////////////////////////////////////////////////////////////
118template <typename PointT> void
120 std::vector<pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
121 int in_class_id,
122 double in_non_maxima_radius,
123 double in_sigma)
124{
125 validateTree ();
126
127 const std::size_t n_vote_classes = votes_class_.size ();
128 if (n_vote_classes == 0)
129 return;
130 for (std::size_t i = 0; i < n_vote_classes ; i++)
131 assert ( votes_class_[i] == in_class_id );
132
133 // heuristic: start from NUM_INIT_PTS different locations selected uniformly
134 // on the votes. Intuitively, it is likely to get a good location in dense regions.
135 constexpr int NUM_INIT_PTS = 100;
136 double SIGMA_DIST = in_sigma;// rule of thumb: 10% of the object radius
137 const double FINAL_EPS = SIGMA_DIST / 100;// another heuristic
138
139 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
140 std::vector<double> peak_densities (NUM_INIT_PTS);
141 double max_density = -1.0;
142 for (int i = 0; i < NUM_INIT_PTS; i++)
143 {
144 Eigen::Vector3f old_center;
145 const auto idx = votes_->size() * i / NUM_INIT_PTS;
146 Eigen::Vector3f curr_center = (*votes_)[idx].getVector3fMap();
147
148 do
149 {
150 old_center = curr_center;
151 curr_center = shiftMean (old_center, SIGMA_DIST);
152 } while ((old_center - curr_center).norm () > FINAL_EPS);
153
154 pcl::PointXYZ point;
155 point.x = curr_center (0);
156 point.y = curr_center (1);
157 point.z = curr_center (2);
158 double curr_density = getDensityAtPoint (point, SIGMA_DIST);
159 assert (curr_density >= 0.0);
160
161 peaks[i] = curr_center;
162 peak_densities[i] = curr_density;
163
164 if ( max_density < curr_density )
165 max_density = curr_density;
166 }
167
168 //extract peaks
169 std::vector<bool> peak_flag (NUM_INIT_PTS, true);
170 for (int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
171 {
172 // find best peak with taking into consideration peak flags
173 double best_density = -1.0;
174 Eigen::Vector3f strongest_peak;
175 int best_peak_ind (-1);
176 int peak_counter (0);
177 for (int i = 0; i < NUM_INIT_PTS; i++)
178 {
179 if ( !peak_flag[i] )
180 continue;
181
182 if ( peak_densities[i] > best_density)
183 {
184 best_density = peak_densities[i];
185 strongest_peak = peaks[i];
186 best_peak_ind = i;
187 ++peak_counter;
188 }
189 }
190
191 if( peak_counter == 0 )
192 break;// no peaks
193
194 pcl::ISMPeak peak;
195 peak.x = strongest_peak(0);
196 peak.y = strongest_peak(1);
197 peak.z = strongest_peak(2);
198 peak.density = best_density;
199 peak.class_id = in_class_id;
200 out_peaks.push_back ( peak );
201
202 // mark best peaks and all its neighbors
203 peak_flag[best_peak_ind] = false;
204 for (int i = 0; i < NUM_INIT_PTS; i++)
205 {
206 // compute distance between best peak and all unmarked peaks
207 if ( !peak_flag[i] )
208 continue;
209
210 double dist = (strongest_peak - peaks[i]).norm ();
211 if ( dist < in_non_maxima_radius )
212 peak_flag[i] = false;
213 }
214 }
215}
216
217//////////////////////////////////////////////////////////////////////////////////////////////
218template <typename PointT> void
220{
221 if (!tree_is_valid_)
222 {
223 if (tree_ == nullptr)
224 tree_.reset (new pcl::KdTreeFLANN<pcl::InterestPoint>);
225 tree_->setInputCloud (votes_);
226 k_ind_.resize ( votes_->size (), -1 );
227 k_sqr_dist_.resize ( votes_->size (), 0.0f );
228 tree_is_valid_ = true;
229 }
230}
231
232//////////////////////////////////////////////////////////////////////////////////////////////
233template <typename PointT> Eigen::Vector3f
234pcl::features::ISMVoteList<PointT>::shiftMean (const Eigen::Vector3f& snap_pt, const double in_sigma_dist)
235{
236 validateTree ();
237
238 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
239 double denom = 0.0;
240
242 pt.x = snap_pt[0];
243 pt.y = snap_pt[1];
244 pt.z = snap_pt[2];
245 std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
246
247 for (std::size_t j = 0; j < n_pts; j++)
248 {
249 double kernel = (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
250 Eigen::Vector3f vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[k_ind_[j]].z);
251 wgh_sum += vote_vec * static_cast<float> (kernel);
252 denom += kernel;
253 }
254 assert (denom > 0.0); // at least one point is close. In fact, this case should be handled too
255
256 return (wgh_sum / static_cast<float> (denom));
257}
258
259//////////////////////////////////////////////////////////////////////////////////////////////
260template <typename PointT> double
262 const PointT &point, double sigma_dist)
263{
264 validateTree ();
265
266 const std::size_t n_vote_classes = votes_class_.size ();
267 if (n_vote_classes == 0)
268 return (0.0);
269
270 double sum_vote = 0.0;
271
273 pt.x = point.x;
274 pt.y = point.y;
275 pt.z = point.z;
276 std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
277
278 for (std::size_t j = 0; j < num_of_pts; j++)
279 sum_vote += (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
280
281 return (sum_vote);
282}
283
284//////////////////////////////////////////////////////////////////////////////////////////////
285template <typename PointT> unsigned int
287{
288 return (static_cast<unsigned int> (votes_->size ()));
289}
290
291//////////////////////////////////////////////////////////////////////////////////////////////
293
294//////////////////////////////////////////////////////////////////////////////////////////////
296{
297 reset ();
298
303
304 std::vector<float> vec;
305 vec.resize (this->number_of_clusters_, 0.0f);
306 this->statistical_weights_.resize (this->number_of_classes_, vec);
307 for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
308 for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
309 this->statistical_weights_[i_class][i_cluster] = copy.statistical_weights_[i_class][i_cluster];
310
311 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
312 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
313 this->learned_weights_[i_visual_word] = copy.learned_weights_[i_visual_word];
314
315 this->classes_.resize (this->number_of_visual_words_, 0);
316 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
317 this->classes_[i_visual_word] = copy.classes_[i_visual_word];
318
319 this->sigmas_.resize (this->number_of_classes_, 0.0f);
320 for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
321 this->sigmas_[i_class] = copy.sigmas_[i_class];
322
323 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
324 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
325 for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
326 this->directions_to_center_ (i_visual_word, i_dim) = copy.directions_to_center_ (i_visual_word, i_dim);
327
328 this->clusters_centers_.resize (this->number_of_clusters_, 3);
329 for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
330 for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
331 this->clusters_centers_ (i_cluster, i_dim) = copy.clusters_centers_ (i_cluster, i_dim);
332}
333
334//////////////////////////////////////////////////////////////////////////////////////////////
336{
337 reset ();
338}
339
340//////////////////////////////////////////////////////////////////////////////////////////////
341bool
343{
344 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
345 if (!output_file)
346 {
347 output_file.close ();
348 return (false);
349 }
350
351 output_file << number_of_classes_ << " ";
352 output_file << number_of_visual_words_ << " ";
353 output_file << number_of_clusters_ << " ";
354 output_file << descriptors_dimension_ << " ";
355
356 //write statistical weights
357 for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
358 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
359 output_file << statistical_weights_[i_class][i_cluster] << " ";
360
361 //write learned weights
362 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
363 output_file << learned_weights_[i_visual_word] << " ";
364
365 //write classes
366 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
367 output_file << classes_[i_visual_word] << " ";
368
369 //write sigmas
370 for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
371 output_file << sigmas_[i_class] << " ";
372
373 //write directions to centers
374 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
375 for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
376 output_file << directions_to_center_ (i_visual_word, i_dim) << " ";
377
378 //write clusters centers
379 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
380 for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
381 output_file << clusters_centers_ (i_cluster, i_dim) << " ";
382
383 //write clusters
384 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
385 {
386 output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) << " ";
387 for (const unsigned int &visual_word : clusters_[i_cluster])
388 output_file << visual_word << " ";
389 }
390
391 output_file.close ();
392 return (true);
393}
394
395//////////////////////////////////////////////////////////////////////////////////////////////
396bool
398{
399 reset ();
400 std::ifstream input_file (file_name.c_str ());
401 if (!input_file)
402 {
403 input_file.close ();
404 return (false);
405 }
406
407 char line[256];
408
409 input_file.getline (line, 256, ' ');
410 number_of_classes_ = static_cast<unsigned int> (strtol (line, nullptr, 10));
411 input_file.getline (line, 256, ' '); number_of_visual_words_ = atoi (line);
412 input_file.getline (line, 256, ' '); number_of_clusters_ = atoi (line);
413 input_file.getline (line, 256, ' '); descriptors_dimension_ = atoi (line);
414
415 //read statistical weights
416 std::vector<float> vec;
417 vec.resize (number_of_clusters_, 0.0f);
418 statistical_weights_.resize (number_of_classes_, vec);
419 for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
420 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
421 input_file >> statistical_weights_[i_class][i_cluster];
422
423 //read learned weights
424 learned_weights_.resize (number_of_visual_words_, 0.0f);
425 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
426 input_file >> learned_weights_[i_visual_word];
427
428 //read classes
429 classes_.resize (number_of_visual_words_, 0);
430 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
431 input_file >> classes_[i_visual_word];
432
433 //read sigmas
434 sigmas_.resize (number_of_classes_, 0.0f);
435 for (unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
436 input_file >> sigmas_[i_class];
437
438 //read directions to centers
439 directions_to_center_.resize (number_of_visual_words_, 3);
440 for (unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
441 for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
442 input_file >> directions_to_center_ (i_visual_word, i_dim);
443
444 //read clusters centers
445 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
446 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
447 for (unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
448 input_file >> clusters_centers_ (i_cluster, i_dim);
449
450 //read clusters
451 std::vector<unsigned int> vect;
452 clusters_.resize (number_of_clusters_, vect);
453 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
454 {
455 unsigned int size_of_current_cluster = 0;
456 input_file >> size_of_current_cluster;
457 clusters_[i_cluster].resize (size_of_current_cluster, 0);
458 for (unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
459 input_file >> clusters_[i_cluster][i_visual_word];
460 }
461
462 input_file.close ();
463 return (true);
464}
465
466//////////////////////////////////////////////////////////////////////////////////////////////
467void
469{
470 statistical_weights_.clear ();
471 learned_weights_.clear ();
472 classes_.clear ();
473 sigmas_.clear ();
474 directions_to_center_.resize (0, 0);
475 clusters_centers_.resize (0, 0);
476 clusters_.clear ();
477 number_of_classes_ = 0;
478 number_of_visual_words_ = 0;
479 number_of_clusters_ = 0;
480 descriptors_dimension_ = 0;
481}
482
483//////////////////////////////////////////////////////////////////////////////////////////////
486{
487 if (this != &other)
488 {
489 this->reset ();
490
491 this->number_of_classes_ = other.number_of_classes_;
492 this->number_of_visual_words_ = other.number_of_visual_words_;
493 this->number_of_clusters_ = other.number_of_clusters_;
494 this->descriptors_dimension_ = other.descriptors_dimension_;
495
496 std::vector<float> vec;
497 vec.resize (number_of_clusters_, 0.0f);
498 this->statistical_weights_.resize (this->number_of_classes_, vec);
499 for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
500 for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
501 this->statistical_weights_[i_class][i_cluster] = other.statistical_weights_[i_class][i_cluster];
502
503 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
504 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
505 this->learned_weights_[i_visual_word] = other.learned_weights_[i_visual_word];
506
507 this->classes_.resize (this->number_of_visual_words_, 0);
508 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
509 this->classes_[i_visual_word] = other.classes_[i_visual_word];
510
511 this->sigmas_.resize (this->number_of_classes_, 0.0f);
512 for (unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
513 this->sigmas_[i_class] = other.sigmas_[i_class];
514
515 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
516 for (unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
517 for (unsigned int i_dim = 0; i_dim < 3; i_dim++)
518 this->directions_to_center_ (i_visual_word, i_dim) = other.directions_to_center_ (i_visual_word, i_dim);
519
520 this->clusters_centers_.resize (this->number_of_clusters_, 3);
521 for (unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
522 for (unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
523 this->clusters_centers_ (i_cluster, i_dim) = other.clusters_centers_ (i_cluster, i_dim);
524 }
525 return (*this);
526}
527
528//////////////////////////////////////////////////////////////////////////////////////////////
529template <int FeatureSize, typename PointT, typename NormalT>
531
532//////////////////////////////////////////////////////////////////////////////////////////////
533template <int FeatureSize, typename PointT, typename NormalT>
535{
536 training_clouds_.clear ();
537 training_classes_.clear ();
538 training_normals_.clear ();
539 training_sigmas_.clear ();
540 feature_estimator_.reset ();
541}
542
543//////////////////////////////////////////////////////////////////////////////////////////////
544template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
549
550//////////////////////////////////////////////////////////////////////////////////////////////
551template <int FeatureSize, typename PointT, typename NormalT> void
553 const std::vector< typename pcl::PointCloud<PointT>::Ptr >& training_clouds)
554{
555 training_clouds_.clear ();
556 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
557 training_clouds_.swap (clouds);
558}
559
560//////////////////////////////////////////////////////////////////////////////////////////////
561template <int FeatureSize, typename PointT, typename NormalT> std::vector<unsigned int>
566
567//////////////////////////////////////////////////////////////////////////////////////////////
568template <int FeatureSize, typename PointT, typename NormalT> void
570{
571 training_classes_.clear ();
572 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
573 training_classes_.swap (classes);
574}
575
576//////////////////////////////////////////////////////////////////////////////////////////////
577template <int FeatureSize, typename PointT, typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
582
583//////////////////////////////////////////////////////////////////////////////////////////////
584template <int FeatureSize, typename PointT, typename NormalT> void
586 const std::vector< typename pcl::PointCloud<NormalT>::Ptr >& training_normals)
587{
588 training_normals_.clear ();
589 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
590 training_normals_.swap (normals);
591}
592
593//////////////////////////////////////////////////////////////////////////////////////////////
594template <int FeatureSize, typename PointT, typename NormalT> float
599
600//////////////////////////////////////////////////////////////////////////////////////////////
601template <int FeatureSize, typename PointT, typename NormalT> void
603{
604 if (sampling_size >= std::numeric_limits<float>::epsilon ())
605 sampling_size_ = sampling_size;
606}
607
608//////////////////////////////////////////////////////////////////////////////////////////////
609template <int FeatureSize, typename PointT, typename NormalT> typename pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::FeaturePtr
614
615//////////////////////////////////////////////////////////////////////////////////////////////
616template <int FeatureSize, typename PointT, typename NormalT> void
621
622//////////////////////////////////////////////////////////////////////////////////////////////
623template <int FeatureSize, typename PointT, typename NormalT> unsigned int
628
629//////////////////////////////////////////////////////////////////////////////////////////////
630template <int FeatureSize, typename PointT, typename NormalT> void
632{
633 if (num_of_clusters > 0)
634 number_of_clusters_ = num_of_clusters;
635}
636
637//////////////////////////////////////////////////////////////////////////////////////////////
638template <int FeatureSize, typename PointT, typename NormalT> std::vector<float>
643
644//////////////////////////////////////////////////////////////////////////////////////////////
645template <int FeatureSize, typename PointT, typename NormalT> void
647{
648 training_sigmas_.clear ();
649 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
650 training_sigmas_.swap (sigmas);
651}
652
653//////////////////////////////////////////////////////////////////////////////////////////////
654template <int FeatureSize, typename PointT, typename NormalT> bool
659
660//////////////////////////////////////////////////////////////////////////////////////////////
661template <int FeatureSize, typename PointT, typename NormalT> void
666
667//////////////////////////////////////////////////////////////////////////////////////////////
668template <int FeatureSize, typename PointT, typename NormalT> bool
670{
671 bool success = true;
672
673 if (trained_model == nullptr)
674 return (false);
675 trained_model->reset ();
676
677 std::vector<pcl::Histogram<FeatureSize> > histograms;
678 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
679 success = extractDescriptors (histograms, locations);
680 if (!success)
681 return (false);
682
683 Eigen::MatrixXi labels;
684 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
685 if (!success)
686 return (false);
687
688 std::vector<unsigned int> vec;
689 trained_model->clusters_.resize (number_of_clusters_, vec);
690 for (std::size_t i_label = 0; i_label < locations.size (); i_label++)
691 trained_model->clusters_[labels (i_label)].push_back (i_label);
692
693 calculateSigmas (trained_model->sigmas_);
694
695 calculateWeights(
696 locations,
697 labels,
698 trained_model->sigmas_,
699 trained_model->clusters_,
700 trained_model->statistical_weights_,
701 trained_model->learned_weights_);
702
703 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
704 trained_model->number_of_visual_words_ = static_cast<unsigned int> (histograms.size ());
705 trained_model->number_of_clusters_ = number_of_clusters_;
706 trained_model->descriptors_dimension_ = FeatureSize;
707
708 trained_model->directions_to_center_.resize (locations.size (), 3);
709 trained_model->classes_.resize (locations.size ());
710 for (std::size_t i_dir = 0; i_dir < locations.size (); i_dir++)
711 {
712 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
713 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
714 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
715 trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
716 }
717
718 return (true);
719}
720
721//////////////////////////////////////////////////////////////////////////////////////////////
722template <int FeatureSize, typename PointT, typename NormalT> typename pcl::features::ISMVoteList<PointT>::Ptr
724 ISMModelPtr model,
725 typename pcl::PointCloud<PointT>::Ptr in_cloud,
726 typename pcl::PointCloud<Normal>::Ptr in_normals,
727 int in_class_of_interest)
728{
730
731 if (in_cloud->points.empty ())
732 return (out_votes);
733
734 typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
735 typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
736 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
737 if (sampled_point_cloud->points.empty ())
738 return (out_votes);
739
741 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
742
743 //find nearest cluster
744 const auto n_key_points = static_cast<unsigned int> (sampled_point_cloud->size ());
745 std::vector<int> min_dist_inds (n_key_points, -1);
746 for (unsigned int i_point = 0; i_point < n_key_points; i_point++)
747 {
748 Eigen::VectorXf curr_descriptor (FeatureSize);
749 for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
750 curr_descriptor (i_dim) = (*feature_cloud)[i_point].histogram[i_dim];
751
752 float descriptor_sum = curr_descriptor.sum ();
753 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
754 continue;
755
756 unsigned int min_dist_idx = 0;
757 Eigen::VectorXf clusters_center (FeatureSize);
758 for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
759 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
760
761 float best_dist = computeDistance (curr_descriptor, clusters_center);
762 for (unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
763 {
764 for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
765 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
766 float curr_dist = computeDistance (clusters_center, curr_descriptor);
767 if (curr_dist < best_dist)
768 {
769 min_dist_idx = i_clust_cent;
770 best_dist = curr_dist;
771 }
772 }
773 min_dist_inds[i_point] = min_dist_idx;
774 }//next keypoint
775
776 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
777 {
778 int min_dist_idx = min_dist_inds[i_point];
779 if (min_dist_idx == -1)
780 continue;
781
782 const auto n_words = static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
783 //compute coord system transform
784 Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
785 for (unsigned int i_word = 0; i_word < n_words; i_word++)
786 {
787 unsigned int index = model->clusters_[min_dist_idx][i_word];
788 unsigned int i_class = model->classes_[index];
789 if (static_cast<int> (i_class) != in_class_of_interest)
790 continue;//skip this class
791
792 //rotate dir to center as needed
793 Eigen::Vector3f direction (
794 model->directions_to_center_(index, 0),
795 model->directions_to_center_(index, 1),
796 model->directions_to_center_(index, 2));
797 applyTransform (direction, transform.transpose ());
798
800 Eigen::Vector3f vote_pos = (*sampled_point_cloud)[i_point].getVector3fMap () + direction;
801 vote.x = vote_pos[0];
802 vote.y = vote_pos[1];
803 vote.z = vote_pos[2];
804 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
805 float learned_weight = model->learned_weights_[index];
806 float power = statistical_weight * learned_weight;
807 vote.strength = power;
808 if (vote.strength > std::numeric_limits<float>::epsilon ())
809 out_votes->addVote (vote, (*sampled_point_cloud)[i_point], i_class);
810 }
811 }//next point
812
813 return (out_votes);
814}
815
816//////////////////////////////////////////////////////////////////////////////////////////////
817template <int FeatureSize, typename PointT, typename NormalT> bool
819 std::vector< pcl::Histogram<FeatureSize> >& histograms,
820 std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
821{
822 histograms.clear ();
823 locations.clear ();
824
825 if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ == nullptr)
826 return (false);
827
828 for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
829 {
830 //compute the center of the training object
831 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
832 const auto num_of_points = training_clouds_[i_cloud]->size ();
833 for (auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
834 models_center += point_j->getVector3fMap ();
835 models_center /= static_cast<float> (num_of_points);
836
837 //downsample the cloud
838 typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud (new pcl::PointCloud<PointT> ());
839 typename pcl::PointCloud<NormalT>::Ptr sampled_normal_cloud (new pcl::PointCloud<NormalT> ());
840 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
841 if (sampled_point_cloud->points.empty ())
842 continue;
843
844 shiftCloud (training_clouds_[i_cloud], models_center);
845 shiftCloud (sampled_point_cloud, models_center);
846
848 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
849
850 int point_index = 0;
851 for (auto point_i = sampled_point_cloud->points.cbegin (); point_i != sampled_point_cloud->points.cend (); point_i++, point_index++)
852 {
853 float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
854 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
855 continue;
856
857 histograms.insert ( histograms.end (), feature_cloud->begin () + point_index, feature_cloud->begin () + point_index + 1 );
858
859 int dist = static_cast<int> (std::distance (sampled_point_cloud->points.cbegin (), point_i));
860 Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
861 Eigen::Vector3f zero;
862 zero (0) = 0.0;
863 zero (1) = 0.0;
864 zero (2) = 0.0;
865 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
866 applyTransform (new_dir, new_basis);
867
868 PointT point (new_dir[0], new_dir[1], new_dir[2]);
869 LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
870 locations.insert(locations.end (), info);
871 }
872 }//next training cloud
873
874 return (true);
875}
876
877//////////////////////////////////////////////////////////////////////////////////////////////
878template <int FeatureSize, typename PointT, typename NormalT> bool
880 std::vector< pcl::Histogram<FeatureSize> >& histograms,
881 Eigen::MatrixXi& labels,
882 Eigen::MatrixXf& clusters_centers)
883{
884 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
885
886 for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
887 for (int i_dim = 0; i_dim < FeatureSize; i_dim++)
888 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
889
890 labels.resize (histograms.size(), 1);
891 computeKMeansClustering (
892 points_to_cluster,
893 number_of_clusters_,
894 labels,
895 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),//1000
896 5,
897 PP_CENTERS,
898 clusters_centers);
899
900 return (true);
901}
902
903//////////////////////////////////////////////////////////////////////////////////////////////
904template <int FeatureSize, typename PointT, typename NormalT> void
906{
907 if (!training_sigmas_.empty ())
908 {
909 sigmas.resize (training_sigmas_.size (), 0.0f);
910 for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
911 sigmas[i_sigma] = training_sigmas_[i_sigma];
912 return;
913 }
914
915 sigmas.clear ();
916 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
917 sigmas.resize (number_of_classes, 0.0f);
918
919 std::vector<float> vec;
920 std::vector<std::vector<float> > objects_sigmas;
921 objects_sigmas.resize (number_of_classes, vec);
922
923 auto number_of_objects = static_cast<unsigned int> (training_clouds_.size ());
924 for (unsigned int i_object = 0; i_object < number_of_objects; i_object++)
925 {
926 float max_distance = 0.0f;
927 const auto number_of_points = training_clouds_[i_object]->size ();
928 for (unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
929 for (unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
930 {
931 float curr_distance = 0.0f;
932 curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
933 curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
934 curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
935 if (curr_distance > max_distance)
936 max_distance = curr_distance;
937 }
938 max_distance = static_cast<float> (std::sqrt (max_distance));
939 unsigned int i_class = training_classes_[i_object];
940 objects_sigmas[i_class].push_back (max_distance);
941 }
942
943 for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
944 {
945 float sig = 0.0f;
946 auto number_of_objects_in_class = static_cast<unsigned int> (objects_sigmas[i_class].size ());
947 for (unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
948 sig += objects_sigmas[i_class][i_object];
949 sig /= (static_cast<float> (number_of_objects_in_class) * 10.0f);
950 sigmas[i_class] = sig;
951 }
952}
953
954//////////////////////////////////////////////////////////////////////////////////////////////
955template <int FeatureSize, typename PointT, typename NormalT> void
957 const std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
958 const Eigen::MatrixXi &labels,
959 std::vector<float>& sigmas,
960 std::vector<std::vector<unsigned int> >& clusters,
961 std::vector<std::vector<float> >& statistical_weights,
962 std::vector<float>& learned_weights)
963{
964 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
965 //Temporary variable
966 std::vector<float> vec;
967 vec.resize (number_of_clusters_, 0.0f);
968 statistical_weights.clear ();
969 learned_weights.clear ();
970 statistical_weights.resize (number_of_classes, vec);
971 learned_weights.resize (locations.size (), 0.0f);
972
973 //Temporary variable
974 std::vector<int> vect;
975 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
976
977 //Number of features from which c_i was learned
978 std::vector<int> n_ftr;
979
980 //Total number of votes from visual word v_j
981 std::vector<int> n_vot;
982
983 //Number of visual words that vote for class c_i
984 std::vector<int> n_vw;
985
986 //Number of votes for class c_i from v_j
987 std::vector<std::vector<int> > n_vot_2;
988
989 n_vot_2.resize (number_of_clusters_, vect);
990 n_vot.resize (number_of_clusters_, 0);
991 n_ftr.resize (number_of_classes, 0);
992 for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
993 {
994 int i_class = training_classes_[locations[i_location].model_num_];
995 int i_cluster = labels (i_location);
996 n_vot_2[i_cluster][i_class] += 1;
997 n_vot[i_cluster] += 1;
998 n_ftr[i_class] += 1;
999 }
1000
1001 n_vw.resize (number_of_classes, 0);
1002 for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1003 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1004 if (n_vot_2[i_cluster][i_class] > 0)
1005 n_vw[i_class] += 1;
1006
1007 //computing learned weights
1008 learned_weights.resize (locations.size (), 0.0);
1009 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1010 {
1011 auto number_of_words_in_cluster = static_cast<unsigned int> (clusters[i_cluster].size ());
1012 for (unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1013 {
1014 unsigned int i_index = clusters[i_cluster][i_visual_word];
1015 int i_class = training_classes_[locations[i_index].model_num_];
1016 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1017 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1018 {
1019 std::vector<float> calculated_sigmas;
1020 calculateSigmas (calculated_sigmas);
1021 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1022 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1023 continue;
1024 }
1025 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1026 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1027 applyTransform (direction, transform);
1028 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1029
1030 //collect gaussian weighted distances
1031 std::vector<float> gauss_dists;
1032 for (unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1033 {
1034 unsigned int j_index = clusters[i_cluster][j_visual_word];
1035 int j_class = training_classes_[locations[j_index].model_num_];
1036 if (i_class != j_class)
1037 continue;
1038 //predict center
1039 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1040 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1041 applyTransform (direction_2, transform_2);
1042 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1043 float residual = (predicted_center - actual_center).norm ();
1044 float value = -residual * residual / square_sigma_dist;
1045 gauss_dists.push_back (static_cast<float> (std::exp (value)));
1046 }//next word
1047 //find median gaussian weighted distance
1048 std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1049 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1050 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1051 }//next word
1052 }//next cluster
1053
1054 //computing statistical weights
1055 for (unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1056 {
1057 for (unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1058 {
1059 if (n_vot_2[i_cluster][i_class] == 0)
1060 continue;//no votes per class of interest in this cluster
1061 if (n_vw[i_class] == 0)
1062 continue;//there were no objects of this class in the training dataset
1063 if (n_vot[i_cluster] == 0)
1064 continue;//this cluster has never been used
1065 if (n_ftr[i_class] == 0)
1066 continue;//there were no objects of this class in the training dataset
1067 float part_1 = static_cast<float> (n_vw[i_class]);
1068 float part_2 = static_cast<float> (n_vot[i_cluster]);
1069 float part_3 = static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1070 float part_4 = 0.0f;
1071
1072 if (!n_vot_ON_)
1073 part_2 = 1.0f;
1074
1075 for (unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1076 if (n_ftr[j_class] != 0)
1077 part_4 += static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1078
1079 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1080 }
1081 }//next cluster
1082}
1083
1084//////////////////////////////////////////////////////////////////////////////////////////////
1085template <int FeatureSize, typename PointT, typename NormalT> void
1087 typename pcl::PointCloud<PointT>::ConstPtr in_point_cloud,
1088 typename pcl::PointCloud<NormalT>::ConstPtr in_normal_cloud,
1089 typename pcl::PointCloud<PointT>::Ptr out_sampled_point_cloud,
1090 typename pcl::PointCloud<NormalT>::Ptr out_sampled_normal_cloud)
1091{
1092 //create voxel grid
1094 grid.setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1095 grid.setSaveLeafLayout (true);
1096 grid.setInputCloud (in_point_cloud);
1097
1098 pcl::PointCloud<PointT> temp_cloud;
1099 grid.filter (temp_cloud);
1100
1101 //extract indices of points from source cloud which are closest to grid points
1102 constexpr float max_value = std::numeric_limits<float>::max ();
1103
1104 const auto num_source_points = in_point_cloud->size ();
1105 const auto num_sample_points = temp_cloud.size ();
1106
1107 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1108 std::vector<int> sampling_indices (num_sample_points, -1);
1109
1110 for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1111 {
1112 int index = grid.getCentroidIndex ((*in_point_cloud)[i_point]);
1113 if (index == -1)
1114 continue;
1115
1116 PointT pt_1 = (*in_point_cloud)[i_point];
1117 PointT pt_2 = temp_cloud[index];
1118
1119 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);
1120 if (distance < dist_to_grid_center[index])
1121 {
1122 dist_to_grid_center[index] = distance;
1123 sampling_indices[index] = static_cast<int> (i_point);
1124 }
1125 }
1126
1127 //extract source points
1128 pcl::PointIndices::Ptr final_inliers_indices (new pcl::PointIndices ());
1129 pcl::ExtractIndices<PointT> extract_points;
1130 pcl::ExtractIndices<NormalT> extract_normals;
1131
1132 final_inliers_indices->indices.reserve (num_sample_points);
1133 for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1134 {
1135 if (sampling_indices[i_point] != -1)
1136 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1137 }
1138
1139 extract_points.setInputCloud (in_point_cloud);
1140 extract_points.setIndices (final_inliers_indices);
1141 extract_points.filter (*out_sampled_point_cloud);
1142
1143 extract_normals.setInputCloud (in_normal_cloud);
1144 extract_normals.setIndices (final_inliers_indices);
1145 extract_normals.filter (*out_sampled_normal_cloud);
1146}
1147
1148//////////////////////////////////////////////////////////////////////////////////////////////
1149template <int FeatureSize, typename PointT, typename NormalT> void
1151 typename pcl::PointCloud<PointT>::Ptr in_cloud,
1152 Eigen::Vector3f shift_point)
1153{
1154 for (auto point_it = in_cloud->points.begin (); point_it != in_cloud->points.end (); point_it++)
1155 {
1156 point_it->x -= shift_point.x ();
1157 point_it->y -= shift_point.y ();
1158 point_it->z -= shift_point.z ();
1159 }
1160}
1161
1162//////////////////////////////////////////////////////////////////////////////////////////////
1163template <int FeatureSize, typename PointT, typename NormalT> Eigen::Matrix3f
1165{
1166 Eigen::Matrix3f result;
1167 Eigen::Matrix3f rotation_matrix_X;
1168 Eigen::Matrix3f rotation_matrix_Z;
1169
1170 float A = 0.0f;
1171 float B = 0.0f;
1172 float sign = -1.0f;
1173
1174 float denom_X = static_cast<float> (std::sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1175 A = in_normal.normal_y / denom_X;
1176 B = sign * in_normal.normal_z / denom_X;
1177 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1178 0.0f, A, -B,
1179 0.0f, B, A;
1180
1181 float denom_Z = static_cast<float> (std::sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1182 A = in_normal.normal_y / denom_Z;
1183 B = sign * in_normal.normal_x / denom_Z;
1184 rotation_matrix_Z << A, -B, 0.0f,
1185 B, A, 0.0f,
1186 0.0f, 0.0f, 1.0f;
1187
1188 result = rotation_matrix_X * rotation_matrix_Z;
1189
1190 return (result);
1191}
1192
1193//////////////////////////////////////////////////////////////////////////////////////////////
1194template <int FeatureSize, typename PointT, typename NormalT> void
1195pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform)
1196{
1197 io_vec = in_transform * io_vec;
1198}
1199
1200//////////////////////////////////////////////////////////////////////////////////////////////
1201template <int FeatureSize, typename PointT, typename NormalT> void
1203 typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud,
1204 typename pcl::PointCloud<NormalT>::Ptr normal_cloud,
1205 typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud)
1206{
1208// tree->setInputCloud (point_cloud);
1209
1210 feature_estimator_->setInputCloud (sampled_point_cloud->makeShared ());
1211// feature_estimator_->setSearchSurface (point_cloud->makeShared ());
1212 feature_estimator_->setSearchMethod (tree);
1213
1214// typename pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm =
1215// dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1216// feat_est_norm->setInputNormals (normal_cloud);
1217
1219 dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1220 feat_est_norm->setInputNormals (normal_cloud);
1221
1222 feature_estimator_->compute (*feature_cloud);
1223}
1224
1225//////////////////////////////////////////////////////////////////////////////////////////////
1226template <int FeatureSize, typename PointT, typename NormalT> double
1228 const Eigen::MatrixXf& points_to_cluster,
1229 int number_of_clusters,
1230 Eigen::MatrixXi& io_labels,
1231 TermCriteria criteria,
1232 int attempts,
1233 int flags,
1234 Eigen::MatrixXf& cluster_centers)
1235{
1236 constexpr int spp_trials = 3;
1237 std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1238 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1239
1240 attempts = std::max (attempts, 1);
1241 srand (static_cast<unsigned int> (time (nullptr)));
1242
1243 Eigen::MatrixXi labels (number_of_points, 1);
1244
1245 if (flags & USE_INITIAL_LABELS)
1246 labels = io_labels;
1247 else
1248 labels.setZero ();
1249
1250 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1251 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1252 std::vector<int> counters (number_of_clusters);
1253 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1254 Eigen::Vector2f* box = boxes.data();
1255
1256 double best_compactness = std::numeric_limits<double>::max ();
1257 double compactness = 0.0;
1258
1259 if (criteria.type_ & TermCriteria::EPS)
1260 criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f);
1261 else
1262 criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1263
1264 criteria.epsilon_ *= criteria.epsilon_;
1265
1266 if (criteria.type_ & TermCriteria::COUNT)
1267 criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100);
1268 else
1269 criteria.max_count_ = 100;
1270
1271 if (number_of_clusters == 1)
1272 {
1273 attempts = 1;
1274 criteria.max_count_ = 2;
1275 }
1276
1277 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1278 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1279
1280 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1281 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1282 {
1283 float v = points_to_cluster (i_point, i_dim);
1284 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1285 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1286 }
1287
1288 for (int i_attempt = 0; i_attempt < attempts; i_attempt++)
1289 {
1290 float max_center_shift = std::numeric_limits<float>::max ();
1291 for (int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++)
1292 {
1293 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1294 temp = centers;
1295 centers = old_centers;
1296 old_centers = temp;
1297
1298 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1299 {
1300 if (flags & PP_CENTERS)
1301 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1302 else
1303 {
1304 for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1305 {
1306 Eigen::VectorXf center (feature_dimension);
1307 generateRandomCenter (boxes, center);
1308 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1309 centers (i_cl_center, i_dim) = center (i_dim);
1310 }//generate center for next cluster
1311 }//end if-else random or PP centers
1312 }
1313 else
1314 {
1315 centers.setZero ();
1316 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1317 counters[i_cluster] = 0;
1318 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1319 {
1320 int i_label = labels (i_point, 0);
1321 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1322 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1323 counters[i_label]++;
1324 }
1325 if (iter > 0)
1326 max_center_shift = 0.0f;
1327 for (int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1328 {
1329 if (counters[i_cl_center] != 0)
1330 {
1331 float scale = 1.0f / static_cast<float> (counters[i_cl_center]);
1332 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1333 centers (i_cl_center, i_dim) *= scale;
1334 }
1335 else
1336 {
1337 Eigen::VectorXf center (feature_dimension);
1338 generateRandomCenter (boxes, center);
1339 for(int i_dim = 0; i_dim < feature_dimension; i_dim++)
1340 centers (i_cl_center, i_dim) = center (i_dim);
1341 }
1342
1343 if (iter > 0)
1344 {
1345 float dist = 0.0f;
1346 for (int i_dim = 0; i_dim < feature_dimension; i_dim++)
1347 {
1348 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1349 dist += diff * diff;
1350 }
1351 max_center_shift = std::max (max_center_shift, dist);
1352 }
1353 }
1354 }
1355 compactness = 0.0f;
1356 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1357 {
1358 Eigen::VectorXf sample (feature_dimension);
1359 sample = points_to_cluster.row (i_point);
1360
1361 int k_best = 0;
1362 float min_dist = std::numeric_limits<float>::max ();
1363
1364 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1365 {
1366 Eigen::VectorXf center (feature_dimension);
1367 center = centers.row (i_cluster);
1368 float dist = computeDistance (sample, center);
1369 if (min_dist > dist)
1370 {
1371 min_dist = dist;
1372 k_best = i_cluster;
1373 }
1374 }
1375 compactness += min_dist;
1376 labels (i_point, 0) = k_best;
1377 }
1378 }//next iteration
1379
1380 if (compactness < best_compactness)
1381 {
1382 best_compactness = compactness;
1383 cluster_centers = centers;
1384 io_labels = labels;
1385 }
1386 }//next attempt
1387
1388 return (best_compactness);
1389}
1390
1391//////////////////////////////////////////////////////////////////////////////////////////////
1392template <int FeatureSize, typename PointT, typename NormalT> void
1394 const Eigen::MatrixXf& data,
1395 Eigen::MatrixXf& out_centers,
1396 int number_of_clusters,
1397 int trials)
1398{
1399 std::size_t dimension = data.cols ();
1400 auto number_of_points = static_cast<unsigned int> (data.rows ());
1401 std::vector<int> centers_vec (number_of_clusters);
1402 int* centers = centers_vec.data();
1403 std::vector<double> dist (number_of_points);
1404 std::vector<double> tdist (number_of_points);
1405 std::vector<double> tdist2 (number_of_points);
1406 double sum0 = 0.0;
1407
1408 unsigned int random_unsigned = rand ();
1409 centers[0] = random_unsigned % number_of_points;
1410
1411 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1412 {
1413 Eigen::VectorXf first (dimension);
1414 Eigen::VectorXf second (dimension);
1415 first = data.row (i_point);
1416 second = data.row (centers[0]);
1417 dist[i_point] = computeDistance (first, second);
1418 sum0 += dist[i_point];
1419 }
1420
1421 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1422 {
1423 double best_sum = std::numeric_limits<double>::max ();
1424 int best_center = -1;
1425 for (int i_trials = 0; i_trials < trials; i_trials++)
1426 {
1427 unsigned int random_integer = rand () - 1;
1428 double random_double = static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1429 double p = random_double * sum0;
1430
1431 unsigned int i_point;
1432 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1433 if ( (p -= dist[i_point]) <= 0.0)
1434 break;
1435
1436 int ci = i_point;
1437
1438 double s = 0.0;
1439 for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
1440 {
1441 Eigen::VectorXf first (dimension);
1442 Eigen::VectorXf second (dimension);
1443 first = data.row (i_point);
1444 second = data.row (ci);
1445 tdist2[i_point] = std::min (static_cast<double> (computeDistance (first, second)), dist[i_point]);
1446 s += tdist2[i_point];
1447 }
1448
1449 if (s <= best_sum)
1450 {
1451 best_sum = s;
1452 best_center = ci;
1453 std::swap (tdist, tdist2);
1454 }
1455 }
1456
1457 centers[i_cluster] = best_center;
1458 sum0 = best_sum;
1459 std::swap (dist, tdist);
1460 }
1461
1462 for (int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1463 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1464 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1465}
1466
1467//////////////////////////////////////////////////////////////////////////////////////////////
1468template <int FeatureSize, typename PointT, typename NormalT> void
1469pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::generateRandomCenter (const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >& boxes,
1470 Eigen::VectorXf& center)
1471{
1472 std::size_t dimension = boxes.size ();
1473 float margin = 1.0f / static_cast<float> (dimension);
1474
1475 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1476 {
1477 unsigned int random_integer = rand () - 1;
1478 float random_float = static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1479 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1480 }
1481}
1482
1483//////////////////////////////////////////////////////////////////////////////////////////////
1484template <int FeatureSize, typename PointT, typename NormalT> float
1486{
1487 std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1488 float distance = 0.0f;
1489 for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1490 {
1491 float diff = vec_1 (i_dim) - vec_2 (i_dim);
1492 distance += diff * diff;
1493 }
1494
1495 return (distance);
1496}
1497
1498#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:210
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
Definition voxel_grid.h:343
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:247
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition voxel_grid.h:304
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.