41 #ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_
42 #define PCL_IMPLICIT_SHAPE_MODEL_HPP_
44 #include "../implicit_shape_model.h"
45 #include <pcl/filters/voxel_grid.h>
46 #include <pcl/filters/extract_indices.h>
51 template <
typename Po
intT>
54 tree_is_valid_ (false),
63 template <
typename Po
intT>
66 votes_class_.clear ();
67 votes_origins_.reset ();
75 template <
typename Po
intT>
void
79 tree_is_valid_ =
false;
80 votes_->points.insert (votes_->points.end (), vote);
82 votes_origins_->points.push_back (vote_origin);
83 votes_class_.push_back (votes_class);
93 colored_cloud->
width = 1;
101 for (
const auto& i_point: *cloud)
106 colored_cloud->
points.push_back (point);
113 for (
const auto &i_vote : votes_->points)
118 colored_cloud->
points.push_back (point);
120 colored_cloud->
height += votes_->size ();
122 return (colored_cloud);
126 template <
typename Po
intT>
void
128 std::vector<
pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
130 double in_non_maxima_radius,
135 const std::size_t n_vote_classes = votes_class_.size ();
136 if (n_vote_classes == 0)
138 for (std::size_t i = 0; i < n_vote_classes ; i++)
139 assert ( votes_class_[i] == in_class_id );
143 constexpr
int NUM_INIT_PTS = 100;
144 double SIGMA_DIST = in_sigma;
145 const double FINAL_EPS = SIGMA_DIST / 100;
147 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
148 std::vector<double> peak_densities (NUM_INIT_PTS);
149 double max_density = -1.0;
150 for (
int i = 0; i < NUM_INIT_PTS; i++)
152 Eigen::Vector3f old_center;
153 const auto idx = votes_->size() * i / NUM_INIT_PTS;
154 Eigen::Vector3f curr_center = (*votes_)[idx].getVector3fMap();
158 old_center = curr_center;
159 curr_center = shiftMean (old_center, SIGMA_DIST);
160 }
while ((old_center - curr_center).norm () > FINAL_EPS);
163 point.x = curr_center (0);
164 point.y = curr_center (1);
165 point.z = curr_center (2);
166 double curr_density = getDensityAtPoint (point, SIGMA_DIST);
167 assert (curr_density >= 0.0);
169 peaks[i] = curr_center;
170 peak_densities[i] = curr_density;
172 if ( max_density < curr_density )
173 max_density = curr_density;
177 std::vector<bool> peak_flag (NUM_INIT_PTS,
true);
178 for (
int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
181 double best_density = -1.0;
182 Eigen::Vector3f strongest_peak;
183 int best_peak_ind (-1);
184 int peak_counter (0);
185 for (
int i = 0; i < NUM_INIT_PTS; i++)
190 if ( peak_densities[i] > best_density)
192 best_density = peak_densities[i];
193 strongest_peak = peaks[i];
199 if( peak_counter == 0 )
203 peak.x = strongest_peak(0);
204 peak.y = strongest_peak(1);
205 peak.z = strongest_peak(2);
208 out_peaks.push_back ( peak );
211 peak_flag[best_peak_ind] =
false;
212 for (
int i = 0; i < NUM_INIT_PTS; i++)
218 double dist = (strongest_peak - peaks[i]).norm ();
219 if ( dist < in_non_maxima_radius )
220 peak_flag[i] =
false;
226 template <
typename Po
intT>
void
231 if (tree_ ==
nullptr)
233 tree_->setInputCloud (votes_);
234 k_ind_.resize ( votes_->size (), -1 );
235 k_sqr_dist_.resize ( votes_->size (), 0.0f );
236 tree_is_valid_ =
true;
241 template <
typename Po
intT> Eigen::Vector3f
246 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
253 std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
255 for (std::size_t j = 0; j < n_pts; j++)
257 double kernel = (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
258 Eigen::Vector3f vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[k_ind_[j]].z);
259 wgh_sum += vote_vec *
static_cast<float> (
kernel);
262 assert (denom > 0.0);
264 return (wgh_sum /
static_cast<float> (denom));
268 template <
typename Po
intT>
double
270 const PointT &point,
double sigma_dist)
274 const std::size_t n_vote_classes = votes_class_.size ();
275 if (n_vote_classes == 0)
278 double sum_vote = 0.0;
284 std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
286 for (std::size_t j = 0; j < num_of_pts; j++)
287 sum_vote += (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
293 template <
typename Po
intT>
unsigned int
296 return (
static_cast<unsigned int> (votes_->size ()));
301 statistical_weights_ (0),
302 learned_weights_ (0),
306 number_of_classes_ (0),
307 number_of_visual_words_ (0),
308 number_of_clusters_ (0),
309 descriptors_dimension_ (0)
323 std::vector<float> vec;
324 vec.resize (this->number_of_clusters_, 0.0f);
325 this->statistical_weights_.resize (this->number_of_classes_, vec);
326 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
327 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
330 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
331 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
332 this->learned_weights_[i_visual_word] = copy.
learned_weights_[i_visual_word];
334 this->classes_.resize (this->number_of_visual_words_, 0);
335 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
336 this->classes_[i_visual_word] = copy.
classes_[i_visual_word];
338 this->sigmas_.resize (this->number_of_classes_, 0.0f);
339 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
340 this->sigmas_[i_class] = copy.
sigmas_[i_class];
342 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
343 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
344 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
345 this->directions_to_center_ (i_visual_word, i_dim) = copy.
directions_to_center_ (i_visual_word, i_dim);
347 this->clusters_centers_.resize (this->number_of_clusters_, 3);
348 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
349 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
350 this->clusters_centers_ (i_cluster, i_dim) = copy.
clusters_centers_ (i_cluster, i_dim);
363 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
366 output_file.close ();
370 output_file << number_of_classes_ <<
" ";
371 output_file << number_of_visual_words_ <<
" ";
372 output_file << number_of_clusters_ <<
" ";
373 output_file << descriptors_dimension_ <<
" ";
376 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
377 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
378 output_file << statistical_weights_[i_class][i_cluster] <<
" ";
381 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
382 output_file << learned_weights_[i_visual_word] <<
" ";
385 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
386 output_file << classes_[i_visual_word] <<
" ";
389 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
390 output_file << sigmas_[i_class] <<
" ";
393 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
394 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
395 output_file << directions_to_center_ (i_visual_word, i_dim) <<
" ";
398 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
399 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
400 output_file << clusters_centers_ (i_cluster, i_dim) <<
" ";
403 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
405 output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) <<
" ";
406 for (
const unsigned int &visual_word : clusters_[i_cluster])
407 output_file << visual_word <<
" ";
410 output_file.close ();
419 std::ifstream input_file (file_name.c_str ());
428 input_file.getline (line, 256,
' ');
429 number_of_classes_ =
static_cast<unsigned int> (strtol (line,
nullptr, 10));
430 input_file.getline (line, 256,
' '); number_of_visual_words_ = atoi (line);
431 input_file.getline (line, 256,
' '); number_of_clusters_ = atoi (line);
432 input_file.getline (line, 256,
' '); descriptors_dimension_ = atoi (line);
435 std::vector<float> vec;
436 vec.resize (number_of_clusters_, 0.0f);
437 statistical_weights_.resize (number_of_classes_, vec);
438 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
439 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
440 input_file >> statistical_weights_[i_class][i_cluster];
443 learned_weights_.resize (number_of_visual_words_, 0.0f);
444 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
445 input_file >> learned_weights_[i_visual_word];
448 classes_.resize (number_of_visual_words_, 0);
449 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
450 input_file >> classes_[i_visual_word];
453 sigmas_.resize (number_of_classes_, 0.0f);
454 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
455 input_file >> sigmas_[i_class];
458 directions_to_center_.resize (number_of_visual_words_, 3);
459 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
460 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
461 input_file >> directions_to_center_ (i_visual_word, i_dim);
464 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
465 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
466 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
467 input_file >> clusters_centers_ (i_cluster, i_dim);
470 std::vector<unsigned int> vect;
471 clusters_.resize (number_of_clusters_, vect);
472 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
474 unsigned int size_of_current_cluster = 0;
475 input_file >> size_of_current_cluster;
476 clusters_[i_cluster].resize (size_of_current_cluster, 0);
477 for (
unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
478 input_file >> clusters_[i_cluster][i_visual_word];
489 statistical_weights_.clear ();
490 learned_weights_.clear ();
493 directions_to_center_.resize (0, 0);
494 clusters_centers_.resize (0, 0);
496 number_of_classes_ = 0;
497 number_of_visual_words_ = 0;
498 number_of_clusters_ = 0;
499 descriptors_dimension_ = 0;
515 std::vector<float> vec;
516 vec.resize (number_of_clusters_, 0.0f);
517 this->statistical_weights_.resize (this->number_of_classes_, vec);
518 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
519 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
520 this->statistical_weights_[i_class][i_cluster] = other.
statistical_weights_[i_class][i_cluster];
522 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
523 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
524 this->learned_weights_[i_visual_word] = other.
learned_weights_[i_visual_word];
526 this->classes_.resize (this->number_of_visual_words_, 0);
527 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
528 this->classes_[i_visual_word] = other.
classes_[i_visual_word];
530 this->sigmas_.resize (this->number_of_classes_, 0.0f);
531 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
532 this->sigmas_[i_class] = other.
sigmas_[i_class];
534 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
535 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
536 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
537 this->directions_to_center_ (i_visual_word, i_dim) = other.
directions_to_center_ (i_visual_word, i_dim);
539 this->clusters_centers_.resize (this->number_of_clusters_, 3);
540 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
541 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
542 this->clusters_centers_ (i_cluster, i_dim) = other.
clusters_centers_ (i_cluster, i_dim);
548 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
550 training_clouds_ (0),
551 training_classes_ (0),
552 training_normals_ (0),
553 training_sigmas_ (0),
554 sampling_size_ (0.1f),
555 feature_estimator_ (),
556 number_of_clusters_ (184),
562 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
565 training_clouds_.clear ();
566 training_classes_.clear ();
567 training_normals_.clear ();
568 training_sigmas_.clear ();
569 feature_estimator_.reset ();
573 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
576 return (training_clouds_);
580 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
584 training_clouds_.clear ();
585 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
586 training_clouds_.swap (clouds);
590 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
593 return (training_classes_);
597 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
600 training_classes_.clear ();
601 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
602 training_classes_.swap (classes);
606 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
609 return (training_normals_);
613 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
617 training_normals_.clear ();
618 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
619 training_normals_.swap (normals);
623 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
626 return (sampling_size_);
630 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
633 if (sampling_size >= std::numeric_limits<float>::epsilon ())
634 sampling_size_ = sampling_size;
641 return (feature_estimator_);
645 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
648 feature_estimator_ = feature;
652 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int
655 return (number_of_clusters_);
659 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
662 if (num_of_clusters > 0)
663 number_of_clusters_ = num_of_clusters;
667 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
670 return (training_sigmas_);
674 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
677 training_sigmas_.clear ();
678 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
679 training_sigmas_.swap (sigmas);
683 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
690 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
697 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
702 if (trained_model ==
nullptr)
704 trained_model->reset ();
706 std::vector<pcl::Histogram<FeatureSize> > histograms;
707 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
708 success = extractDescriptors (histograms, locations);
712 Eigen::MatrixXi labels;
713 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
717 std::vector<unsigned int> vec;
718 trained_model->clusters_.resize (number_of_clusters_, vec);
719 for (std::size_t i_label = 0; i_label < locations.size (); i_label++)
720 trained_model->clusters_[labels (i_label)].push_back (i_label);
722 calculateSigmas (trained_model->sigmas_);
727 trained_model->sigmas_,
728 trained_model->clusters_,
729 trained_model->statistical_weights_,
730 trained_model->learned_weights_);
732 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
733 trained_model->number_of_visual_words_ =
static_cast<unsigned int> (histograms.size ());
734 trained_model->number_of_clusters_ = number_of_clusters_;
735 trained_model->descriptors_dimension_ = FeatureSize;
737 trained_model->directions_to_center_.resize (locations.size (), 3);
738 trained_model->classes_.resize (locations.size ());
739 for (std::size_t i_dir = 0; i_dir < locations.size (); i_dir++)
741 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
742 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
743 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
744 trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
756 int in_class_of_interest)
760 if (in_cloud->
points.empty ())
765 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
766 if (sampled_point_cloud->
points.empty ())
770 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
773 const auto n_key_points =
static_cast<unsigned int> (sampled_point_cloud->
size ());
774 std::vector<int> min_dist_inds (n_key_points, -1);
775 for (
unsigned int i_point = 0; i_point < n_key_points; i_point++)
777 Eigen::VectorXf curr_descriptor (FeatureSize);
778 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
779 curr_descriptor (i_dim) = (*feature_cloud)[i_point].histogram[i_dim];
781 float descriptor_sum = curr_descriptor.sum ();
782 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
785 unsigned int min_dist_idx = 0;
786 Eigen::VectorXf clusters_center (FeatureSize);
787 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
788 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
790 float best_dist = computeDistance (curr_descriptor, clusters_center);
791 for (
unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
793 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
794 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
795 float curr_dist = computeDistance (clusters_center, curr_descriptor);
796 if (curr_dist < best_dist)
798 min_dist_idx = i_clust_cent;
799 best_dist = curr_dist;
802 min_dist_inds[i_point] = min_dist_idx;
805 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
807 int min_dist_idx = min_dist_inds[i_point];
808 if (min_dist_idx == -1)
811 const auto n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
813 Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
814 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
816 unsigned int index = model->clusters_[min_dist_idx][i_word];
817 unsigned int i_class = model->classes_[index];
818 if (
static_cast<int> (i_class) != in_class_of_interest)
822 Eigen::Vector3f direction (
823 model->directions_to_center_(index, 0),
824 model->directions_to_center_(index, 1),
825 model->directions_to_center_(index, 2));
826 applyTransform (direction, transform.transpose ());
829 Eigen::Vector3f vote_pos = (*sampled_point_cloud)[i_point].getVector3fMap () + direction;
830 vote.x = vote_pos[0];
831 vote.y = vote_pos[1];
832 vote.z = vote_pos[2];
833 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
834 float learned_weight = model->learned_weights_[index];
835 float power = statistical_weight * learned_weight;
837 if (vote.
strength > std::numeric_limits<float>::epsilon ())
838 out_votes->
addVote (vote, (*sampled_point_cloud)[i_point], i_class);
846 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
849 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
854 if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ ==
nullptr)
857 for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
860 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
861 const auto num_of_points = training_clouds_[i_cloud]->size ();
862 for (
auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
863 models_center += point_j->getVector3fMap ();
864 models_center /=
static_cast<float> (num_of_points);
869 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
870 if (sampled_point_cloud->
points.empty ())
873 shiftCloud (training_clouds_[i_cloud], models_center);
874 shiftCloud (sampled_point_cloud, models_center);
877 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
880 for (
auto point_i = sampled_point_cloud->
points.cbegin (); point_i != sampled_point_cloud->
points.cend (); point_i++, point_index++)
882 float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
883 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
886 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
888 int dist =
static_cast<int> (
std::distance (sampled_point_cloud->
points.cbegin (), point_i));
889 Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
890 Eigen::Vector3f zero;
894 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
895 applyTransform (new_dir, new_basis);
897 PointT point (new_dir[0], new_dir[1], new_dir[2]);
898 LocationInfo info (
static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
899 locations.insert(locations.end (), info);
907 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
910 Eigen::MatrixXi& labels,
911 Eigen::MatrixXf& clusters_centers)
913 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
915 for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
916 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
917 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
919 labels.resize (histograms.size(), 1);
920 computeKMeansClustering (
924 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
933 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
936 if (!training_sigmas_.empty ())
938 sigmas.resize (training_sigmas_.size (), 0.0f);
939 for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
940 sigmas[i_sigma] = training_sigmas_[i_sigma];
945 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
946 sigmas.resize (number_of_classes, 0.0f);
948 std::vector<float> vec;
949 std::vector<std::vector<float> > objects_sigmas;
950 objects_sigmas.resize (number_of_classes, vec);
952 auto number_of_objects =
static_cast<unsigned int> (training_clouds_.size ());
953 for (
unsigned int i_object = 0; i_object < number_of_objects; i_object++)
955 float max_distance = 0.0f;
956 const auto number_of_points = training_clouds_[i_object]->size ();
957 for (
unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
958 for (
unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
960 float curr_distance = 0.0f;
961 curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
962 curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
963 curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
964 if (curr_distance > max_distance)
965 max_distance = curr_distance;
967 max_distance =
static_cast<float> (std::sqrt (max_distance));
968 unsigned int i_class = training_classes_[i_object];
969 objects_sigmas[i_class].push_back (max_distance);
972 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
975 auto number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
976 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
977 sig += objects_sigmas[i_class][i_object];
978 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
979 sigmas[i_class] = sig;
984 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
986 const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
987 const Eigen::MatrixXi &labels,
988 std::vector<float>& sigmas,
989 std::vector<std::vector<unsigned int> >& clusters,
990 std::vector<std::vector<float> >& statistical_weights,
991 std::vector<float>& learned_weights)
993 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
995 std::vector<float> vec;
996 vec.resize (number_of_clusters_, 0.0f);
997 statistical_weights.clear ();
998 learned_weights.clear ();
999 statistical_weights.resize (number_of_classes, vec);
1000 learned_weights.resize (locations.size (), 0.0f);
1003 std::vector<int> vect;
1004 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
1007 std::vector<int> n_ftr;
1010 std::vector<int> n_vot;
1013 std::vector<int> n_vw;
1016 std::vector<std::vector<int> > n_vot_2;
1018 n_vot_2.resize (number_of_clusters_, vect);
1019 n_vot.resize (number_of_clusters_, 0);
1020 n_ftr.resize (number_of_classes, 0);
1021 for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
1023 int i_class = training_classes_[locations[i_location].model_num_];
1024 int i_cluster = labels (i_location);
1025 n_vot_2[i_cluster][i_class] += 1;
1026 n_vot[i_cluster] += 1;
1027 n_ftr[i_class] += 1;
1030 n_vw.resize (number_of_classes, 0);
1031 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1032 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1033 if (n_vot_2[i_cluster][i_class] > 0)
1037 learned_weights.resize (locations.size (), 0.0);
1038 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1040 auto number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1041 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1043 unsigned int i_index = clusters[i_cluster][i_visual_word];
1044 int i_class = training_classes_[locations[i_index].model_num_];
1045 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1046 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1048 std::vector<float> calculated_sigmas;
1049 calculateSigmas (calculated_sigmas);
1050 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1051 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1054 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1055 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1056 applyTransform (direction, transform);
1057 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1060 std::vector<float> gauss_dists;
1061 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1063 unsigned int j_index = clusters[i_cluster][j_visual_word];
1064 int j_class = training_classes_[locations[j_index].model_num_];
1065 if (i_class != j_class)
1068 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1069 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1070 applyTransform (direction_2, transform_2);
1071 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1072 float residual = (predicted_center - actual_center).norm ();
1073 float value = -residual * residual / square_sigma_dist;
1074 gauss_dists.push_back (
static_cast<float> (std::exp (value)));
1077 std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1078 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1079 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1084 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1086 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1088 if (n_vot_2[i_cluster][i_class] == 0)
1090 if (n_vw[i_class] == 0)
1092 if (n_vot[i_cluster] == 0)
1094 if (n_ftr[i_class] == 0)
1096 float part_1 =
static_cast<float> (n_vw[i_class]);
1097 float part_2 =
static_cast<float> (n_vot[i_cluster]);
1098 float part_3 =
static_cast<float> (n_vot_2[i_cluster][i_class]) /
static_cast<float> (n_ftr[i_class]);
1099 float part_4 = 0.0f;
1104 for (
unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1105 if (n_ftr[j_class] != 0)
1106 part_4 +=
static_cast<float> (n_vot_2[i_cluster][j_class]) /
static_cast<float> (n_ftr[j_class]);
1108 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1114 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1123 grid.
setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1128 grid.
filter (temp_cloud);
1131 constexpr
float max_value = std::numeric_limits<float>::max ();
1133 const auto num_source_points = in_point_cloud->
size ();
1134 const auto num_sample_points = temp_cloud.
size ();
1136 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1137 std::vector<int> sampling_indices (num_sample_points, -1);
1139 for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1145 PointT pt_1 = (*in_point_cloud)[i_point];
1146 PointT pt_2 = temp_cloud[index];
1148 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);
1149 if (
distance < dist_to_grid_center[index])
1151 dist_to_grid_center[index] =
distance;
1152 sampling_indices[index] =
static_cast<int> (i_point);
1161 final_inliers_indices->indices.reserve (num_sample_points);
1162 for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1164 if (sampling_indices[i_point] != -1)
1165 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1169 extract_points.
setIndices (final_inliers_indices);
1170 extract_points.
filter (*out_sampled_point_cloud);
1173 extract_normals.
setIndices (final_inliers_indices);
1174 extract_normals.
filter (*out_sampled_normal_cloud);
1178 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1181 Eigen::Vector3f shift_point)
1183 for (
auto point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1185 point_it->x -= shift_point.x ();
1186 point_it->y -= shift_point.y ();
1187 point_it->z -= shift_point.z ();
1192 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1195 Eigen::Matrix3f result;
1196 Eigen::Matrix3f rotation_matrix_X;
1197 Eigen::Matrix3f rotation_matrix_Z;
1203 float denom_X =
static_cast<float> (std::sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1204 A = in_normal.normal_y / denom_X;
1205 B = sign * in_normal.normal_z / denom_X;
1206 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1210 float denom_Z =
static_cast<float> (std::sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1211 A = in_normal.normal_y / denom_Z;
1212 B = sign * in_normal.normal_x / denom_Z;
1213 rotation_matrix_Z << A, -
B, 0.0f,
1217 result = rotation_matrix_X * rotation_matrix_Z;
1223 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1226 io_vec = in_transform * io_vec;
1230 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1239 feature_estimator_->setInputCloud (sampled_point_cloud->
makeShared ());
1241 feature_estimator_->setSearchMethod (tree);
1248 dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1251 feature_estimator_->compute (*feature_cloud);
1255 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double
1257 const Eigen::MatrixXf& points_to_cluster,
1258 int number_of_clusters,
1259 Eigen::MatrixXi& io_labels,
1263 Eigen::MatrixXf& cluster_centers)
1265 constexpr
int spp_trials = 3;
1266 std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1267 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1269 attempts = std::max (attempts, 1);
1270 srand (
static_cast<unsigned int> (time (
nullptr)));
1272 Eigen::MatrixXi labels (number_of_points, 1);
1274 if (flags & USE_INITIAL_LABELS)
1279 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1280 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1281 std::vector<int> counters (number_of_clusters);
1282 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1283 Eigen::Vector2f* box = boxes.data();
1285 double best_compactness = std::numeric_limits<double>::max ();
1286 double compactness = 0.0;
1288 if (criteria.
type_ & TermCriteria::EPS)
1291 criteria.
epsilon_ = std::numeric_limits<float>::epsilon ();
1295 if (criteria.
type_ & TermCriteria::COUNT)
1300 if (number_of_clusters == 1)
1306 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1307 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1309 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1310 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1312 float v = points_to_cluster (i_point, i_dim);
1313 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1314 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1317 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
1319 float max_center_shift = std::numeric_limits<float>::max ();
1320 for (
int iter = 0; iter < criteria.
max_count_ && max_center_shift > criteria.
epsilon_; iter++)
1322 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1324 centers = old_centers;
1327 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1329 if (flags & PP_CENTERS)
1330 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1333 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1335 Eigen::VectorXf center (feature_dimension);
1336 generateRandomCenter (boxes, center);
1337 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1338 centers (i_cl_center, i_dim) = center (i_dim);
1345 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1346 counters[i_cluster] = 0;
1347 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1349 int i_label = labels (i_point, 0);
1350 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1351 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1352 counters[i_label]++;
1355 max_center_shift = 0.0f;
1356 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1358 if (counters[i_cl_center] != 0)
1360 float scale = 1.0f /
static_cast<float> (counters[i_cl_center]);
1361 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1362 centers (i_cl_center, i_dim) *= scale;
1366 Eigen::VectorXf center (feature_dimension);
1367 generateRandomCenter (boxes, center);
1368 for(
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1369 centers (i_cl_center, i_dim) = center (i_dim);
1375 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1377 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1378 dist += diff * diff;
1380 max_center_shift = std::max (max_center_shift, dist);
1385 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1387 Eigen::VectorXf sample (feature_dimension);
1388 sample = points_to_cluster.row (i_point);
1391 float min_dist = std::numeric_limits<float>::max ();
1393 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1395 Eigen::VectorXf center (feature_dimension);
1396 center = centers.row (i_cluster);
1397 float dist = computeDistance (sample, center);
1398 if (min_dist > dist)
1404 compactness += min_dist;
1405 labels (i_point, 0) = k_best;
1409 if (compactness < best_compactness)
1411 best_compactness = compactness;
1412 cluster_centers = centers;
1417 return (best_compactness);
1421 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1423 const Eigen::MatrixXf& data,
1424 Eigen::MatrixXf& out_centers,
1425 int number_of_clusters,
1428 std::size_t dimension = data.cols ();
1429 auto number_of_points =
static_cast<unsigned int> (data.rows ());
1430 std::vector<int> centers_vec (number_of_clusters);
1431 int* centers = centers_vec.data();
1432 std::vector<double> dist (number_of_points);
1433 std::vector<double> tdist (number_of_points);
1434 std::vector<double> tdist2 (number_of_points);
1437 unsigned int random_unsigned = rand ();
1438 centers[0] = random_unsigned % number_of_points;
1440 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1442 Eigen::VectorXf first (dimension);
1443 Eigen::VectorXf second (dimension);
1444 first = data.row (i_point);
1445 second = data.row (centers[0]);
1446 dist[i_point] = computeDistance (first, second);
1447 sum0 += dist[i_point];
1450 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1452 double best_sum = std::numeric_limits<double>::max ();
1453 int best_center = -1;
1454 for (
int i_trials = 0; i_trials < trials; i_trials++)
1456 unsigned int random_integer = rand () - 1;
1457 double random_double =
static_cast<double> (random_integer) /
static_cast<double> (std::numeric_limits<unsigned int>::max ());
1458 double p = random_double * sum0;
1460 unsigned int i_point;
1461 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1462 if ( (p -= dist[i_point]) <= 0.0)
1468 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1470 Eigen::VectorXf first (dimension);
1471 Eigen::VectorXf second (dimension);
1472 first = data.row (i_point);
1473 second = data.row (ci);
1474 tdist2[i_point] = std::min (
static_cast<double> (computeDistance (first, second)), dist[i_point]);
1475 s += tdist2[i_point];
1482 std::swap (tdist, tdist2);
1486 centers[i_cluster] = best_center;
1488 std::swap (dist, tdist);
1491 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1492 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1493 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1497 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1499 Eigen::VectorXf& center)
1501 std::size_t dimension = boxes.size ();
1502 float margin = 1.0f /
static_cast<float> (dimension);
1504 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1506 unsigned int random_integer = rand () - 1;
1507 float random_float =
static_cast<float> (random_integer) /
static_cast<float> (std::numeric_limits<unsigned int>::max ());
1508 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1513 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
1516 std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1518 for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1520 float diff = vec_1 (i_dim) - vec_2 (i_dim);
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
PointCloud represents the base class in PCL for storing collections of 3D points.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
iterator begin() noexcept
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
shared_ptr< const PointCloud< PointT > > ConstPtr
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed,...
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm.
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
shared_ptr< ISMVoteList< PointT > > Ptr
void addVote(pcl::InterestPoint &in_vote, const PointT &vote_origin, int in_class)
This method simply adds another vote to the list.
void findStrongestPeaks(std::vector< ISMPeak, Eigen::aligned_allocator< ISMPeak > > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma)
This method finds the strongest peaks (points were density has most higher values).
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud(typename pcl::PointCloud< PointT >::Ptr cloud=0)
Returns the colored cloud that consists of votes for center (blue points) and initial point cloud (if...
unsigned int getNumberOfVotes()
This method simply returns the number of votes.
void validateTree()
this method is simply setting up the search tree.
double getDensityAtPoint(const PointT &point, double sigma_dist)
Returns the density at the specified point.
virtual ~ISMVoteList()
virtual descriptor.
ISMVoteList()
Empty constructor with member variables initialization.
bool trainISM(ISMModelPtr &trained_model)
This method performs training and forms a visual vocabulary.
typename Feature::Ptr FeaturePtr
void applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
Eigen::Matrix3f alignYCoordWithNormal(const NormalT &in_normal)
This method simply computes the rotation matrix, so that the given normal would match the Y axis afte...
std::vector< typename pcl::PointCloud< PointT >::Ptr > getTrainingClouds()
This method simply returns the clouds that were set as the training clouds.
void generateRandomCenter(const std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > &boxes, Eigen::VectorXf ¢er)
Generates random center for cluster.
void setFeatureEstimator(FeaturePtr feature)
Changes the feature estimator.
void setTrainingClouds(const std::vector< typename pcl::PointCloud< PointT >::Ptr > &training_clouds)
Allows to set clouds for training the ISM model.
void setNVotState(bool state)
Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
float computeDistance(Eigen::VectorXf &vec_1, Eigen::VectorXf &vec_2)
Computes the square distance between two vectors.
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
void shiftCloud(typename pcl::PointCloud< PointT >::Ptr in_cloud, Eigen::Vector3f shift_point)
This method simply shifts the clouds points relative to the passed point.
pcl::features::ISMModel::Ptr ISMModelPtr
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
void setTrainingNormals(const std::vector< typename pcl::PointCloud< NormalT >::Ptr > &training_normals)
Allows to set normals for the training clouds that were passed through setTrainingClouds method.
void generateCentersPP(const Eigen::MatrixXf &data, Eigen::MatrixXf &out_centers, int number_of_clusters, int trials)
Generates centers for clusters as described in Arthur, David and Sergei Vassilvitski (2007) k-means++...
ImplicitShapeModelEstimation()
Simple constructor that initializes everything.
double computeKMeansClustering(const Eigen::MatrixXf &points_to_cluster, int number_of_clusters, Eigen::MatrixXi &io_labels, TermCriteria criteria, int attempts, int flags, Eigen::MatrixXf &cluster_centers)
Performs K-means clustering.
std::vector< unsigned int > getTrainingClasses()
Returns the array of classes that indicates which class the corresponding training cloud belongs.
void calculateWeights(const std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations, const Eigen::MatrixXi &labels, std::vector< float > &sigmas, std::vector< std::vector< unsigned int > > &clusters, std::vector< std::vector< float > > &statistical_weights, std::vector< float > &learned_weights)
This function forms a visual vocabulary and evaluates weights described in [Knopp et al....
void estimateFeatures(typename pcl::PointCloud< PointT >::Ptr sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr normal_cloud, typename pcl::PointCloud< pcl::Histogram< FeatureSize > >::Ptr feature_cloud)
This method estimates features for the given point cloud.
bool extractDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations)
Extracts the descriptors from the input clouds.
FeaturePtr getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
unsigned int getNumberOfClusters()
Returns the number of clusters used for descriptor clustering.
void calculateSigmas(std::vector< float > &sigmas)
This method calculates the value of sigma used for calculating the learned weights for every single c...
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
bool clusterDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, Eigen::MatrixXi &labels, Eigen::MatrixXf &clusters_centers)
This method performs descriptor clustering.
std::vector< float > getSigmaDists()
Returns the array of sigma values.
void setSamplingSize(float sampling_size)
Changes the sampling size used for cloud simplification.
pcl::features::ISMVoteList< PointT >::Ptr findObjects(ISMModelPtr model, typename pcl::PointCloud< PointT >::Ptr in_cloud, typename pcl::PointCloud< Normal >::Ptr in_normals, int in_class_of_interest)
This function is searching for the class of interest in a given cloud and returns the list of votes.
void simplifyCloud(typename pcl::PointCloud< PointT >::ConstPtr in_point_cloud, typename pcl::PointCloud< NormalT >::ConstPtr in_normal_cloud, typename pcl::PointCloud< PointT >::Ptr out_sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr out_sampled_normal_cloud)
Simplifies the cloud using voxel grid principles.
void setSigmaDists(const std::vector< float > &training_sigmas)
This method allows to set the value of sigma used for calculating the learned weights for every singl...
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the corresponding cloud of normals for every training point cloud.
void setTrainingClasses(const std::vector< unsigned int > &training_classes)
Allows to set the class labels for the corresponding training clouds.
float getSamplingSize()
Returns the sampling size used for cloud simplification.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
shared_ptr< pcl::search::Search< PointT > > Ptr
Defines functions, macros and traits for allocating and using memory.
float distance(const PointT &p1, const PointT &p2)
A point structure representing an N-D histogram.
This struct is used for storing peak.
double density
Density of this peak.
int class_id
Determines which class this peak belongs.
A point structure representing an interest point with Euclidean xyz coordinates, and an interest valu...
A point structure representing normal coordinates and the surface curvature estimate.
shared_ptr< ::pcl::PointIndices > Ptr
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.
The assignment of this structure is to store the statistical/learned weights and other information of...
unsigned int number_of_clusters_
Stores the number of clusters.
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
ISMModel()
Simple constructor that initializes the structure.
bool loadModelFromfile(std::string &file_name)
This method loads the trained model from file.
ISMModel & operator=(const ISMModel &other)
Operator overloading for deep copy.
virtual ~ISMModel()
Destructor that frees memory.
unsigned int descriptors_dimension_
Stores descriptors dimension.
std::vector< float > sigmas_
Stores the sigma value for each class.
std::vector< float > learned_weights_
Stores learned weights.
void reset()
this method resets all variables and frees memory.
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
std::vector< unsigned int > classes_
Stores the class label for every direction.
Eigen::MatrixXf directions_to_center_
Stores the directions to objects center for each visual word.
unsigned int number_of_classes_
Stores the number of classes.
std::vector< std::vector< float > > statistical_weights_
Stores statistical weights.
unsigned int number_of_visual_words_
Stores the number of visual words.
This structure stores the information about the keypoint.
This structure is used for determining the end of the k-means clustering process.
int type_
Flag that determines when the k-means clustering must be stopped.
float epsilon_
Defines the accuracy for k-means clustering.
int max_count_
Defines maximum number of iterations for k-means clustering.