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>
47 #include <pcl/search/kdtree.h>
52 template <
typename Po
intT>
56 template <
typename Po
intT>
59 votes_class_.clear ();
60 votes_origins_.reset ();
68 template <
typename Po
intT>
void
72 tree_is_valid_ =
false;
73 votes_->points.insert (votes_->points.end (), vote);
75 votes_origins_->points.push_back (vote_origin);
76 votes_class_.push_back (votes_class);
86 colored_cloud->
width = 1;
94 for (
const auto& i_point: *cloud)
99 colored_cloud->
points.push_back (point);
106 for (
const auto &i_vote : votes_->points)
111 colored_cloud->
points.push_back (point);
113 colored_cloud->
height += votes_->size ();
115 return (colored_cloud);
119 template <
typename Po
intT>
void
121 std::vector<
pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
123 double in_non_maxima_radius,
128 const std::size_t n_vote_classes = votes_class_.size ();
129 if (n_vote_classes == 0)
131 for (std::size_t i = 0; i < n_vote_classes ; i++)
132 assert ( votes_class_[i] == in_class_id );
136 constexpr
int NUM_INIT_PTS = 100;
137 double SIGMA_DIST = in_sigma;
138 const double FINAL_EPS = SIGMA_DIST / 100;
140 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
141 std::vector<double> peak_densities (NUM_INIT_PTS);
142 double max_density = -1.0;
143 for (
int i = 0; i < NUM_INIT_PTS; i++)
145 Eigen::Vector3f old_center;
146 const auto idx = votes_->size() * i / NUM_INIT_PTS;
147 Eigen::Vector3f curr_center = (*votes_)[idx].getVector3fMap();
151 old_center = curr_center;
152 curr_center = shiftMean (old_center, SIGMA_DIST);
153 }
while ((old_center - curr_center).norm () > FINAL_EPS);
156 point.x = curr_center (0);
157 point.y = curr_center (1);
158 point.z = curr_center (2);
159 double curr_density = getDensityAtPoint (point, SIGMA_DIST);
160 assert (curr_density >= 0.0);
162 peaks[i] = curr_center;
163 peak_densities[i] = curr_density;
165 if ( max_density < curr_density )
166 max_density = curr_density;
170 std::vector<bool> peak_flag (NUM_INIT_PTS,
true);
171 for (
int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
174 double best_density = -1.0;
175 Eigen::Vector3f strongest_peak = Eigen::Vector3f::Constant (-1);
176 int best_peak_ind (-1);
177 int peak_counter (0);
178 for (
int i = 0; i < NUM_INIT_PTS; i++)
183 if ( peak_densities[i] > best_density)
185 best_density = peak_densities[i];
186 strongest_peak = peaks[i];
192 if( best_density == -1.0 || strongest_peak == Eigen::Vector3f::Constant (-1) ||
193 best_peak_ind == -1 || peak_counter == 0 )
197 peak.x = strongest_peak(0);
198 peak.y = strongest_peak(1);
199 peak.z = strongest_peak(2);
202 out_peaks.push_back ( peak );
205 peak_flag[best_peak_ind] =
false;
206 for (
int i = 0; i < NUM_INIT_PTS; i++)
212 double dist = (strongest_peak - peaks[i]).norm ();
213 if ( dist < in_non_maxima_radius )
214 peak_flag[i] =
false;
220 template <
typename Po
intT>
void
225 if (tree_ ==
nullptr)
227 tree_->setInputCloud (votes_);
228 k_ind_.resize ( votes_->size (), -1 );
229 k_sqr_dist_.resize ( votes_->size (), 0.0f );
230 tree_is_valid_ =
true;
235 template <
typename Po
intT> Eigen::Vector3f
240 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
247 std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
249 for (std::size_t j = 0; j < n_pts; j++)
251 double kernel = (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
252 Eigen::Vector3f vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[k_ind_[j]].z);
253 wgh_sum += vote_vec *
static_cast<float> (
kernel);
256 assert (denom > 0.0);
258 return (wgh_sum /
static_cast<float> (denom));
262 template <
typename Po
intT>
double
264 const PointT &point,
double sigma_dist)
268 const std::size_t n_vote_classes = votes_class_.size ();
269 if (n_vote_classes == 0)
272 double sum_vote = 0.0;
278 std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
280 for (std::size_t j = 0; j < num_of_pts; j++)
281 sum_vote += (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
287 template <
typename Po
intT>
unsigned int
290 return (
static_cast<unsigned int> (votes_->size ()));
306 std::vector<float> vec;
313 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
317 this->classes_.resize (this->number_of_visual_words_, 0);
321 this->sigmas_.resize (this->number_of_classes_, 0.0f);
325 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
327 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
346 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
349 output_file.close ();
353 output_file << number_of_classes_ <<
" ";
354 output_file << number_of_visual_words_ <<
" ";
355 output_file << number_of_clusters_ <<
" ";
356 output_file << descriptors_dimension_ <<
" ";
359 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
360 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
361 output_file << statistical_weights_[i_class][i_cluster] <<
" ";
364 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
365 output_file << learned_weights_[i_visual_word] <<
" ";
368 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
369 output_file << classes_[i_visual_word] <<
" ";
372 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
373 output_file << sigmas_[i_class] <<
" ";
376 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
377 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
378 output_file << directions_to_center_ (i_visual_word, i_dim) <<
" ";
381 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
382 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
383 output_file << clusters_centers_ (i_cluster, i_dim) <<
" ";
386 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
388 output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) <<
" ";
389 for (
const unsigned int &visual_word : clusters_[i_cluster])
390 output_file << visual_word <<
" ";
393 output_file.close ();
402 std::ifstream input_file (file_name.c_str ());
411 input_file.getline (line, 256,
' ');
412 number_of_classes_ =
static_cast<unsigned int> (strtol (line,
nullptr, 10));
413 input_file.getline (line, 256,
' '); number_of_visual_words_ = atoi (line);
414 input_file.getline (line, 256,
' '); number_of_clusters_ = atoi (line);
415 input_file.getline (line, 256,
' '); descriptors_dimension_ = atoi (line);
418 std::vector<float> vec;
419 vec.resize (number_of_clusters_, 0.0f);
420 statistical_weights_.resize (number_of_classes_, vec);
421 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
422 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
423 input_file >> statistical_weights_[i_class][i_cluster];
426 learned_weights_.resize (number_of_visual_words_, 0.0f);
427 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
428 input_file >> learned_weights_[i_visual_word];
431 classes_.resize (number_of_visual_words_, 0);
432 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
433 input_file >> classes_[i_visual_word];
436 sigmas_.resize (number_of_classes_, 0.0f);
437 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
438 input_file >> sigmas_[i_class];
441 directions_to_center_.resize (number_of_visual_words_, 3);
442 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
443 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
444 input_file >> directions_to_center_ (i_visual_word, i_dim);
447 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
448 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
449 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
450 input_file >> clusters_centers_ (i_cluster, i_dim);
453 std::vector<unsigned int> vect;
454 clusters_.resize (number_of_clusters_, vect);
455 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
457 unsigned int size_of_current_cluster = 0;
458 input_file >> size_of_current_cluster;
459 clusters_[i_cluster].resize (size_of_current_cluster, 0);
460 for (
unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
461 input_file >> clusters_[i_cluster][i_visual_word];
472 statistical_weights_.clear ();
473 learned_weights_.clear ();
476 directions_to_center_.resize (0, 0);
477 clusters_centers_.resize (0, 0);
479 number_of_classes_ = 0;
480 number_of_visual_words_ = 0;
481 number_of_clusters_ = 0;
482 descriptors_dimension_ = 0;
498 std::vector<float> vec;
499 vec.resize (number_of_clusters_, 0.0f);
500 this->statistical_weights_.resize (this->number_of_classes_, vec);
501 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
502 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
503 this->statistical_weights_[i_class][i_cluster] = other.
statistical_weights_[i_class][i_cluster];
505 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
506 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
507 this->learned_weights_[i_visual_word] = other.
learned_weights_[i_visual_word];
509 this->classes_.resize (this->number_of_visual_words_, 0);
510 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
511 this->classes_[i_visual_word] = other.
classes_[i_visual_word];
513 this->sigmas_.resize (this->number_of_classes_, 0.0f);
514 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
515 this->sigmas_[i_class] = other.
sigmas_[i_class];
517 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
518 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
519 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
520 this->directions_to_center_ (i_visual_word, i_dim) = other.
directions_to_center_ (i_visual_word, i_dim);
522 this->clusters_centers_.resize (this->number_of_clusters_, 3);
523 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
524 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
525 this->clusters_centers_ (i_cluster, i_dim) = other.
clusters_centers_ (i_cluster, i_dim);
531 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
535 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
538 training_clouds_.clear ();
539 training_classes_.clear ();
540 training_normals_.clear ();
541 training_sigmas_.clear ();
542 feature_estimator_.reset ();
546 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
549 return (training_clouds_);
553 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
557 training_clouds_.clear ();
558 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
559 training_clouds_.swap (clouds);
563 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
566 return (training_classes_);
570 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
573 training_classes_.clear ();
574 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
575 training_classes_.swap (classes);
579 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
582 return (training_normals_);
586 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
590 training_normals_.clear ();
591 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
592 training_normals_.swap (normals);
596 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
599 return (sampling_size_);
603 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
606 if (sampling_size >= std::numeric_limits<float>::epsilon ())
607 sampling_size_ = sampling_size;
614 return (feature_estimator_);
618 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
621 feature_estimator_ = feature;
625 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int
628 return (number_of_clusters_);
632 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
635 if (num_of_clusters > 0)
636 number_of_clusters_ = num_of_clusters;
640 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
643 return (training_sigmas_);
647 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
650 training_sigmas_.clear ();
651 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
652 training_sigmas_.swap (sigmas);
656 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
663 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
670 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
675 if (trained_model ==
nullptr)
677 trained_model->reset ();
679 std::vector<pcl::Histogram<FeatureSize> > histograms;
680 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
681 success = extractDescriptors (histograms, locations);
685 Eigen::MatrixXi labels;
686 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
690 std::vector<unsigned int> vec;
691 trained_model->clusters_.resize (number_of_clusters_, vec);
692 for (std::size_t i_label = 0; i_label < locations.size (); i_label++)
693 trained_model->clusters_[labels (i_label)].push_back (i_label);
695 calculateSigmas (trained_model->sigmas_);
700 trained_model->sigmas_,
701 trained_model->clusters_,
702 trained_model->statistical_weights_,
703 trained_model->learned_weights_);
705 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
706 trained_model->number_of_visual_words_ =
static_cast<unsigned int> (histograms.size ());
707 trained_model->number_of_clusters_ = number_of_clusters_;
708 trained_model->descriptors_dimension_ = FeatureSize;
710 trained_model->directions_to_center_.resize (locations.size (), 3);
711 trained_model->classes_.resize (locations.size ());
712 for (std::size_t i_dir = 0; i_dir < locations.size (); i_dir++)
714 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
715 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
716 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
717 trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
729 int in_class_of_interest)
733 if (in_cloud->
points.empty ())
738 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
739 if (sampled_point_cloud->
points.empty ())
743 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
746 const auto n_key_points =
static_cast<unsigned int> (sampled_point_cloud->
size ());
747 std::vector<int> min_dist_inds (n_key_points, -1);
748 for (
unsigned int i_point = 0; i_point < n_key_points; i_point++)
750 Eigen::VectorXf curr_descriptor (FeatureSize);
751 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
752 curr_descriptor (i_dim) = (*feature_cloud)[i_point].histogram[i_dim];
754 float descriptor_sum = curr_descriptor.sum ();
755 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
758 unsigned int min_dist_idx = 0;
759 Eigen::VectorXf clusters_center (FeatureSize);
760 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
761 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
763 float best_dist = computeDistance (curr_descriptor, clusters_center);
764 for (
unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
766 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
767 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
768 float curr_dist = computeDistance (clusters_center, curr_descriptor);
769 if (curr_dist < best_dist)
771 min_dist_idx = i_clust_cent;
772 best_dist = curr_dist;
775 min_dist_inds[i_point] = min_dist_idx;
778 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
780 int min_dist_idx = min_dist_inds[i_point];
781 if (min_dist_idx == -1)
784 const auto n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
786 Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
787 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
789 unsigned int index = model->clusters_[min_dist_idx][i_word];
790 unsigned int i_class = model->classes_[index];
791 if (
static_cast<int> (i_class) != in_class_of_interest)
795 Eigen::Vector3f direction (
796 model->directions_to_center_(index, 0),
797 model->directions_to_center_(index, 1),
798 model->directions_to_center_(index, 2));
799 applyTransform (direction, transform.transpose ());
802 Eigen::Vector3f vote_pos = (*sampled_point_cloud)[i_point].getVector3fMap () + direction;
803 vote.x = vote_pos[0];
804 vote.y = vote_pos[1];
805 vote.z = vote_pos[2];
806 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
807 float learned_weight = model->learned_weights_[index];
808 float power = statistical_weight * learned_weight;
810 if (vote.
strength > std::numeric_limits<float>::epsilon ())
811 out_votes->
addVote (vote, (*sampled_point_cloud)[i_point], i_class);
819 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
822 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
827 if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ ==
nullptr)
830 for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
833 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
834 const auto num_of_points = training_clouds_[i_cloud]->size ();
835 for (
auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
836 models_center += point_j->getVector3fMap ();
837 models_center /=
static_cast<float> (num_of_points);
842 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
843 if (sampled_point_cloud->
points.empty ())
846 shiftCloud (training_clouds_[i_cloud], models_center);
847 shiftCloud (sampled_point_cloud, models_center);
850 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
853 for (
auto point_i = sampled_point_cloud->
points.cbegin (); point_i != sampled_point_cloud->
points.cend (); point_i++, point_index++)
855 float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
856 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
859 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
861 int dist =
static_cast<int> (
std::distance (sampled_point_cloud->
points.cbegin (), point_i));
862 Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
863 Eigen::Vector3f zero;
867 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
868 applyTransform (new_dir, new_basis);
870 PointT point (new_dir[0], new_dir[1], new_dir[2]);
871 LocationInfo info (
static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
872 locations.insert(locations.end (), info);
880 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
883 Eigen::MatrixXi& labels,
884 Eigen::MatrixXf& clusters_centers)
886 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
888 for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
889 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
890 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
892 labels.resize (histograms.size(), 1);
893 computeKMeansClustering (
897 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
906 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
909 if (!training_sigmas_.empty ())
911 sigmas.resize (training_sigmas_.size (), 0.0f);
912 for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
913 sigmas[i_sigma] = training_sigmas_[i_sigma];
918 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
919 sigmas.resize (number_of_classes, 0.0f);
921 std::vector<float> vec;
922 std::vector<std::vector<float> > objects_sigmas;
923 objects_sigmas.resize (number_of_classes, vec);
925 auto number_of_objects =
static_cast<unsigned int> (training_clouds_.size ());
926 for (
unsigned int i_object = 0; i_object < number_of_objects; i_object++)
928 float max_distance = 0.0f;
929 const auto number_of_points = training_clouds_[i_object]->size ();
930 for (
unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
931 for (
unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
933 float curr_distance = 0.0f;
934 curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
935 curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
936 curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
937 if (curr_distance > max_distance)
938 max_distance = curr_distance;
940 max_distance =
static_cast<float> (std::sqrt (max_distance));
941 unsigned int i_class = training_classes_[i_object];
942 objects_sigmas[i_class].push_back (max_distance);
945 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
948 auto number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
949 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
950 sig += objects_sigmas[i_class][i_object];
951 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
952 sigmas[i_class] = sig;
957 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
959 const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
960 const Eigen::MatrixXi &labels,
961 std::vector<float>& sigmas,
962 std::vector<std::vector<unsigned int> >& clusters,
963 std::vector<std::vector<float> >& statistical_weights,
964 std::vector<float>& learned_weights)
966 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
968 std::vector<float> vec;
969 vec.resize (number_of_clusters_, 0.0f);
970 statistical_weights.clear ();
971 learned_weights.clear ();
972 statistical_weights.resize (number_of_classes, vec);
973 learned_weights.resize (locations.size (), 0.0f);
976 std::vector<int> vect;
977 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
980 std::vector<int> n_ftr;
983 std::vector<int> n_vot;
986 std::vector<int> n_vw;
989 std::vector<std::vector<int> > n_vot_2;
991 n_vot_2.resize (number_of_clusters_, vect);
992 n_vot.resize (number_of_clusters_, 0);
993 n_ftr.resize (number_of_classes, 0);
994 for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
996 int i_class = training_classes_[locations[i_location].model_num_];
997 int i_cluster = labels (i_location);
998 n_vot_2[i_cluster][i_class] += 1;
999 n_vot[i_cluster] += 1;
1000 n_ftr[i_class] += 1;
1003 n_vw.resize (number_of_classes, 0);
1004 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1005 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1006 if (n_vot_2[i_cluster][i_class] > 0)
1010 learned_weights.resize (locations.size (), 0.0);
1011 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1013 auto number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1014 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1016 unsigned int i_index = clusters[i_cluster][i_visual_word];
1017 int i_class = training_classes_[locations[i_index].model_num_];
1018 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1019 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1021 std::vector<float> calculated_sigmas;
1022 calculateSigmas (calculated_sigmas);
1023 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1024 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1027 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1028 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1029 applyTransform (direction, transform);
1030 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1033 std::vector<float> gauss_dists;
1034 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1036 unsigned int j_index = clusters[i_cluster][j_visual_word];
1037 int j_class = training_classes_[locations[j_index].model_num_];
1038 if (i_class != j_class)
1041 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1042 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1043 applyTransform (direction_2, transform_2);
1044 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1045 float residual = (predicted_center - actual_center).norm ();
1046 float value = -residual * residual / square_sigma_dist;
1047 gauss_dists.push_back (
static_cast<float> (std::exp (value)));
1050 std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1051 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1052 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1057 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1059 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1061 if (n_vot_2[i_cluster][i_class] == 0)
1063 if (n_vw[i_class] == 0)
1065 if (n_vot[i_cluster] == 0)
1067 if (n_ftr[i_class] == 0)
1069 float part_1 =
static_cast<float> (n_vw[i_class]);
1070 float part_2 =
static_cast<float> (n_vot[i_cluster]);
1071 float part_3 =
static_cast<float> (n_vot_2[i_cluster][i_class]) /
static_cast<float> (n_ftr[i_class]);
1072 float part_4 = 0.0f;
1077 for (
unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1078 if (n_ftr[j_class] != 0)
1079 part_4 +=
static_cast<float> (n_vot_2[i_cluster][j_class]) /
static_cast<float> (n_ftr[j_class]);
1081 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1087 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1096 grid.
setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1101 grid.
filter (temp_cloud);
1104 constexpr
float max_value = std::numeric_limits<float>::max ();
1106 const auto num_source_points = in_point_cloud->
size ();
1107 const auto num_sample_points = temp_cloud.
size ();
1109 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1110 std::vector<int> sampling_indices (num_sample_points, -1);
1112 for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1118 PointT pt_1 = (*in_point_cloud)[i_point];
1119 PointT pt_2 = temp_cloud[index];
1121 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);
1122 if (
distance < dist_to_grid_center[index])
1124 dist_to_grid_center[index] =
distance;
1125 sampling_indices[index] =
static_cast<int> (i_point);
1134 final_inliers_indices->indices.reserve (num_sample_points);
1135 for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1137 if (sampling_indices[i_point] != -1)
1138 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1142 extract_points.
setIndices (final_inliers_indices);
1143 extract_points.
filter (*out_sampled_point_cloud);
1146 extract_normals.
setIndices (final_inliers_indices);
1147 extract_normals.
filter (*out_sampled_normal_cloud);
1151 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1154 Eigen::Vector3f shift_point)
1156 for (
auto point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1158 point_it->x -= shift_point.x ();
1159 point_it->y -= shift_point.y ();
1160 point_it->z -= shift_point.z ();
1165 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1168 Eigen::Matrix3f result;
1169 Eigen::Matrix3f rotation_matrix_X;
1170 Eigen::Matrix3f rotation_matrix_Z;
1176 float denom_X =
static_cast<float> (std::sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1177 A = in_normal.normal_y / denom_X;
1178 B = sign * in_normal.normal_z / denom_X;
1179 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1183 float denom_Z =
static_cast<float> (std::sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1184 A = in_normal.normal_y / denom_Z;
1185 B = sign * in_normal.normal_x / denom_Z;
1186 rotation_matrix_Z << A, -
B, 0.0f,
1190 result.noalias() = rotation_matrix_X * rotation_matrix_Z;
1196 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1199 io_vec = in_transform * io_vec;
1203 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1212 feature_estimator_->setInputCloud (sampled_point_cloud->
makeShared ());
1214 feature_estimator_->setSearchMethod (tree);
1221 dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1224 feature_estimator_->compute (*feature_cloud);
1228 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double
1230 const Eigen::MatrixXf& points_to_cluster,
1231 int number_of_clusters,
1232 Eigen::MatrixXi& io_labels,
1236 Eigen::MatrixXf& cluster_centers)
1238 constexpr
int spp_trials = 3;
1239 std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1240 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1242 attempts = std::max (attempts, 1);
1243 srand (
static_cast<unsigned int> (time (
nullptr)));
1245 Eigen::MatrixXi labels (number_of_points, 1);
1247 if (flags & USE_INITIAL_LABELS)
1252 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1253 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1254 std::vector<int> counters (number_of_clusters);
1255 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1256 Eigen::Vector2f* box = boxes.data();
1258 double best_compactness = std::numeric_limits<double>::max ();
1259 double compactness = 0.0;
1261 if (criteria.
type_ & TermCriteria::EPS)
1264 criteria.
epsilon_ = std::numeric_limits<float>::epsilon ();
1268 if (criteria.
type_ & TermCriteria::COUNT)
1273 if (number_of_clusters == 1)
1279 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1280 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1282 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1283 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1285 float v = points_to_cluster (i_point, i_dim);
1286 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1287 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1290 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
1292 float max_center_shift = std::numeric_limits<float>::max ();
1293 for (
int iter = 0; iter < criteria.
max_count_ && max_center_shift > criteria.
epsilon_; iter++)
1295 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1297 centers = old_centers;
1300 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1302 if (flags & PP_CENTERS)
1303 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1306 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1308 Eigen::VectorXf center (feature_dimension);
1309 generateRandomCenter (boxes, center);
1310 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1311 centers (i_cl_center, i_dim) = center (i_dim);
1318 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1319 counters[i_cluster] = 0;
1320 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1322 int i_label = labels (i_point, 0);
1323 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1324 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1325 counters[i_label]++;
1328 max_center_shift = 0.0f;
1329 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1331 if (counters[i_cl_center] != 0)
1333 float scale = 1.0f /
static_cast<float> (counters[i_cl_center]);
1334 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1335 centers (i_cl_center, i_dim) *= scale;
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);
1348 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1350 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1351 dist += diff * diff;
1353 max_center_shift = std::max (max_center_shift, dist);
1358 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1360 Eigen::VectorXf sample (feature_dimension);
1361 sample = points_to_cluster.row (i_point);
1364 float min_dist = std::numeric_limits<float>::max ();
1366 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1368 Eigen::VectorXf center (feature_dimension);
1369 center = centers.row (i_cluster);
1370 float dist = computeDistance (sample, center);
1371 if (min_dist > dist)
1377 compactness += min_dist;
1378 labels (i_point, 0) = k_best;
1382 if (compactness < best_compactness)
1384 best_compactness = compactness;
1385 cluster_centers = centers;
1390 return (best_compactness);
1394 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1396 const Eigen::MatrixXf& data,
1397 Eigen::MatrixXf& out_centers,
1398 int number_of_clusters,
1401 std::size_t dimension = data.cols ();
1402 auto number_of_points =
static_cast<unsigned int> (data.rows ());
1403 std::vector<int> centers_vec (number_of_clusters);
1404 int* centers = centers_vec.data();
1405 std::vector<double> dist (number_of_points);
1406 std::vector<double> tdist (number_of_points);
1407 std::vector<double> tdist2 (number_of_points);
1410 unsigned int random_unsigned = rand ();
1411 centers[0] = random_unsigned % number_of_points;
1413 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1415 Eigen::VectorXf first (dimension);
1416 Eigen::VectorXf second (dimension);
1417 first = data.row (i_point);
1418 second = data.row (centers[0]);
1419 dist[i_point] = computeDistance (first, second);
1420 sum0 += dist[i_point];
1423 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1425 double best_sum = std::numeric_limits<double>::max ();
1426 int best_center = -1;
1427 for (
int i_trials = 0; i_trials < trials; i_trials++)
1429 unsigned int random_integer = rand () - 1;
1430 double random_double =
static_cast<double> (random_integer) /
static_cast<double> (std::numeric_limits<unsigned int>::max ());
1431 double p = random_double * sum0;
1433 unsigned int i_point;
1434 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1435 if ( (p -= dist[i_point]) <= 0.0)
1441 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1443 Eigen::VectorXf first (dimension);
1444 Eigen::VectorXf second (dimension);
1445 first = data.row (i_point);
1446 second = data.row (ci);
1447 tdist2[i_point] = std::min (
static_cast<double> (computeDistance (first, second)), dist[i_point]);
1448 s += tdist2[i_point];
1455 std::swap (tdist, tdist2);
1459 centers[i_cluster] = best_center;
1461 std::swap (dist, tdist);
1464 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1465 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1466 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1470 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1472 Eigen::VectorXf& center)
1474 std::size_t dimension = boxes.size ();
1475 float margin = 1.0f /
static_cast<float> (dimension);
1477 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1479 unsigned int random_integer = rand () - 1;
1480 float random_float =
static_cast<float> (random_integer) /
static_cast<float> (std::numeric_limits<unsigned int>::max ());
1481 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1486 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
1489 std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1491 for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1493 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.