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>
55 template <
typename Po
intT>
58 votes_class_.clear ();
59 votes_origins_.reset ();
67 template <
typename Po
intT>
void
71 tree_is_valid_ =
false;
72 votes_->points.insert (votes_->points.end (), vote);
74 votes_origins_->points.push_back (vote_origin);
75 votes_class_.push_back (votes_class);
85 colored_cloud->
width = 1;
93 for (
const auto& i_point: *cloud)
98 colored_cloud->
points.push_back (point);
105 for (
const auto &i_vote : votes_->points)
110 colored_cloud->
points.push_back (point);
112 colored_cloud->
height += votes_->size ();
114 return (colored_cloud);
118 template <
typename Po
intT>
void
120 std::vector<
pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
122 double in_non_maxima_radius,
127 const std::size_t n_vote_classes = votes_class_.size ();
128 if (n_vote_classes == 0)
130 for (std::size_t i = 0; i < n_vote_classes ; i++)
131 assert ( votes_class_[i] == in_class_id );
135 constexpr
int NUM_INIT_PTS = 100;
136 double SIGMA_DIST = in_sigma;
137 const double FINAL_EPS = SIGMA_DIST / 100;
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++)
144 Eigen::Vector3f old_center;
145 const auto idx = votes_->size() * i / NUM_INIT_PTS;
146 Eigen::Vector3f curr_center = (*votes_)[idx].getVector3fMap();
150 old_center = curr_center;
151 curr_center = shiftMean (old_center, SIGMA_DIST);
152 }
while ((old_center - curr_center).norm () > FINAL_EPS);
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);
161 peaks[i] = curr_center;
162 peak_densities[i] = curr_density;
164 if ( max_density < curr_density )
165 max_density = curr_density;
169 std::vector<bool> peak_flag (NUM_INIT_PTS,
true);
170 for (
int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
173 double best_density = -1.0;
174 Eigen::Vector3f strongest_peak = Eigen::Vector3f::Constant (-1);
175 int best_peak_ind (-1);
176 int peak_counter (0);
177 for (
int i = 0; i < NUM_INIT_PTS; i++)
182 if ( peak_densities[i] > best_density)
184 best_density = peak_densities[i];
185 strongest_peak = peaks[i];
191 if( best_density == -1.0 || strongest_peak == Eigen::Vector3f::Constant (-1) ||
192 best_peak_ind == -1 || peak_counter == 0 )
196 peak.x = strongest_peak(0);
197 peak.y = strongest_peak(1);
198 peak.z = strongest_peak(2);
201 out_peaks.push_back ( peak );
204 peak_flag[best_peak_ind] =
false;
205 for (
int i = 0; i < NUM_INIT_PTS; i++)
211 double dist = (strongest_peak - peaks[i]).norm ();
212 if ( dist < in_non_maxima_radius )
213 peak_flag[i] =
false;
219 template <
typename Po
intT>
void
224 if (tree_ ==
nullptr)
226 tree_->setInputCloud (votes_);
227 k_ind_.resize ( votes_->size (), -1 );
228 k_sqr_dist_.resize ( votes_->size (), 0.0f );
229 tree_is_valid_ =
true;
234 template <
typename Po
intT> Eigen::Vector3f
239 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
246 std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
248 for (std::size_t j = 0; j < n_pts; j++)
250 double kernel = (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
251 Eigen::Vector3f vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[k_ind_[j]].z);
252 wgh_sum += vote_vec *
static_cast<float> (
kernel);
255 assert (denom > 0.0);
257 return (wgh_sum /
static_cast<float> (denom));
261 template <
typename Po
intT>
double
263 const PointT &point,
double sigma_dist)
267 const std::size_t n_vote_classes = votes_class_.size ();
268 if (n_vote_classes == 0)
271 double sum_vote = 0.0;
277 std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
279 for (std::size_t j = 0; j < num_of_pts; j++)
280 sum_vote += (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
286 template <
typename Po
intT>
unsigned int
289 return (
static_cast<unsigned int> (votes_->size ()));
305 std::vector<float> vec;
312 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
316 this->classes_.resize (this->number_of_visual_words_, 0);
320 this->sigmas_.resize (this->number_of_classes_, 0.0f);
324 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
326 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
345 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
348 output_file.close ();
352 output_file << number_of_classes_ <<
" ";
353 output_file << number_of_visual_words_ <<
" ";
354 output_file << number_of_clusters_ <<
" ";
355 output_file << descriptors_dimension_ <<
" ";
358 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
359 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
360 output_file << statistical_weights_[i_class][i_cluster] <<
" ";
363 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
364 output_file << learned_weights_[i_visual_word] <<
" ";
367 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
368 output_file << classes_[i_visual_word] <<
" ";
371 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
372 output_file << sigmas_[i_class] <<
" ";
375 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
376 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
377 output_file << directions_to_center_ (i_visual_word, i_dim) <<
" ";
380 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
381 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
382 output_file << clusters_centers_ (i_cluster, i_dim) <<
" ";
385 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
387 output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) <<
" ";
388 for (
const unsigned int &visual_word : clusters_[i_cluster])
389 output_file << visual_word <<
" ";
392 output_file.close ();
401 std::ifstream input_file (file_name.c_str ());
410 input_file.getline (line, 256,
' ');
411 number_of_classes_ =
static_cast<unsigned int> (strtol (line,
nullptr, 10));
412 input_file.getline (line, 256,
' '); number_of_visual_words_ = atoi (line);
413 input_file.getline (line, 256,
' '); number_of_clusters_ = atoi (line);
414 input_file.getline (line, 256,
' '); descriptors_dimension_ = atoi (line);
417 std::vector<float> vec;
418 vec.resize (number_of_clusters_, 0.0f);
419 statistical_weights_.resize (number_of_classes_, vec);
420 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
421 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
422 input_file >> statistical_weights_[i_class][i_cluster];
425 learned_weights_.resize (number_of_visual_words_, 0.0f);
426 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
427 input_file >> learned_weights_[i_visual_word];
430 classes_.resize (number_of_visual_words_, 0);
431 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
432 input_file >> classes_[i_visual_word];
435 sigmas_.resize (number_of_classes_, 0.0f);
436 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
437 input_file >> sigmas_[i_class];
440 directions_to_center_.resize (number_of_visual_words_, 3);
441 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
442 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
443 input_file >> directions_to_center_ (i_visual_word, i_dim);
446 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
447 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
448 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
449 input_file >> clusters_centers_ (i_cluster, i_dim);
452 std::vector<unsigned int> vect;
453 clusters_.resize (number_of_clusters_, vect);
454 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
456 unsigned int size_of_current_cluster = 0;
457 input_file >> size_of_current_cluster;
458 clusters_[i_cluster].resize (size_of_current_cluster, 0);
459 for (
unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
460 input_file >> clusters_[i_cluster][i_visual_word];
471 statistical_weights_.clear ();
472 learned_weights_.clear ();
475 directions_to_center_.resize (0, 0);
476 clusters_centers_.resize (0, 0);
478 number_of_classes_ = 0;
479 number_of_visual_words_ = 0;
480 number_of_clusters_ = 0;
481 descriptors_dimension_ = 0;
497 std::vector<float> vec;
498 vec.resize (number_of_clusters_, 0.0f);
499 this->statistical_weights_.resize (this->number_of_classes_, vec);
500 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
501 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
502 this->statistical_weights_[i_class][i_cluster] = other.
statistical_weights_[i_class][i_cluster];
504 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
505 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
506 this->learned_weights_[i_visual_word] = other.
learned_weights_[i_visual_word];
508 this->classes_.resize (this->number_of_visual_words_, 0);
509 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
510 this->classes_[i_visual_word] = other.
classes_[i_visual_word];
512 this->sigmas_.resize (this->number_of_classes_, 0.0f);
513 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
514 this->sigmas_[i_class] = other.
sigmas_[i_class];
516 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
517 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
518 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
519 this->directions_to_center_ (i_visual_word, i_dim) = other.
directions_to_center_ (i_visual_word, i_dim);
521 this->clusters_centers_.resize (this->number_of_clusters_, 3);
522 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
523 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
524 this->clusters_centers_ (i_cluster, i_dim) = other.
clusters_centers_ (i_cluster, i_dim);
530 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
534 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
537 training_clouds_.clear ();
538 training_classes_.clear ();
539 training_normals_.clear ();
540 training_sigmas_.clear ();
541 feature_estimator_.reset ();
545 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
548 return (training_clouds_);
552 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
556 training_clouds_.clear ();
557 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
558 training_clouds_.swap (clouds);
562 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
565 return (training_classes_);
569 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
572 training_classes_.clear ();
573 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
574 training_classes_.swap (classes);
578 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
581 return (training_normals_);
585 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
589 training_normals_.clear ();
590 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
591 training_normals_.swap (normals);
595 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
598 return (sampling_size_);
602 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
605 if (sampling_size >= std::numeric_limits<float>::epsilon ())
606 sampling_size_ = sampling_size;
613 return (feature_estimator_);
617 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
620 feature_estimator_ = feature;
624 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int
627 return (number_of_clusters_);
631 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
634 if (num_of_clusters > 0)
635 number_of_clusters_ = num_of_clusters;
639 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
642 return (training_sigmas_);
646 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
649 training_sigmas_.clear ();
650 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
651 training_sigmas_.swap (sigmas);
655 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
662 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
669 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
674 if (trained_model ==
nullptr)
676 trained_model->reset ();
678 std::vector<pcl::Histogram<FeatureSize> > histograms;
679 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
680 success = extractDescriptors (histograms, locations);
684 Eigen::MatrixXi labels;
685 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
689 std::vector<unsigned int> vec;
690 trained_model->clusters_.resize (number_of_clusters_, vec);
691 for (std::size_t i_label = 0; i_label < locations.size (); i_label++)
692 trained_model->clusters_[labels (i_label)].push_back (i_label);
694 calculateSigmas (trained_model->sigmas_);
699 trained_model->sigmas_,
700 trained_model->clusters_,
701 trained_model->statistical_weights_,
702 trained_model->learned_weights_);
704 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
705 trained_model->number_of_visual_words_ =
static_cast<unsigned int> (histograms.size ());
706 trained_model->number_of_clusters_ = number_of_clusters_;
707 trained_model->descriptors_dimension_ = FeatureSize;
709 trained_model->directions_to_center_.resize (locations.size (), 3);
710 trained_model->classes_.resize (locations.size ());
711 for (std::size_t i_dir = 0; i_dir < locations.size (); i_dir++)
713 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
714 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
715 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
716 trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
728 int in_class_of_interest)
732 if (in_cloud->
points.empty ())
737 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
738 if (sampled_point_cloud->
points.empty ())
742 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
745 const auto n_key_points =
static_cast<unsigned int> (sampled_point_cloud->
size ());
746 std::vector<int> min_dist_inds (n_key_points, -1);
747 for (
unsigned int i_point = 0; i_point < n_key_points; i_point++)
749 Eigen::VectorXf curr_descriptor (FeatureSize);
750 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
751 curr_descriptor (i_dim) = (*feature_cloud)[i_point].histogram[i_dim];
753 float descriptor_sum = curr_descriptor.sum ();
754 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
757 unsigned int min_dist_idx = 0;
758 Eigen::VectorXf clusters_center (FeatureSize);
759 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
760 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
762 float best_dist = computeDistance (curr_descriptor, clusters_center);
763 for (
unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
765 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
766 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
767 float curr_dist = computeDistance (clusters_center, curr_descriptor);
768 if (curr_dist < best_dist)
770 min_dist_idx = i_clust_cent;
771 best_dist = curr_dist;
774 min_dist_inds[i_point] = min_dist_idx;
777 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
779 int min_dist_idx = min_dist_inds[i_point];
780 if (min_dist_idx == -1)
783 const auto n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
785 Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
786 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
788 unsigned int index = model->clusters_[min_dist_idx][i_word];
789 unsigned int i_class = model->classes_[index];
790 if (
static_cast<int> (i_class) != in_class_of_interest)
794 Eigen::Vector3f direction (
795 model->directions_to_center_(index, 0),
796 model->directions_to_center_(index, 1),
797 model->directions_to_center_(index, 2));
798 applyTransform (direction, transform.transpose ());
801 Eigen::Vector3f vote_pos = (*sampled_point_cloud)[i_point].getVector3fMap () + direction;
802 vote.x = vote_pos[0];
803 vote.y = vote_pos[1];
804 vote.z = vote_pos[2];
805 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
806 float learned_weight = model->learned_weights_[index];
807 float power = statistical_weight * learned_weight;
809 if (vote.
strength > std::numeric_limits<float>::epsilon ())
810 out_votes->
addVote (vote, (*sampled_point_cloud)[i_point], i_class);
818 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
821 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
826 if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ ==
nullptr)
829 for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
832 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
833 const auto num_of_points = training_clouds_[i_cloud]->size ();
834 for (
auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
835 models_center += point_j->getVector3fMap ();
836 models_center /=
static_cast<float> (num_of_points);
841 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
842 if (sampled_point_cloud->
points.empty ())
845 shiftCloud (training_clouds_[i_cloud], models_center);
846 shiftCloud (sampled_point_cloud, models_center);
849 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
852 for (
auto point_i = sampled_point_cloud->
points.cbegin (); point_i != sampled_point_cloud->
points.cend (); point_i++, point_index++)
854 float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
855 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
858 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
860 int dist =
static_cast<int> (
std::distance (sampled_point_cloud->
points.cbegin (), point_i));
861 Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
862 Eigen::Vector3f zero;
866 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
867 applyTransform (new_dir, new_basis);
869 PointT point (new_dir[0], new_dir[1], new_dir[2]);
870 LocationInfo info (
static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
871 locations.insert(locations.end (), info);
879 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
882 Eigen::MatrixXi& labels,
883 Eigen::MatrixXf& clusters_centers)
885 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
887 for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
888 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
889 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
891 labels.resize (histograms.size(), 1);
892 computeKMeansClustering (
896 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
905 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
908 if (!training_sigmas_.empty ())
910 sigmas.resize (training_sigmas_.size (), 0.0f);
911 for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
912 sigmas[i_sigma] = training_sigmas_[i_sigma];
917 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
918 sigmas.resize (number_of_classes, 0.0f);
920 std::vector<float> vec;
921 std::vector<std::vector<float> > objects_sigmas;
922 objects_sigmas.resize (number_of_classes, vec);
924 auto number_of_objects =
static_cast<unsigned int> (training_clouds_.size ());
925 for (
unsigned int i_object = 0; i_object < number_of_objects; i_object++)
927 float max_distance = 0.0f;
928 const auto number_of_points = training_clouds_[i_object]->size ();
929 for (
unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
930 for (
unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
932 float curr_distance = 0.0f;
933 curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
934 curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
935 curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
936 if (curr_distance > max_distance)
937 max_distance = curr_distance;
939 max_distance =
static_cast<float> (std::sqrt (max_distance));
940 unsigned int i_class = training_classes_[i_object];
941 objects_sigmas[i_class].push_back (max_distance);
944 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
947 auto number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
948 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
949 sig += objects_sigmas[i_class][i_object];
950 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
951 sigmas[i_class] = sig;
956 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
958 const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
959 const Eigen::MatrixXi &labels,
960 std::vector<float>& sigmas,
961 std::vector<std::vector<unsigned int> >& clusters,
962 std::vector<std::vector<float> >& statistical_weights,
963 std::vector<float>& learned_weights)
965 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
967 std::vector<float> vec;
968 vec.resize (number_of_clusters_, 0.0f);
969 statistical_weights.clear ();
970 learned_weights.clear ();
971 statistical_weights.resize (number_of_classes, vec);
972 learned_weights.resize (locations.size (), 0.0f);
975 std::vector<int> vect;
976 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
979 std::vector<int> n_ftr;
982 std::vector<int> n_vot;
985 std::vector<int> n_vw;
988 std::vector<std::vector<int> > n_vot_2;
990 n_vot_2.resize (number_of_clusters_, vect);
991 n_vot.resize (number_of_clusters_, 0);
992 n_ftr.resize (number_of_classes, 0);
993 for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
995 int i_class = training_classes_[locations[i_location].model_num_];
996 int i_cluster = labels (i_location);
997 n_vot_2[i_cluster][i_class] += 1;
998 n_vot[i_cluster] += 1;
1002 n_vw.resize (number_of_classes, 0);
1003 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1004 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1005 if (n_vot_2[i_cluster][i_class] > 0)
1009 learned_weights.resize (locations.size (), 0.0);
1010 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1012 auto number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1013 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1015 unsigned int i_index = clusters[i_cluster][i_visual_word];
1016 int i_class = training_classes_[locations[i_index].model_num_];
1017 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1018 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1020 std::vector<float> calculated_sigmas;
1021 calculateSigmas (calculated_sigmas);
1022 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1023 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1026 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1027 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1028 applyTransform (direction, transform);
1029 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1032 std::vector<float> gauss_dists;
1033 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1035 unsigned int j_index = clusters[i_cluster][j_visual_word];
1036 int j_class = training_classes_[locations[j_index].model_num_];
1037 if (i_class != j_class)
1040 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1041 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1042 applyTransform (direction_2, transform_2);
1043 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1044 float residual = (predicted_center - actual_center).norm ();
1045 float value = -residual * residual / square_sigma_dist;
1046 gauss_dists.push_back (
static_cast<float> (std::exp (value)));
1049 std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1050 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1051 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1056 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1058 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1060 if (n_vot_2[i_cluster][i_class] == 0)
1062 if (n_vw[i_class] == 0)
1064 if (n_vot[i_cluster] == 0)
1066 if (n_ftr[i_class] == 0)
1068 float part_1 =
static_cast<float> (n_vw[i_class]);
1069 float part_2 =
static_cast<float> (n_vot[i_cluster]);
1070 float part_3 =
static_cast<float> (n_vot_2[i_cluster][i_class]) /
static_cast<float> (n_ftr[i_class]);
1071 float part_4 = 0.0f;
1076 for (
unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1077 if (n_ftr[j_class] != 0)
1078 part_4 +=
static_cast<float> (n_vot_2[i_cluster][j_class]) /
static_cast<float> (n_ftr[j_class]);
1080 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1086 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1095 grid.
setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1100 grid.
filter (temp_cloud);
1103 constexpr
float max_value = std::numeric_limits<float>::max ();
1105 const auto num_source_points = in_point_cloud->
size ();
1106 const auto num_sample_points = temp_cloud.
size ();
1108 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1109 std::vector<int> sampling_indices (num_sample_points, -1);
1111 for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1117 PointT pt_1 = (*in_point_cloud)[i_point];
1118 PointT pt_2 = temp_cloud[index];
1120 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);
1121 if (
distance < dist_to_grid_center[index])
1123 dist_to_grid_center[index] =
distance;
1124 sampling_indices[index] =
static_cast<int> (i_point);
1133 final_inliers_indices->indices.reserve (num_sample_points);
1134 for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1136 if (sampling_indices[i_point] != -1)
1137 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1141 extract_points.
setIndices (final_inliers_indices);
1142 extract_points.
filter (*out_sampled_point_cloud);
1145 extract_normals.
setIndices (final_inliers_indices);
1146 extract_normals.
filter (*out_sampled_normal_cloud);
1150 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1153 Eigen::Vector3f shift_point)
1155 for (
auto point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1157 point_it->x -= shift_point.x ();
1158 point_it->y -= shift_point.y ();
1159 point_it->z -= shift_point.z ();
1164 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1167 Eigen::Matrix3f result;
1168 Eigen::Matrix3f rotation_matrix_X;
1169 Eigen::Matrix3f rotation_matrix_Z;
1175 float denom_X =
static_cast<float> (std::sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1176 A = in_normal.normal_y / denom_X;
1177 B = sign * in_normal.normal_z / denom_X;
1178 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1182 float denom_Z =
static_cast<float> (std::sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1183 A = in_normal.normal_y / denom_Z;
1184 B = sign * in_normal.normal_x / denom_Z;
1185 rotation_matrix_Z << A, -
B, 0.0f,
1189 result.noalias() = rotation_matrix_X * rotation_matrix_Z;
1195 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1198 io_vec = in_transform * io_vec;
1202 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1211 feature_estimator_->setInputCloud (sampled_point_cloud->
makeShared ());
1213 feature_estimator_->setSearchMethod (tree);
1220 dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1223 feature_estimator_->compute (*feature_cloud);
1227 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double
1229 const Eigen::MatrixXf& points_to_cluster,
1230 int number_of_clusters,
1231 Eigen::MatrixXi& io_labels,
1235 Eigen::MatrixXf& cluster_centers)
1237 constexpr
int spp_trials = 3;
1238 std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1239 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1241 attempts = std::max (attempts, 1);
1242 srand (
static_cast<unsigned int> (time (
nullptr)));
1244 Eigen::MatrixXi labels (number_of_points, 1);
1246 if (flags & USE_INITIAL_LABELS)
1251 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1252 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1253 std::vector<int> counters (number_of_clusters);
1254 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1255 Eigen::Vector2f* box = boxes.data();
1257 double best_compactness = std::numeric_limits<double>::max ();
1258 double compactness = 0.0;
1260 if (criteria.
type_ & TermCriteria::EPS)
1263 criteria.
epsilon_ = std::numeric_limits<float>::epsilon ();
1267 if (criteria.
type_ & TermCriteria::COUNT)
1272 if (number_of_clusters == 1)
1278 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1279 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1281 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1282 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1284 float v = points_to_cluster (i_point, i_dim);
1285 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1286 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1289 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
1291 float max_center_shift = std::numeric_limits<float>::max ();
1292 for (
int iter = 0; iter < criteria.
max_count_ && max_center_shift > criteria.
epsilon_; iter++)
1294 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1296 centers = old_centers;
1299 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1301 if (flags & PP_CENTERS)
1302 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1305 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1307 Eigen::VectorXf center (feature_dimension);
1308 generateRandomCenter (boxes, center);
1309 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1310 centers (i_cl_center, i_dim) = center (i_dim);
1317 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1318 counters[i_cluster] = 0;
1319 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1321 int i_label = labels (i_point, 0);
1322 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1323 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1324 counters[i_label]++;
1327 max_center_shift = 0.0f;
1328 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1330 if (counters[i_cl_center] != 0)
1332 float scale = 1.0f /
static_cast<float> (counters[i_cl_center]);
1333 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1334 centers (i_cl_center, i_dim) *= scale;
1338 Eigen::VectorXf center (feature_dimension);
1339 generateRandomCenter (boxes, center);
1340 for(
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1341 centers (i_cl_center, i_dim) = center (i_dim);
1347 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1349 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1350 dist += diff * diff;
1352 max_center_shift = std::max (max_center_shift, dist);
1357 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1359 Eigen::VectorXf sample (feature_dimension);
1360 sample = points_to_cluster.row (i_point);
1363 float min_dist = std::numeric_limits<float>::max ();
1365 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1367 Eigen::VectorXf center (feature_dimension);
1368 center = centers.row (i_cluster);
1369 float dist = computeDistance (sample, center);
1370 if (min_dist > dist)
1376 compactness += min_dist;
1377 labels (i_point, 0) = k_best;
1381 if (compactness < best_compactness)
1383 best_compactness = compactness;
1384 cluster_centers = centers;
1389 return (best_compactness);
1393 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1395 const Eigen::MatrixXf& data,
1396 Eigen::MatrixXf& out_centers,
1397 int number_of_clusters,
1400 std::size_t dimension = data.cols ();
1401 auto number_of_points =
static_cast<unsigned int> (data.rows ());
1402 std::vector<int> centers_vec (number_of_clusters);
1403 int* centers = centers_vec.data();
1404 std::vector<double> dist (number_of_points);
1405 std::vector<double> tdist (number_of_points);
1406 std::vector<double> tdist2 (number_of_points);
1409 unsigned int random_unsigned = rand ();
1410 centers[0] = random_unsigned % number_of_points;
1412 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1414 Eigen::VectorXf first (dimension);
1415 Eigen::VectorXf second (dimension);
1416 first = data.row (i_point);
1417 second = data.row (centers[0]);
1418 dist[i_point] = computeDistance (first, second);
1419 sum0 += dist[i_point];
1422 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1424 double best_sum = std::numeric_limits<double>::max ();
1425 int best_center = -1;
1426 for (
int i_trials = 0; i_trials < trials; i_trials++)
1428 unsigned int random_integer = rand () - 1;
1429 double random_double =
static_cast<double> (random_integer) /
static_cast<double> (std::numeric_limits<unsigned int>::max ());
1430 double p = random_double * sum0;
1432 unsigned int i_point;
1433 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1434 if ( (p -= dist[i_point]) <= 0.0)
1440 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1442 Eigen::VectorXf first (dimension);
1443 Eigen::VectorXf second (dimension);
1444 first = data.row (i_point);
1445 second = data.row (ci);
1446 tdist2[i_point] = std::min (
static_cast<double> (computeDistance (first, second)), dist[i_point]);
1447 s += tdist2[i_point];
1454 std::swap (tdist, tdist2);
1458 centers[i_cluster] = best_center;
1460 std::swap (dist, tdist);
1463 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1464 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1465 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1469 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1471 Eigen::VectorXf& center)
1473 std::size_t dimension = boxes.size ();
1474 float margin = 1.0f /
static_cast<float> (dimension);
1476 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1478 unsigned int random_integer = rand () - 1;
1479 float random_float =
static_cast<float> (random_integer) /
static_cast<float> (std::numeric_limits<unsigned int>::max ());
1480 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1485 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
1488 std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1490 for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1492 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.
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.