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>
48 #include <pcl/memory.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;
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( peak_counter == 0 )
195 peak.x = strongest_peak(0);
196 peak.y = strongest_peak(1);
197 peak.z = strongest_peak(2);
200 out_peaks.push_back ( peak );
203 peak_flag[best_peak_ind] =
false;
204 for (
int i = 0; i < NUM_INIT_PTS; i++)
210 double dist = (strongest_peak - peaks[i]).norm ();
211 if ( dist < in_non_maxima_radius )
212 peak_flag[i] =
false;
218 template <
typename Po
intT>
void
223 if (tree_ ==
nullptr)
225 tree_->setInputCloud (votes_);
226 k_ind_.resize ( votes_->size (), -1 );
227 k_sqr_dist_.resize ( votes_->size (), 0.0f );
228 tree_is_valid_ =
true;
233 template <
typename Po
intT> Eigen::Vector3f
238 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
245 std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
247 for (std::size_t j = 0; j < n_pts; j++)
249 double kernel = (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
250 Eigen::Vector3f vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[k_ind_[j]].z);
251 wgh_sum += vote_vec *
static_cast<float> (kernel);
254 assert (denom > 0.0);
256 return (wgh_sum / static_cast<float> (denom));
260 template <
typename Po
intT>
double
262 const PointT &point,
double sigma_dist)
266 const std::size_t n_vote_classes = votes_class_.size ();
267 if (n_vote_classes == 0)
270 double sum_vote = 0.0;
276 std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
278 for (std::size_t j = 0; j < num_of_pts; j++)
279 sum_vote += (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
285 template <
typename Po
intT>
unsigned int
288 return (static_cast<unsigned int> (votes_->size ()));
304 std::vector<float> vec;
311 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
315 this->classes_.resize (this->number_of_visual_words_, 0);
319 this->sigmas_.resize (this->number_of_classes_, 0.0f);
323 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
325 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
344 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
347 output_file.close ();
351 output_file << number_of_classes_ <<
" ";
352 output_file << number_of_visual_words_ <<
" ";
353 output_file << number_of_clusters_ <<
" ";
354 output_file << descriptors_dimension_ <<
" ";
357 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
358 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
359 output_file << statistical_weights_[i_class][i_cluster] <<
" ";
362 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
363 output_file << learned_weights_[i_visual_word] <<
" ";
366 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
367 output_file << classes_[i_visual_word] <<
" ";
370 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
371 output_file << sigmas_[i_class] <<
" ";
374 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
375 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
376 output_file << directions_to_center_ (i_visual_word, i_dim) <<
" ";
379 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
380 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
381 output_file << clusters_centers_ (i_cluster, i_dim) <<
" ";
384 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
386 output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) <<
" ";
387 for (
const unsigned int &visual_word : clusters_[i_cluster])
388 output_file << visual_word <<
" ";
391 output_file.close ();
400 std::ifstream input_file (file_name.c_str ());
409 input_file.getline (line, 256,
' ');
410 number_of_classes_ =
static_cast<unsigned int> (strtol (line,
nullptr, 10));
411 input_file.getline (line, 256,
' '); number_of_visual_words_ = atoi (line);
412 input_file.getline (line, 256,
' '); number_of_clusters_ = atoi (line);
413 input_file.getline (line, 256,
' '); descriptors_dimension_ = atoi (line);
416 std::vector<float> vec;
417 vec.resize (number_of_clusters_, 0.0f);
418 statistical_weights_.resize (number_of_classes_, vec);
419 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
420 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
421 input_file >> statistical_weights_[i_class][i_cluster];
424 learned_weights_.resize (number_of_visual_words_, 0.0f);
425 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
426 input_file >> learned_weights_[i_visual_word];
429 classes_.resize (number_of_visual_words_, 0);
430 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
431 input_file >> classes_[i_visual_word];
434 sigmas_.resize (number_of_classes_, 0.0f);
435 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
436 input_file >> sigmas_[i_class];
439 directions_to_center_.resize (number_of_visual_words_, 3);
440 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
441 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
442 input_file >> directions_to_center_ (i_visual_word, i_dim);
445 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
446 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
447 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
448 input_file >> clusters_centers_ (i_cluster, i_dim);
451 std::vector<unsigned int> vect;
452 clusters_.resize (number_of_clusters_, vect);
453 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
455 unsigned int size_of_current_cluster = 0;
456 input_file >> size_of_current_cluster;
457 clusters_[i_cluster].resize (size_of_current_cluster, 0);
458 for (
unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
459 input_file >> clusters_[i_cluster][i_visual_word];
470 statistical_weights_.clear ();
471 learned_weights_.clear ();
474 directions_to_center_.resize (0, 0);
475 clusters_centers_.resize (0, 0);
477 number_of_classes_ = 0;
478 number_of_visual_words_ = 0;
479 number_of_clusters_ = 0;
480 descriptors_dimension_ = 0;
496 std::vector<float> vec;
497 vec.resize (number_of_clusters_, 0.0f);
498 this->statistical_weights_.resize (this->number_of_classes_, vec);
499 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
500 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
501 this->statistical_weights_[i_class][i_cluster] = other.
statistical_weights_[i_class][i_cluster];
503 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
504 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
505 this->learned_weights_[i_visual_word] = other.
learned_weights_[i_visual_word];
507 this->classes_.resize (this->number_of_visual_words_, 0);
508 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
509 this->classes_[i_visual_word] = other.
classes_[i_visual_word];
511 this->sigmas_.resize (this->number_of_classes_, 0.0f);
512 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
513 this->sigmas_[i_class] = other.
sigmas_[i_class];
515 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
516 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
517 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
518 this->directions_to_center_ (i_visual_word, i_dim) = other.
directions_to_center_ (i_visual_word, i_dim);
520 this->clusters_centers_.resize (this->number_of_clusters_, 3);
521 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
522 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
523 this->clusters_centers_ (i_cluster, i_dim) = other.
clusters_centers_ (i_cluster, i_dim);
529 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
533 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
536 training_clouds_.clear ();
537 training_classes_.clear ();
538 training_normals_.clear ();
539 training_sigmas_.clear ();
540 feature_estimator_.reset ();
544 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
547 return (training_clouds_);
551 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
555 training_clouds_.clear ();
556 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
557 training_clouds_.swap (clouds);
561 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
564 return (training_classes_);
568 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
571 training_classes_.clear ();
572 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
573 training_classes_.swap (classes);
577 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
580 return (training_normals_);
584 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
588 training_normals_.clear ();
589 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
590 training_normals_.swap (normals);
594 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
597 return (sampling_size_);
601 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
604 if (sampling_size >= std::numeric_limits<float>::epsilon ())
605 sampling_size_ = sampling_size;
612 return (feature_estimator_);
616 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
619 feature_estimator_ = feature;
623 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int
626 return (number_of_clusters_);
630 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
633 if (num_of_clusters > 0)
634 number_of_clusters_ = num_of_clusters;
638 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
641 return (training_sigmas_);
645 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
648 training_sigmas_.clear ();
649 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
650 training_sigmas_.swap (sigmas);
654 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
661 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
668 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
673 if (trained_model ==
nullptr)
675 trained_model->reset ();
677 std::vector<pcl::Histogram<FeatureSize> > histograms;
678 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
679 success = extractDescriptors (histograms, locations);
683 Eigen::MatrixXi labels;
684 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
688 std::vector<unsigned int> vec;
689 trained_model->clusters_.resize (number_of_clusters_, vec);
690 for (std::size_t i_label = 0; i_label < locations.size (); i_label++)
691 trained_model->clusters_[labels (i_label)].push_back (i_label);
693 calculateSigmas (trained_model->sigmas_);
698 trained_model->sigmas_,
699 trained_model->clusters_,
700 trained_model->statistical_weights_,
701 trained_model->learned_weights_);
703 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
704 trained_model->number_of_visual_words_ =
static_cast<unsigned int> (histograms.size ());
705 trained_model->number_of_clusters_ = number_of_clusters_;
706 trained_model->descriptors_dimension_ = FeatureSize;
708 trained_model->directions_to_center_.resize (locations.size (), 3);
709 trained_model->classes_.resize (locations.size ());
710 for (std::size_t i_dir = 0; i_dir < locations.size (); i_dir++)
712 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
713 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
714 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
715 trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
727 int in_class_of_interest)
731 if (in_cloud->
points.empty ())
736 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
737 if (sampled_point_cloud->
points.empty ())
741 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
744 const auto n_key_points =
static_cast<unsigned int> (sampled_point_cloud->
size ());
745 std::vector<int> min_dist_inds (n_key_points, -1);
746 for (
unsigned int i_point = 0; i_point < n_key_points; i_point++)
748 Eigen::VectorXf curr_descriptor (FeatureSize);
749 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
750 curr_descriptor (i_dim) = (*feature_cloud)[i_point].histogram[i_dim];
752 float descriptor_sum = curr_descriptor.sum ();
753 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
756 unsigned int min_dist_idx = 0;
757 Eigen::VectorXf clusters_center (FeatureSize);
758 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
759 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
761 float best_dist = computeDistance (curr_descriptor, clusters_center);
762 for (
unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
764 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
765 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
766 float curr_dist = computeDistance (clusters_center, curr_descriptor);
767 if (curr_dist < best_dist)
769 min_dist_idx = i_clust_cent;
770 best_dist = curr_dist;
773 min_dist_inds[i_point] = min_dist_idx;
776 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
778 int min_dist_idx = min_dist_inds[i_point];
779 if (min_dist_idx == -1)
782 const auto n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
784 Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
785 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
787 unsigned int index = model->clusters_[min_dist_idx][i_word];
788 unsigned int i_class = model->classes_[index];
789 if (static_cast<int> (i_class) != in_class_of_interest)
793 Eigen::Vector3f direction (
794 model->directions_to_center_(index, 0),
795 model->directions_to_center_(index, 1),
796 model->directions_to_center_(index, 2));
797 applyTransform (direction, transform.transpose ());
800 Eigen::Vector3f vote_pos = (*sampled_point_cloud)[i_point].getVector3fMap () + direction;
801 vote.x = vote_pos[0];
802 vote.y = vote_pos[1];
803 vote.z = vote_pos[2];
804 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
805 float learned_weight = model->learned_weights_[index];
806 float power = statistical_weight * learned_weight;
808 if (vote.strength > std::numeric_limits<float>::epsilon ())
809 out_votes->
addVote (vote, (*sampled_point_cloud)[i_point], i_class);
817 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
820 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
825 if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ ==
nullptr)
828 for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
831 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
832 const auto num_of_points = training_clouds_[i_cloud]->size ();
833 for (
auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
834 models_center += point_j->getVector3fMap ();
835 models_center /=
static_cast<float> (num_of_points);
840 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
841 if (sampled_point_cloud->
points.empty ())
844 shiftCloud (training_clouds_[i_cloud], models_center);
845 shiftCloud (sampled_point_cloud, models_center);
848 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
851 for (
auto point_i = sampled_point_cloud->
points.cbegin (); point_i != sampled_point_cloud->
points.cend (); point_i++, point_index++)
853 float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
854 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
857 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
859 int dist =
static_cast<int> (std::distance (sampled_point_cloud->
points.cbegin (), point_i));
860 Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
861 Eigen::Vector3f zero;
865 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
866 applyTransform (new_dir, new_basis);
868 PointT point (new_dir[0], new_dir[1], new_dir[2]);
869 LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
870 locations.insert(locations.end (), info);
878 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
881 Eigen::MatrixXi& labels,
882 Eigen::MatrixXf& clusters_centers)
884 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
886 for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
887 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
888 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
890 labels.resize (histograms.size(), 1);
891 computeKMeansClustering (
895 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
904 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
907 if (!training_sigmas_.empty ())
909 sigmas.resize (training_sigmas_.size (), 0.0f);
910 for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
911 sigmas[i_sigma] = training_sigmas_[i_sigma];
916 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
917 sigmas.resize (number_of_classes, 0.0f);
919 std::vector<float> vec;
920 std::vector<std::vector<float> > objects_sigmas;
921 objects_sigmas.resize (number_of_classes, vec);
923 auto number_of_objects =
static_cast<unsigned int> (training_clouds_.size ());
924 for (
unsigned int i_object = 0; i_object < number_of_objects; i_object++)
926 float max_distance = 0.0f;
927 const auto number_of_points = training_clouds_[i_object]->size ();
928 for (
unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
929 for (
unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
931 float curr_distance = 0.0f;
932 curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
933 curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
934 curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
935 if (curr_distance > max_distance)
936 max_distance = curr_distance;
938 max_distance =
static_cast<float> (std::sqrt (max_distance));
939 unsigned int i_class = training_classes_[i_object];
940 objects_sigmas[i_class].push_back (max_distance);
943 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
946 auto number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
947 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
948 sig += objects_sigmas[i_class][i_object];
949 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
950 sigmas[i_class] = sig;
955 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
957 const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
958 const Eigen::MatrixXi &labels,
959 std::vector<float>& sigmas,
960 std::vector<std::vector<unsigned int> >& clusters,
961 std::vector<std::vector<float> >& statistical_weights,
962 std::vector<float>& learned_weights)
964 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
966 std::vector<float> vec;
967 vec.resize (number_of_clusters_, 0.0f);
968 statistical_weights.clear ();
969 learned_weights.clear ();
970 statistical_weights.resize (number_of_classes, vec);
971 learned_weights.resize (locations.size (), 0.0f);
974 std::vector<int> vect;
975 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
978 std::vector<int> n_ftr;
981 std::vector<int> n_vot;
984 std::vector<int> n_vw;
987 std::vector<std::vector<int> > n_vot_2;
989 n_vot_2.resize (number_of_clusters_, vect);
990 n_vot.resize (number_of_clusters_, 0);
991 n_ftr.resize (number_of_classes, 0);
992 for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
994 int i_class = training_classes_[locations[i_location].model_num_];
995 int i_cluster = labels (i_location);
996 n_vot_2[i_cluster][i_class] += 1;
997 n_vot[i_cluster] += 1;
1001 n_vw.resize (number_of_classes, 0);
1002 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1003 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1004 if (n_vot_2[i_cluster][i_class] > 0)
1008 learned_weights.resize (locations.size (), 0.0);
1009 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1011 auto number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1012 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1014 unsigned int i_index = clusters[i_cluster][i_visual_word];
1015 int i_class = training_classes_[locations[i_index].model_num_];
1016 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1017 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1019 std::vector<float> calculated_sigmas;
1020 calculateSigmas (calculated_sigmas);
1021 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1022 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1025 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1026 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1027 applyTransform (direction, transform);
1028 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1031 std::vector<float> gauss_dists;
1032 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1034 unsigned int j_index = clusters[i_cluster][j_visual_word];
1035 int j_class = training_classes_[locations[j_index].model_num_];
1036 if (i_class != j_class)
1039 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1040 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1041 applyTransform (direction_2, transform_2);
1042 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1043 float residual = (predicted_center - actual_center).norm ();
1044 float value = -residual * residual / square_sigma_dist;
1045 gauss_dists.push_back (static_cast<float> (std::exp (value)));
1048 std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1049 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1050 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1055 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1057 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1059 if (n_vot_2[i_cluster][i_class] == 0)
1061 if (n_vw[i_class] == 0)
1063 if (n_vot[i_cluster] == 0)
1065 if (n_ftr[i_class] == 0)
1067 float part_1 =
static_cast<float> (n_vw[i_class]);
1068 float part_2 =
static_cast<float> (n_vot[i_cluster]);
1069 float part_3 =
static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1070 float part_4 = 0.0f;
1075 for (
unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1076 if (n_ftr[j_class] != 0)
1077 part_4 +=
static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1079 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1085 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1094 grid.
setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1099 grid.
filter (temp_cloud);
1102 constexpr
float max_value = std::numeric_limits<float>::max ();
1104 const auto num_source_points = in_point_cloud->
size ();
1105 const auto num_sample_points = temp_cloud.
size ();
1107 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1108 std::vector<int> sampling_indices (num_sample_points, -1);
1110 for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1116 PointT pt_1 = (*in_point_cloud)[i_point];
1117 PointT pt_2 = temp_cloud[index];
1119 float distance = (pt_1.x - pt_2.x) * (pt_1.x - pt_2.x) + (pt_1.y - pt_2.y) * (pt_1.y - pt_2.y) + (pt_1.z - pt_2.z) * (pt_1.z - pt_2.z);
1120 if (distance < dist_to_grid_center[index])
1122 dist_to_grid_center[index] = distance;
1123 sampling_indices[index] =
static_cast<int> (i_point);
1132 final_inliers_indices->indices.reserve (num_sample_points);
1133 for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1135 if (sampling_indices[i_point] != -1)
1136 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1140 extract_points.
setIndices (final_inliers_indices);
1141 extract_points.
filter (*out_sampled_point_cloud);
1144 extract_normals.
setIndices (final_inliers_indices);
1145 extract_normals.
filter (*out_sampled_normal_cloud);
1149 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1152 Eigen::Vector3f shift_point)
1154 for (
auto point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1156 point_it->x -= shift_point.x ();
1157 point_it->y -= shift_point.y ();
1158 point_it->z -= shift_point.z ();
1163 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1166 Eigen::Matrix3f result;
1167 Eigen::Matrix3f rotation_matrix_X;
1168 Eigen::Matrix3f rotation_matrix_Z;
1174 float denom_X =
static_cast<float> (std::sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1175 A = in_normal.normal_y / denom_X;
1176 B = sign * in_normal.normal_z / denom_X;
1177 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1181 float denom_Z =
static_cast<float> (std::sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1182 A = in_normal.normal_y / denom_Z;
1183 B = sign * in_normal.normal_x / denom_Z;
1184 rotation_matrix_Z << A, -
B, 0.0f,
1188 result = rotation_matrix_X * rotation_matrix_Z;
1194 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1197 io_vec = in_transform * io_vec;
1201 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1210 feature_estimator_->setInputCloud (sampled_point_cloud->
makeShared ());
1212 feature_estimator_->setSearchMethod (tree);
1222 feature_estimator_->compute (*feature_cloud);
1226 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double
1228 const Eigen::MatrixXf& points_to_cluster,
1229 int number_of_clusters,
1230 Eigen::MatrixXi& io_labels,
1234 Eigen::MatrixXf& cluster_centers)
1236 constexpr
int spp_trials = 3;
1237 std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1238 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1240 attempts = std::max (attempts, 1);
1241 srand (static_cast<unsigned int> (time (
nullptr)));
1243 Eigen::MatrixXi labels (number_of_points, 1);
1245 if (flags & USE_INITIAL_LABELS)
1250 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1251 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1252 std::vector<int> counters (number_of_clusters);
1253 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1254 Eigen::Vector2f* box = boxes.data();
1256 double best_compactness = std::numeric_limits<double>::max ();
1257 double compactness = 0.0;
1259 if (criteria.
type_ & TermCriteria::EPS)
1262 criteria.
epsilon_ = std::numeric_limits<float>::epsilon ();
1266 if (criteria.
type_ & TermCriteria::COUNT)
1271 if (number_of_clusters == 1)
1277 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1278 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1280 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1281 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1283 float v = points_to_cluster (i_point, i_dim);
1284 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1285 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1288 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
1290 float max_center_shift = std::numeric_limits<float>::max ();
1291 for (
int iter = 0; iter < criteria.
max_count_ && max_center_shift > criteria.
epsilon_; iter++)
1293 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1295 centers = old_centers;
1298 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1300 if (flags & PP_CENTERS)
1301 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1304 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1306 Eigen::VectorXf center (feature_dimension);
1307 generateRandomCenter (boxes, center);
1308 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1309 centers (i_cl_center, i_dim) = center (i_dim);
1316 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1317 counters[i_cluster] = 0;
1318 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1320 int i_label = labels (i_point, 0);
1321 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1322 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1323 counters[i_label]++;
1326 max_center_shift = 0.0f;
1327 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1329 if (counters[i_cl_center] != 0)
1331 float scale = 1.0f /
static_cast<float> (counters[i_cl_center]);
1332 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1333 centers (i_cl_center, i_dim) *= scale;
1337 Eigen::VectorXf center (feature_dimension);
1338 generateRandomCenter (boxes, center);
1339 for(
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1340 centers (i_cl_center, i_dim) = center (i_dim);
1346 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1348 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1349 dist += diff * diff;
1351 max_center_shift = std::max (max_center_shift, dist);
1356 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1358 Eigen::VectorXf sample (feature_dimension);
1359 sample = points_to_cluster.row (i_point);
1362 float min_dist = std::numeric_limits<float>::max ();
1364 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1366 Eigen::VectorXf center (feature_dimension);
1367 center = centers.row (i_cluster);
1368 float dist = computeDistance (sample, center);
1369 if (min_dist > dist)
1375 compactness += min_dist;
1376 labels (i_point, 0) = k_best;
1380 if (compactness < best_compactness)
1382 best_compactness = compactness;
1383 cluster_centers = centers;
1388 return (best_compactness);
1392 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1394 const Eigen::MatrixXf& data,
1395 Eigen::MatrixXf& out_centers,
1396 int number_of_clusters,
1399 std::size_t dimension = data.cols ();
1400 auto number_of_points =
static_cast<unsigned int> (data.rows ());
1401 std::vector<int> centers_vec (number_of_clusters);
1402 int* centers = centers_vec.data();
1403 std::vector<double> dist (number_of_points);
1404 std::vector<double> tdist (number_of_points);
1405 std::vector<double> tdist2 (number_of_points);
1408 unsigned int random_unsigned = rand ();
1409 centers[0] = random_unsigned % number_of_points;
1411 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1413 Eigen::VectorXf first (dimension);
1414 Eigen::VectorXf second (dimension);
1415 first = data.row (i_point);
1416 second = data.row (centers[0]);
1417 dist[i_point] = computeDistance (first, second);
1418 sum0 += dist[i_point];
1421 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1423 double best_sum = std::numeric_limits<double>::max ();
1424 int best_center = -1;
1425 for (
int i_trials = 0; i_trials < trials; i_trials++)
1427 unsigned int random_integer = rand () - 1;
1428 double random_double =
static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1429 double p = random_double * sum0;
1431 unsigned int i_point;
1432 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1433 if ( (p -= dist[i_point]) <= 0.0)
1439 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1441 Eigen::VectorXf first (dimension);
1442 Eigen::VectorXf second (dimension);
1443 first = data.row (i_point);
1444 second = data.row (ci);
1445 tdist2[i_point] = std::min (static_cast<double> (computeDistance (first, second)), dist[i_point]);
1446 s += tdist2[i_point];
1453 std::swap (tdist, tdist2);
1457 centers[i_cluster] = best_center;
1459 std::swap (dist, tdist);
1462 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1463 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1464 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1468 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1470 Eigen::VectorXf& center)
1472 std::size_t dimension = boxes.size ();
1473 float margin = 1.0f /
static_cast<float> (dimension);
1475 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1477 unsigned int random_integer = rand () - 1;
1478 float random_float =
static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1479 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1484 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
1487 std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1488 float distance = 0.0f;
1489 for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1491 float diff = vec_1 (i_dim) - vec_2 (i_dim);
1492 distance += diff * diff;
1498 #endif //#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_
A point structure representing normal coordinates and the surface curvature estimate.
void generateRandomCenter(const std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > &boxes, Eigen::VectorXf ¢er)
Generates random center for cluster.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
std::vector< typename pcl::PointCloud< PointT >::Ptr > getTrainingClouds()
This method simply returns the clouds that were set as the training clouds.
std::vector< float > learned_weights_
Stores learned weights.
shared_ptr< PointCloud< PointT > > Ptr
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
ISMVoteList()
Empty constructor with member variables initialization.
unsigned int getNumberOfVotes()
This method simply returns the number of votes.
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++...
bool loadModelFromfile(std::string &file_name)
This method loads the trained model from file.
The assignment of this structure is to store the statistical/learned weights and other information of...
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 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.
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed, so avoid using this function on non-empty clouds.
std::vector< float > getSigmaDists()
Returns the array of sigma values.
virtual ~ISMVoteList()
virtual descriptor.
iterator begin() noexcept
unsigned int descriptors_dimension_
Stores descriptors dimension.
unsigned int number_of_classes_
Stores the number of classes.
Eigen::MatrixXf directions_to_center_
Stores the directions to objects center for each visual word.
This structure is used for determining the end of the k-means clustering process. ...
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
pcl::features::ISMModel::Ptr ISMModelPtr
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...
ISMModel()
Simple constructor that initializes the structure.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
void setNVotState(bool state)
Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
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 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...
virtual ~ISMModel()
Destructor that frees memory.
std::uint32_t width
The point cloud width (if organized as an image-structure).
shared_ptr< ::pcl::PointIndices > Ptr
A point structure representing an N-D histogram.
bool trainISM(ISMModelPtr &trained_model)
This method performs training and forms a visual vocabulary.
ImplicitShapeModelEstimation()
Simple constructor that initializes everything.
unsigned int number_of_clusters_
Stores the number of clusters.
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.
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
A point structure representing Euclidean xyz coordinates.
shared_ptr< ISMVoteList< PointT > > Ptr
float computeDistance(Eigen::VectorXf &vec_1, Eigen::VectorXf &vec_2)
Computes the square distance between two vectors.
std::vector< std::vector< float > > statistical_weights_
Stores statistical weights.
A point structure representing an interest point with Euclidean xyz coordinates, and an interest valu...
void reset()
this method resets all variables and frees memory.
std::uint32_t height
The point cloud height (if organized as an image-structure).
double getDensityAtPoint(const PointT &point, double sigma_dist)
Returns the density at the specified point.
double density
Density of this peak.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void setFeatureEstimator(FeaturePtr feature)
Changes the feature estimator.
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 applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
FeaturePtr getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the corresponding cloud of normals for every training point cloud.
std::vector< unsigned int > getTrainingClasses()
Returns the array of classes that indicates which class the corresponding training cloud belongs...
bool clusterDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, Eigen::MatrixXi &labels, Eigen::MatrixXf &clusters_centers)
This method performs descriptor clustering.
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.
ISMModel & operator=(const ISMModel &other)
Operator overloading for deep copy.
int class_id
Determines which class this peak belongs.
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).
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
shared_ptr< const PointCloud< PointT > > ConstPtr
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
This struct is used for storing peak.
unsigned int number_of_visual_words_
Stores the number of visual words.
std::vector< unsigned int > classes_
Stores the class label for every direction.
shared_ptr< pcl::search::Search< PointT > > Ptr
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
void addVote(pcl::InterestPoint &in_vote, const PointT &vote_origin, int in_class)
This method simply adds another vote to the list.
std::vector< float > sigmas_
Stores the sigma value for each class.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool extractDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations)
Extracts the descriptors from the input clouds.
float getSamplingSize()
Returns the sampling size used for cloud simplification.
int type_
Flag that determines when the k-means clustering must be stopped.
typename Feature::Ptr FeaturePtr
unsigned int getNumberOfClusters()
Returns the number of clusters used for descriptor clustering.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
float epsilon_
Defines the accuracy for k-means clustering.
void validateTree()
this method is simply setting up the search tree.
This structure stores the information about the keypoint.
void setSamplingSize(float sampling_size)
Changes the sampling size used for cloud simplification.
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.
int max_count_
Defines maximum number of iterations for k-means clustering.
void setTrainingClasses(const std::vector< unsigned int > &training_classes)
Allows to set the class labels for the corresponding training clouds.
void setTrainingClouds(const std::vector< typename pcl::PointCloud< PointT >::Ptr > &training_clouds)
Allows to set clouds for training the ISM model.
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...
void calculateSigmas(std::vector< float > &sigmas)
This method calculates the value of sigma used for calculating the learned weights for every single c...
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm...