41 #ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_
42 #define PCL_IMPLICIT_SHAPE_MODEL_HPP_
44 #include "../implicit_shape_model.h"
45 #include <pcl/filters/voxel_grid.h>
46 #include <pcl/filters/extract_indices.h>
51 template <
typename Po
intT>
54 tree_is_valid_ (false),
63 template <
typename Po
intT>
66 votes_class_.clear ();
67 votes_origins_.reset ();
75 template <
typename Po
intT>
void
79 tree_is_valid_ =
false;
80 votes_->points.insert (votes_->points.end (), vote);
82 votes_origins_->points.push_back (vote_origin);
83 votes_class_.push_back (votes_class);
93 colored_cloud->
width = 1;
101 for (
const auto& i_point: *cloud)
106 colored_cloud->
points.push_back (point);
113 for (
const auto &i_vote : votes_->points)
118 colored_cloud->
points.push_back (point);
120 colored_cloud->
height += votes_->size ();
122 return (colored_cloud);
126 template <
typename Po
intT>
void
128 std::vector<
pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
130 double in_non_maxima_radius,
135 const std::size_t n_vote_classes = votes_class_.size ();
136 if (n_vote_classes == 0)
138 for (std::size_t i = 0; i < n_vote_classes ; i++)
139 assert ( votes_class_[i] == in_class_id );
143 const int NUM_INIT_PTS = 100;
144 double SIGMA_DIST = in_sigma;
145 const double FINAL_EPS = SIGMA_DIST / 100;
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++)
152 Eigen::Vector3f old_center;
153 const auto idx = votes_->size() * i / NUM_INIT_PTS;
154 Eigen::Vector3f curr_center = (*votes_)[idx].getVector3fMap();
158 old_center = curr_center;
159 curr_center = shiftMean (old_center, SIGMA_DIST);
160 }
while ((old_center - curr_center).norm () > FINAL_EPS);
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);
169 peaks[i] = curr_center;
170 peak_densities[i] = curr_density;
172 if ( max_density < curr_density )
173 max_density = curr_density;
177 std::vector<bool> peak_flag (NUM_INIT_PTS,
true);
178 for (
int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
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++)
190 if ( peak_densities[i] > best_density)
192 best_density = peak_densities[i];
193 strongest_peak = peaks[i];
199 if( peak_counter == 0 )
203 peak.x = strongest_peak(0);
204 peak.y = strongest_peak(1);
205 peak.z = strongest_peak(2);
208 out_peaks.push_back ( peak );
211 peak_flag[best_peak_ind] =
false;
212 for (
int i = 0; i < NUM_INIT_PTS; i++)
218 double dist = (strongest_peak - peaks[i]).norm ();
219 if ( dist < in_non_maxima_radius )
220 peak_flag[i] =
false;
226 template <
typename Po
intT>
void
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;
241 template <
typename Po
intT> Eigen::Vector3f
246 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
253 std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
255 for (std::size_t j = 0; j < n_pts; j++)
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);
262 assert (denom > 0.0);
264 return (wgh_sum /
static_cast<float> (denom));
268 template <
typename Po
intT>
double
270 const PointT &point,
double sigma_dist)
274 const std::size_t n_vote_classes = votes_class_.size ();
275 if (n_vote_classes == 0)
278 double sum_vote = 0.0;
284 std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
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));
293 template <
typename Po
intT>
unsigned int
296 return (
static_cast<unsigned int> (votes_->size ()));
301 statistical_weights_ (0),
302 learned_weights_ (0),
306 number_of_classes_ (0),
307 number_of_visual_words_ (0),
308 number_of_clusters_ (0),
309 descriptors_dimension_ (0)
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++)
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];
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];
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];
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);
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);
363 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
366 output_file.close ();
370 output_file << number_of_classes_ <<
" ";
371 output_file << number_of_visual_words_ <<
" ";
372 output_file << number_of_clusters_ <<
" ";
373 output_file << descriptors_dimension_ <<
" ";
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] <<
" ";
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] <<
" ";
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] <<
" ";
389 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
390 output_file << sigmas_[i_class] <<
" ";
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) <<
" ";
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) <<
" ";
403 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
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 <<
" ";
410 output_file.close ();
419 std::ifstream input_file (file_name.c_str ());
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);
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];
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];
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];
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];
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);
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);
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++)
474 unsigned int size_of_current_cluster = 0;
475 input_file >> size_of_current_cluster;
476 clusters_[i_cluster].resize (size_of_current_cluster, 0);
477 for (
unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
478 input_file >> clusters_[i_cluster][i_visual_word];
489 statistical_weights_.clear ();
490 learned_weights_.clear ();
493 directions_to_center_.resize (0, 0);
494 clusters_centers_.resize (0, 0);
496 number_of_classes_ = 0;
497 number_of_visual_words_ = 0;
498 number_of_clusters_ = 0;
499 descriptors_dimension_ = 0;
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];
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];
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];
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];
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);
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);
548 template <
int FeatureSize,
typename Po
intT,
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),
562 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
565 training_clouds_.clear ();
566 training_classes_.clear ();
567 training_normals_.clear ();
568 training_sigmas_.clear ();
569 feature_estimator_.reset ();
573 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
576 return (training_clouds_);
580 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
584 training_clouds_.clear ();
585 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
586 training_clouds_.swap (clouds);
590 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
593 return (training_classes_);
597 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
600 training_classes_.clear ();
601 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
602 training_classes_.swap (classes);
606 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
609 return (training_normals_);
613 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
617 training_normals_.clear ();
618 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
619 training_normals_.swap (normals);
623 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
626 return (sampling_size_);
630 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
633 if (sampling_size >= std::numeric_limits<float>::epsilon ())
634 sampling_size_ = sampling_size;
641 return (feature_estimator_);
645 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
648 feature_estimator_ = feature;
652 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int
655 return (number_of_clusters_);
659 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
662 if (num_of_clusters > 0)
663 number_of_clusters_ = num_of_clusters;
667 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
670 return (training_sigmas_);
674 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
677 training_sigmas_.clear ();
678 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
679 training_sigmas_.swap (sigmas);
683 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
690 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
697 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
702 if (trained_model ==
nullptr)
704 trained_model->reset ();
706 std::vector<pcl::Histogram<FeatureSize> > histograms;
707 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
708 success = extractDescriptors (histograms, locations);
712 Eigen::MatrixXi labels;
713 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
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);
722 calculateSigmas (trained_model->sigmas_);
727 trained_model->sigmas_,
728 trained_model->clusters_,
729 trained_model->statistical_weights_,
730 trained_model->learned_weights_);
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;
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++)
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_];
756 int in_class_of_interest)
760 if (in_cloud->
points.empty ())
765 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
766 if (sampled_point_cloud->
points.empty ())
770 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
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++)
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];
781 float descriptor_sum = curr_descriptor.sum ();
782 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
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);
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++)
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)
798 min_dist_idx = i_clust_cent;
799 best_dist = curr_dist;
802 min_dist_inds[i_point] = min_dist_idx;
805 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
807 int min_dist_idx = min_dist_inds[i_point];
808 if (min_dist_idx == -1)
811 const unsigned int n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
813 Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
814 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
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)
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 ());
829 Eigen::Vector3f vote_pos = (*sampled_point_cloud)[i_point].getVector3fMap () + direction;
830 vote.x = vote_pos[0];
831 vote.y = vote_pos[1];
832 vote.z = vote_pos[2];
833 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
834 float learned_weight = model->learned_weights_[index];
835 float power = statistical_weight * learned_weight;
837 if (vote.
strength > std::numeric_limits<float>::epsilon ())
838 out_votes->
addVote (vote, (*sampled_point_cloud)[i_point], i_class);
846 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
849 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
854 int n_key_points = 0;
856 if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ ==
nullptr)
859 for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
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);
871 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
872 if (sampled_point_cloud->
points.empty ())
875 shiftCloud (training_clouds_[i_cloud], models_center);
876 shiftCloud (sampled_point_cloud, models_center);
878 n_key_points +=
static_cast<int> (sampled_point_cloud->
size ());
881 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
884 for (
auto point_i = sampled_point_cloud->
points.cbegin (); point_i != sampled_point_cloud->
points.cend (); point_i++, point_index++)
886 float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
887 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
890 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
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;
898 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
899 applyTransform (new_dir, new_basis);
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);
911 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
914 Eigen::MatrixXi& labels,
915 Eigen::MatrixXf& clusters_centers)
917 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
919 for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
920 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
921 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
923 labels.resize (histograms.size(), 1);
924 computeKMeansClustering (
928 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
937 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
940 if (!training_sigmas_.empty ())
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];
949 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
950 sigmas.resize (number_of_classes, 0.0f);
952 std::vector<float> vec;
953 std::vector<std::vector<float> > objects_sigmas;
954 objects_sigmas.resize (number_of_classes, vec);
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++)
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++)
964 float curr_distance = 0.0f;
965 curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
966 curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
967 curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
968 if (curr_distance > max_distance)
969 max_distance = curr_distance;
971 max_distance =
static_cast<float> (sqrt (max_distance));
972 unsigned int i_class = training_classes_[i_object];
973 objects_sigmas[i_class].push_back (max_distance);
976 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
979 unsigned int number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
980 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
981 sig += objects_sigmas[i_class][i_object];
982 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
983 sigmas[i_class] = sig;
988 template <
int FeatureSize,
typename Po
intT,
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)
997 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
999 std::vector<float> vec;
1000 vec.resize (number_of_clusters_, 0.0f);
1001 statistical_weights.clear ();
1002 learned_weights.clear ();
1003 statistical_weights.resize (number_of_classes, vec);
1004 learned_weights.resize (locations.size (), 0.0f);
1007 std::vector<int> vect;
1008 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
1011 std::vector<int> n_ftr;
1014 std::vector<int> n_vot;
1017 std::vector<int> n_vw;
1020 std::vector<std::vector<int> > n_vot_2;
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++)
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;
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)
1041 learned_weights.resize (locations.size (), 0.0);
1042 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1044 unsigned int number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1045 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1047 unsigned int i_index = clusters[i_cluster][i_visual_word];
1048 int i_class = training_classes_[locations[i_index].model_num_];
1049 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1050 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1052 std::vector<float> calculated_sigmas;
1053 calculateSigmas (calculated_sigmas);
1054 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1055 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
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;
1064 std::vector<float> gauss_dists;
1065 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
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)
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)));
1081 std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1082 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1083 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1088 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1090 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1092 if (n_vot_2[i_cluster][i_class] == 0)
1094 if (n_vw[i_class] == 0)
1096 if (n_vot[i_cluster] == 0)
1098 if (n_ftr[i_class] == 0)
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;
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]);
1112 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1118 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1127 grid.
setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1132 grid.
filter (temp_cloud);
1135 const float max_value = std::numeric_limits<float>::max ();
1137 const auto num_source_points = in_point_cloud->
size ();
1138 const auto num_sample_points = temp_cloud.
size ();
1140 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1141 std::vector<int> sampling_indices (num_sample_points, -1);
1143 for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1149 PointT pt_1 = (*in_point_cloud)[i_point];
1150 PointT pt_2 = temp_cloud[index];
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])
1155 dist_to_grid_center[index] =
distance;
1156 sampling_indices[index] =
static_cast<int> (i_point);
1165 final_inliers_indices->indices.reserve (num_sample_points);
1166 for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1168 if (sampling_indices[i_point] != -1)
1169 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1173 extract_points.
setIndices (final_inliers_indices);
1174 extract_points.
filter (*out_sampled_point_cloud);
1177 extract_normals.
setIndices (final_inliers_indices);
1178 extract_normals.
filter (*out_sampled_normal_cloud);
1182 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1185 Eigen::Vector3f shift_point)
1187 for (
auto point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1189 point_it->x -= shift_point.x ();
1190 point_it->y -= shift_point.y ();
1191 point_it->z -= shift_point.z ();
1196 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1199 Eigen::Matrix3f result;
1200 Eigen::Matrix3f rotation_matrix_X;
1201 Eigen::Matrix3f rotation_matrix_Z;
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,
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,
1221 result = rotation_matrix_X * rotation_matrix_Z;
1227 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1230 io_vec = in_transform * io_vec;
1234 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1243 feature_estimator_->setInputCloud (sampled_point_cloud->
makeShared ());
1245 feature_estimator_->setSearchMethod (tree);
1252 dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1255 feature_estimator_->compute (*feature_cloud);
1259 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double
1261 const Eigen::MatrixXf& points_to_cluster,
1262 int number_of_clusters,
1263 Eigen::MatrixXi& io_labels,
1267 Eigen::MatrixXf& cluster_centers)
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;
1273 attempts = std::max (attempts, 1);
1274 srand (
static_cast<unsigned int> (time (
nullptr)));
1276 Eigen::MatrixXi labels (number_of_points, 1);
1278 if (flags & USE_INITIAL_LABELS)
1283 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1284 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1285 std::vector<int> counters (number_of_clusters);
1286 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1287 Eigen::Vector2f* box = &boxes[0];
1289 double best_compactness = std::numeric_limits<double>::max ();
1290 double compactness = 0.0;
1292 if (criteria.
type_ & TermCriteria::EPS)
1295 criteria.
epsilon_ = std::numeric_limits<float>::epsilon ();
1299 if (criteria.
type_ & TermCriteria::COUNT)
1304 if (number_of_clusters == 1)
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));
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++)
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);
1321 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
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++)
1326 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1328 centers = old_centers;
1331 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1333 if (flags & PP_CENTERS)
1334 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1337 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1339 Eigen::VectorXf center (feature_dimension);
1340 generateRandomCenter (boxes, center);
1341 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1342 centers (i_cl_center, i_dim) = center (i_dim);
1349 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1350 counters[i_cluster] = 0;
1351 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1353 int i_label = labels (i_point, 0);
1354 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1355 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1356 counters[i_label]++;
1359 max_center_shift = 0.0f;
1360 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1362 if (counters[i_cl_center] != 0)
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;
1370 Eigen::VectorXf center (feature_dimension);
1371 generateRandomCenter (boxes, center);
1372 for(
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1373 centers (i_cl_center, i_dim) = center (i_dim);
1379 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1381 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1382 dist += diff * diff;
1384 max_center_shift = std::max (max_center_shift, dist);
1389 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1391 Eigen::VectorXf sample (feature_dimension);
1392 sample = points_to_cluster.row (i_point);
1395 float min_dist = std::numeric_limits<float>::max ();
1397 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1399 Eigen::VectorXf center (feature_dimension);
1400 center = centers.row (i_cluster);
1401 float dist = computeDistance (sample, center);
1402 if (min_dist > dist)
1408 compactness += min_dist;
1409 labels (i_point, 0) = k_best;
1413 if (compactness < best_compactness)
1415 best_compactness = compactness;
1416 cluster_centers = centers;
1421 return (best_compactness);
1425 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1427 const Eigen::MatrixXf& data,
1428 Eigen::MatrixXf& out_centers,
1429 int number_of_clusters,
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 = ¢ers_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);
1441 unsigned int random_unsigned = rand ();
1442 centers[0] = random_unsigned % number_of_points;
1444 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
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];
1454 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
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++)
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;
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)
1472 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
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];
1486 std::swap (tdist, tdist2);
1490 centers[i_cluster] = best_center;
1492 std::swap (dist, tdist);
1495 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1496 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1497 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1501 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1503 Eigen::VectorXf& center)
1505 std::size_t dimension = boxes.size ();
1506 float margin = 1.0f /
static_cast<float> (dimension);
1508 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
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);
1517 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
1520 std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1522 for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1524 float diff = vec_1 (i_dim) - vec_2 (i_dim);
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
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).
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.
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
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.
typename Feature::Ptr FeaturePtr
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 ¢er)
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.
pcl::features::ISMModel::Ptr ISMModelPtr
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...
shared_ptr< pcl::search::Search< PointT > > Ptr
Defines functions, macros and traits for allocating and using memory.
float distance(const PointT &p1, const PointT &p2)
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.