41 #ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_
42 #define PCL_IMPLICIT_SHAPE_MODEL_HPP_
44 #include "../implicit_shape_model.h"
49 template <
typename Po
intT>
52 tree_is_valid_ (false),
61 template <
typename Po
intT>
64 votes_class_.clear ();
65 votes_origins_.reset ();
73 template <
typename Po
intT>
void
77 tree_is_valid_ =
false;
78 votes_->points.insert (votes_->points.end (), vote);
80 votes_origins_->points.push_back (vote_origin);
81 votes_class_.push_back (votes_class);
91 colored_cloud->
width = 1;
99 for (
const auto& i_point: *cloud)
104 colored_cloud->
points.push_back (point);
111 for (
const auto &i_vote : votes_->points)
116 colored_cloud->
points.push_back (point);
118 colored_cloud->
height += votes_->size ();
120 return (colored_cloud);
124 template <
typename Po
intT>
void
126 std::vector<
pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
128 double in_non_maxima_radius,
133 const std::size_t n_vote_classes = votes_class_.size ();
134 if (n_vote_classes == 0)
136 for (std::size_t i = 0; i < n_vote_classes ; i++)
137 assert ( votes_class_[i] == in_class_id );
141 const int NUM_INIT_PTS = 100;
142 double SIGMA_DIST = in_sigma;
143 const double FINAL_EPS = SIGMA_DIST / 100;
145 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
146 std::vector<double> peak_densities (NUM_INIT_PTS);
147 double max_density = -1.0;
148 for (
int i = 0; i < NUM_INIT_PTS; i++)
150 Eigen::Vector3f old_center;
151 const auto idx = votes_->size() * i / NUM_INIT_PTS;
152 Eigen::Vector3f curr_center = (*votes_)[idx].getVector3fMap();
156 old_center = curr_center;
157 curr_center = shiftMean (old_center, SIGMA_DIST);
158 }
while ((old_center - curr_center).norm () > FINAL_EPS);
161 point.x = curr_center (0);
162 point.y = curr_center (1);
163 point.z = curr_center (2);
164 double curr_density = getDensityAtPoint (point, SIGMA_DIST);
165 assert (curr_density >= 0.0);
167 peaks[i] = curr_center;
168 peak_densities[i] = curr_density;
170 if ( max_density < curr_density )
171 max_density = curr_density;
175 std::vector<bool> peak_flag (NUM_INIT_PTS,
true);
176 for (
int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
179 double best_density = -1.0;
180 Eigen::Vector3f strongest_peak;
181 int best_peak_ind (-1);
182 int peak_counter (0);
183 for (
int i = 0; i < NUM_INIT_PTS; i++)
188 if ( peak_densities[i] > best_density)
190 best_density = peak_densities[i];
191 strongest_peak = peaks[i];
197 if( peak_counter == 0 )
201 peak.x = strongest_peak(0);
202 peak.y = strongest_peak(1);
203 peak.z = strongest_peak(2);
206 out_peaks.push_back ( peak );
209 peak_flag[best_peak_ind] =
false;
210 for (
int i = 0; i < NUM_INIT_PTS; i++)
216 double dist = (strongest_peak - peaks[i]).norm ();
217 if ( dist < in_non_maxima_radius )
218 peak_flag[i] =
false;
224 template <
typename Po
intT>
void
229 if (tree_ ==
nullptr)
231 tree_->setInputCloud (votes_);
232 k_ind_.resize ( votes_->size (), -1 );
233 k_sqr_dist_.resize ( votes_->size (), 0.0f );
234 tree_is_valid_ =
true;
239 template <
typename Po
intT> Eigen::Vector3f
244 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
251 std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
253 for (std::size_t j = 0; j < n_pts; j++)
255 double kernel = (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
256 Eigen::Vector3f vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[k_ind_[j]].z);
257 wgh_sum += vote_vec *
static_cast<float> (
kernel);
260 assert (denom > 0.0);
262 return (wgh_sum /
static_cast<float> (denom));
266 template <
typename Po
intT>
double
268 const PointT &point,
double sigma_dist)
272 const std::size_t n_vote_classes = votes_class_.size ();
273 if (n_vote_classes == 0)
276 double sum_vote = 0.0;
282 std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
284 for (std::size_t j = 0; j < num_of_pts; j++)
285 sum_vote += (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
291 template <
typename Po
intT>
unsigned int
294 return (
static_cast<unsigned int> (votes_->size ()));
299 statistical_weights_ (0),
300 learned_weights_ (0),
304 number_of_classes_ (0),
305 number_of_visual_words_ (0),
306 number_of_clusters_ (0),
307 descriptors_dimension_ (0)
321 std::vector<float> vec;
322 vec.resize (this->number_of_clusters_, 0.0f);
323 this->statistical_weights_.resize (this->number_of_classes_, vec);
324 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
325 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
328 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
329 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
330 this->learned_weights_[i_visual_word] = copy.
learned_weights_[i_visual_word];
332 this->classes_.resize (this->number_of_visual_words_, 0);
333 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
334 this->classes_[i_visual_word] = copy.
classes_[i_visual_word];
336 this->sigmas_.resize (this->number_of_classes_, 0.0f);
337 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
338 this->sigmas_[i_class] = copy.
sigmas_[i_class];
340 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
341 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
342 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
343 this->directions_to_center_ (i_visual_word, i_dim) = copy.
directions_to_center_ (i_visual_word, i_dim);
345 this->clusters_centers_.resize (this->number_of_clusters_, 3);
346 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
347 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
348 this->clusters_centers_ (i_cluster, i_dim) = copy.
clusters_centers_ (i_cluster, i_dim);
361 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
364 output_file.close ();
368 output_file << number_of_classes_ <<
" ";
369 output_file << number_of_visual_words_ <<
" ";
370 output_file << number_of_clusters_ <<
" ";
371 output_file << descriptors_dimension_ <<
" ";
374 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
375 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
376 output_file << statistical_weights_[i_class][i_cluster] <<
" ";
379 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
380 output_file << learned_weights_[i_visual_word] <<
" ";
383 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
384 output_file << classes_[i_visual_word] <<
" ";
387 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
388 output_file << sigmas_[i_class] <<
" ";
391 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
392 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
393 output_file << directions_to_center_ (i_visual_word, i_dim) <<
" ";
396 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
397 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
398 output_file << clusters_centers_ (i_cluster, i_dim) <<
" ";
401 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
403 output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) <<
" ";
404 for (
const unsigned int &visual_word : clusters_[i_cluster])
405 output_file << visual_word <<
" ";
408 output_file.close ();
417 std::ifstream input_file (file_name.c_str ());
426 input_file.getline (line, 256,
' ');
427 number_of_classes_ =
static_cast<unsigned int> (strtol (line,
nullptr, 10));
428 input_file.getline (line, 256,
' '); number_of_visual_words_ = atoi (line);
429 input_file.getline (line, 256,
' '); number_of_clusters_ = atoi (line);
430 input_file.getline (line, 256,
' '); descriptors_dimension_ = atoi (line);
433 std::vector<float> vec;
434 vec.resize (number_of_clusters_, 0.0f);
435 statistical_weights_.resize (number_of_classes_, vec);
436 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
437 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
438 input_file >> statistical_weights_[i_class][i_cluster];
441 learned_weights_.resize (number_of_visual_words_, 0.0f);
442 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
443 input_file >> learned_weights_[i_visual_word];
446 classes_.resize (number_of_visual_words_, 0);
447 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
448 input_file >> classes_[i_visual_word];
451 sigmas_.resize (number_of_classes_, 0.0f);
452 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
453 input_file >> sigmas_[i_class];
456 directions_to_center_.resize (number_of_visual_words_, 3);
457 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
458 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
459 input_file >> directions_to_center_ (i_visual_word, i_dim);
462 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
463 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
464 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
465 input_file >> clusters_centers_ (i_cluster, i_dim);
468 std::vector<unsigned int> vect;
469 clusters_.resize (number_of_clusters_, vect);
470 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
472 unsigned int size_of_current_cluster = 0;
473 input_file >> size_of_current_cluster;
474 clusters_[i_cluster].resize (size_of_current_cluster, 0);
475 for (
unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
476 input_file >> clusters_[i_cluster][i_visual_word];
487 statistical_weights_.clear ();
488 learned_weights_.clear ();
491 directions_to_center_.resize (0, 0);
492 clusters_centers_.resize (0, 0);
494 number_of_classes_ = 0;
495 number_of_visual_words_ = 0;
496 number_of_clusters_ = 0;
497 descriptors_dimension_ = 0;
513 std::vector<float> vec;
514 vec.resize (number_of_clusters_, 0.0f);
515 this->statistical_weights_.resize (this->number_of_classes_, vec);
516 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
517 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
518 this->statistical_weights_[i_class][i_cluster] = other.
statistical_weights_[i_class][i_cluster];
520 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
521 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
522 this->learned_weights_[i_visual_word] = other.
learned_weights_[i_visual_word];
524 this->classes_.resize (this->number_of_visual_words_, 0);
525 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
526 this->classes_[i_visual_word] = other.
classes_[i_visual_word];
528 this->sigmas_.resize (this->number_of_classes_, 0.0f);
529 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
530 this->sigmas_[i_class] = other.
sigmas_[i_class];
532 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
533 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
534 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
535 this->directions_to_center_ (i_visual_word, i_dim) = other.
directions_to_center_ (i_visual_word, i_dim);
537 this->clusters_centers_.resize (this->number_of_clusters_, 3);
538 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
539 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
540 this->clusters_centers_ (i_cluster, i_dim) = other.
clusters_centers_ (i_cluster, i_dim);
546 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
548 training_clouds_ (0),
549 training_classes_ (0),
550 training_normals_ (0),
551 training_sigmas_ (0),
552 sampling_size_ (0.1f),
553 feature_estimator_ (),
554 number_of_clusters_ (184),
560 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
563 training_clouds_.clear ();
564 training_classes_.clear ();
565 training_normals_.clear ();
566 training_sigmas_.clear ();
567 feature_estimator_.reset ();
571 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
574 return (training_clouds_);
578 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
582 training_clouds_.clear ();
583 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
584 training_clouds_.swap (clouds);
588 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
591 return (training_classes_);
595 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
598 training_classes_.clear ();
599 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
600 training_classes_.swap (classes);
604 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
607 return (training_normals_);
611 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
615 training_normals_.clear ();
616 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
617 training_normals_.swap (normals);
621 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
624 return (sampling_size_);
628 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
631 if (sampling_size >= std::numeric_limits<float>::epsilon ())
632 sampling_size_ = sampling_size;
639 return (feature_estimator_);
643 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
646 feature_estimator_ = feature;
650 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int
653 return (number_of_clusters_);
657 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
660 if (num_of_clusters > 0)
661 number_of_clusters_ = num_of_clusters;
665 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
668 return (training_sigmas_);
672 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
675 training_sigmas_.clear ();
676 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
677 training_sigmas_.swap (sigmas);
681 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
688 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
695 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
700 if (trained_model ==
nullptr)
702 trained_model->reset ();
704 std::vector<pcl::Histogram<FeatureSize> > histograms;
705 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
706 success = extractDescriptors (histograms, locations);
710 Eigen::MatrixXi labels;
711 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
715 std::vector<unsigned int> vec;
716 trained_model->clusters_.resize (number_of_clusters_, vec);
717 for (std::size_t i_label = 0; i_label < locations.size (); i_label++)
718 trained_model->clusters_[labels (i_label)].push_back (i_label);
720 calculateSigmas (trained_model->sigmas_);
725 trained_model->sigmas_,
726 trained_model->clusters_,
727 trained_model->statistical_weights_,
728 trained_model->learned_weights_);
730 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
731 trained_model->number_of_visual_words_ =
static_cast<unsigned int> (histograms.size ());
732 trained_model->number_of_clusters_ = number_of_clusters_;
733 trained_model->descriptors_dimension_ = FeatureSize;
735 trained_model->directions_to_center_.resize (locations.size (), 3);
736 trained_model->classes_.resize (locations.size ());
737 for (std::size_t i_dir = 0; i_dir < locations.size (); i_dir++)
739 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
740 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
741 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
742 trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
754 int in_class_of_interest)
758 if (in_cloud->
points.empty ())
763 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
764 if (sampled_point_cloud->
points.empty ())
768 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
771 const unsigned int n_key_points =
static_cast<unsigned int> (sampled_point_cloud->
size ());
772 std::vector<int> min_dist_inds (n_key_points, -1);
773 for (
unsigned int i_point = 0; i_point < n_key_points; i_point++)
775 Eigen::VectorXf curr_descriptor (FeatureSize);
776 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
777 curr_descriptor (i_dim) = (*feature_cloud)[i_point].histogram[i_dim];
779 float descriptor_sum = curr_descriptor.sum ();
780 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
783 unsigned int min_dist_idx = 0;
784 Eigen::VectorXf clusters_center (FeatureSize);
785 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
786 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
788 float best_dist = computeDistance (curr_descriptor, clusters_center);
789 for (
unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
791 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
792 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
793 float curr_dist = computeDistance (clusters_center, curr_descriptor);
794 if (curr_dist < best_dist)
796 min_dist_idx = i_clust_cent;
797 best_dist = curr_dist;
800 min_dist_inds[i_point] = min_dist_idx;
803 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
805 int min_dist_idx = min_dist_inds[i_point];
806 if (min_dist_idx == -1)
809 const unsigned int n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
811 Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
812 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
814 unsigned int index = model->clusters_[min_dist_idx][i_word];
815 unsigned int i_class = model->classes_[index];
816 if (
static_cast<int> (i_class) != in_class_of_interest)
820 Eigen::Vector3f direction (
821 model->directions_to_center_(index, 0),
822 model->directions_to_center_(index, 1),
823 model->directions_to_center_(index, 2));
824 applyTransform (direction, transform.transpose ());
827 Eigen::Vector3f vote_pos = (*sampled_point_cloud)[i_point].getVector3fMap () + direction;
828 vote.x = vote_pos[0];
829 vote.y = vote_pos[1];
830 vote.z = vote_pos[2];
831 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
832 float learned_weight = model->learned_weights_[index];
833 float power = statistical_weight * learned_weight;
835 if (vote.
strength > std::numeric_limits<float>::epsilon ())
836 out_votes->
addVote (vote, (*sampled_point_cloud)[i_point], i_class);
844 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
847 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
852 int n_key_points = 0;
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);
876 n_key_points +=
static_cast<int> (sampled_point_cloud->
size ());
879 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
882 for (
auto point_i = sampled_point_cloud->
points.cbegin (); point_i != sampled_point_cloud->
points.cend (); point_i++, point_index++)
884 float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
885 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
888 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
890 int dist =
static_cast<int> (
std::distance (sampled_point_cloud->
points.cbegin (), point_i));
891 Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
892 Eigen::Vector3f zero;
896 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
897 applyTransform (new_dir, new_basis);
899 PointT point (new_dir[0], new_dir[1], new_dir[2]);
900 LocationInfo info (
static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
901 locations.insert(locations.end (), info);
909 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
912 Eigen::MatrixXi& labels,
913 Eigen::MatrixXf& clusters_centers)
915 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
917 for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
918 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
919 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
921 labels.resize (histograms.size(), 1);
922 computeKMeansClustering (
926 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
935 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
938 if (!training_sigmas_.empty ())
940 sigmas.resize (training_sigmas_.size (), 0.0f);
941 for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
942 sigmas[i_sigma] = training_sigmas_[i_sigma];
947 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
948 sigmas.resize (number_of_classes, 0.0f);
950 std::vector<float> vec;
951 std::vector<std::vector<float> > objects_sigmas;
952 objects_sigmas.resize (number_of_classes, vec);
954 unsigned int number_of_objects =
static_cast<unsigned int> (training_clouds_.size ());
955 for (
unsigned int i_object = 0; i_object < number_of_objects; i_object++)
957 float max_distance = 0.0f;
958 const auto number_of_points = training_clouds_[i_object]->size ();
959 for (
unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
960 for (
unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
962 float curr_distance = 0.0f;
963 curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
964 curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
965 curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
966 if (curr_distance > max_distance)
967 max_distance = curr_distance;
969 max_distance =
static_cast<float> (sqrt (max_distance));
970 unsigned int i_class = training_classes_[i_object];
971 objects_sigmas[i_class].push_back (max_distance);
974 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
977 unsigned int number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
978 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
979 sig += objects_sigmas[i_class][i_object];
980 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
981 sigmas[i_class] = sig;
986 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
988 const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
989 const Eigen::MatrixXi &labels,
990 std::vector<float>& sigmas,
991 std::vector<std::vector<unsigned int> >& clusters,
992 std::vector<std::vector<float> >& statistical_weights,
993 std::vector<float>& learned_weights)
995 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
997 std::vector<float> vec;
998 vec.resize (number_of_clusters_, 0.0f);
999 statistical_weights.clear ();
1000 learned_weights.clear ();
1001 statistical_weights.resize (number_of_classes, vec);
1002 learned_weights.resize (locations.size (), 0.0f);
1005 std::vector<int> vect;
1006 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
1009 std::vector<int> n_ftr;
1012 std::vector<int> n_vot;
1015 std::vector<int> n_vw;
1018 std::vector<std::vector<int> > n_vot_2;
1020 n_vot_2.resize (number_of_clusters_, vect);
1021 n_vot.resize (number_of_clusters_, 0);
1022 n_ftr.resize (number_of_classes, 0);
1023 for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
1025 int i_class = training_classes_[locations[i_location].model_num_];
1026 int i_cluster = labels (i_location);
1027 n_vot_2[i_cluster][i_class] += 1;
1028 n_vot[i_cluster] += 1;
1029 n_ftr[i_class] += 1;
1032 n_vw.resize (number_of_classes, 0);
1033 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1034 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1035 if (n_vot_2[i_cluster][i_class] > 0)
1039 learned_weights.resize (locations.size (), 0.0);
1040 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1042 unsigned int number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1043 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1045 unsigned int i_index = clusters[i_cluster][i_visual_word];
1046 int i_class = training_classes_[locations[i_index].model_num_];
1047 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1048 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1050 std::vector<float> calculated_sigmas;
1051 calculateSigmas (calculated_sigmas);
1052 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1053 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1056 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1057 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1058 applyTransform (direction, transform);
1059 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1062 std::vector<float> gauss_dists;
1063 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1065 unsigned int j_index = clusters[i_cluster][j_visual_word];
1066 int j_class = training_classes_[locations[j_index].model_num_];
1067 if (i_class != j_class)
1070 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1071 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1072 applyTransform (direction_2, transform_2);
1073 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1074 float residual = (predicted_center - actual_center).norm ();
1075 float value = -residual * residual / square_sigma_dist;
1076 gauss_dists.push_back (
static_cast<float> (std::exp (value)));
1079 std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1080 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1081 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1086 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1088 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1090 if (n_vot_2[i_cluster][i_class] == 0)
1092 if (n_vw[i_class] == 0)
1094 if (n_vot[i_cluster] == 0)
1096 if (n_ftr[i_class] == 0)
1098 float part_1 =
static_cast<float> (n_vw[i_class]);
1099 float part_2 =
static_cast<float> (n_vot[i_cluster]);
1100 float part_3 =
static_cast<float> (n_vot_2[i_cluster][i_class]) /
static_cast<float> (n_ftr[i_class]);
1101 float part_4 = 0.0f;
1106 for (
unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1107 if (n_ftr[j_class] != 0)
1108 part_4 +=
static_cast<float> (n_vot_2[i_cluster][j_class]) /
static_cast<float> (n_ftr[j_class]);
1110 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1116 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1125 grid.
setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1130 grid.
filter (temp_cloud);
1133 const float max_value = std::numeric_limits<float>::max ();
1135 const auto num_source_points = in_point_cloud->
size ();
1136 const auto num_sample_points = temp_cloud.
size ();
1138 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1139 std::vector<int> sampling_indices (num_sample_points, -1);
1141 for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1147 PointT pt_1 = (*in_point_cloud)[i_point];
1148 PointT pt_2 = temp_cloud[index];
1150 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);
1151 if (
distance < dist_to_grid_center[index])
1153 dist_to_grid_center[index] =
distance;
1154 sampling_indices[index] =
static_cast<int> (i_point);
1163 final_inliers_indices->indices.reserve (num_sample_points);
1164 for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1166 if (sampling_indices[i_point] != -1)
1167 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1171 extract_points.
setIndices (final_inliers_indices);
1172 extract_points.
filter (*out_sampled_point_cloud);
1175 extract_normals.
setIndices (final_inliers_indices);
1176 extract_normals.
filter (*out_sampled_normal_cloud);
1180 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1183 Eigen::Vector3f shift_point)
1185 for (
auto point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1187 point_it->x -= shift_point.x ();
1188 point_it->y -= shift_point.y ();
1189 point_it->z -= shift_point.z ();
1194 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1197 Eigen::Matrix3f result;
1198 Eigen::Matrix3f rotation_matrix_X;
1199 Eigen::Matrix3f rotation_matrix_Z;
1205 float denom_X =
static_cast<float> (sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1206 A = in_normal.normal_y / denom_X;
1207 B = sign * in_normal.normal_z / denom_X;
1208 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1212 float denom_Z =
static_cast<float> (sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1213 A = in_normal.normal_y / denom_Z;
1214 B = sign * in_normal.normal_x / denom_Z;
1215 rotation_matrix_Z << A, -
B, 0.0f,
1219 result = rotation_matrix_X * rotation_matrix_Z;
1225 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1228 io_vec = in_transform * io_vec;
1232 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1241 feature_estimator_->setInputCloud (sampled_point_cloud->
makeShared ());
1243 feature_estimator_->setSearchMethod (tree);
1250 dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1253 feature_estimator_->compute (*feature_cloud);
1257 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double
1259 const Eigen::MatrixXf& points_to_cluster,
1260 int number_of_clusters,
1261 Eigen::MatrixXi& io_labels,
1265 Eigen::MatrixXf& cluster_centers)
1267 const int spp_trials = 3;
1268 std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1269 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1271 attempts = std::max (attempts, 1);
1272 srand (
static_cast<unsigned int> (time (
nullptr)));
1274 Eigen::MatrixXi labels (number_of_points, 1);
1276 if (flags & USE_INITIAL_LABELS)
1281 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1282 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1283 std::vector<int> counters (number_of_clusters);
1284 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1285 Eigen::Vector2f* box = &boxes[0];
1287 double best_compactness = std::numeric_limits<double>::max ();
1288 double compactness = 0.0;
1290 if (criteria.
type_ & TermCriteria::EPS)
1293 criteria.
epsilon_ = std::numeric_limits<float>::epsilon ();
1297 if (criteria.
type_ & TermCriteria::COUNT)
1302 if (number_of_clusters == 1)
1308 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1309 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1311 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1312 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1314 float v = points_to_cluster (i_point, i_dim);
1315 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1316 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1319 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
1321 float max_center_shift = std::numeric_limits<float>::max ();
1322 for (
int iter = 0; iter < criteria.
max_count_ && max_center_shift > criteria.
epsilon_; iter++)
1324 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1326 centers = old_centers;
1329 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1331 if (flags & PP_CENTERS)
1332 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1335 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
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);
1347 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1348 counters[i_cluster] = 0;
1349 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1351 int i_label = labels (i_point, 0);
1352 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1353 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1354 counters[i_label]++;
1357 max_center_shift = 0.0f;
1358 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1360 if (counters[i_cl_center] != 0)
1362 float scale = 1.0f /
static_cast<float> (counters[i_cl_center]);
1363 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1364 centers (i_cl_center, i_dim) *= scale;
1368 Eigen::VectorXf center (feature_dimension);
1369 generateRandomCenter (boxes, center);
1370 for(
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1371 centers (i_cl_center, i_dim) = center (i_dim);
1377 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1379 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1380 dist += diff * diff;
1382 max_center_shift = std::max (max_center_shift, dist);
1387 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1389 Eigen::VectorXf sample (feature_dimension);
1390 sample = points_to_cluster.row (i_point);
1393 float min_dist = std::numeric_limits<float>::max ();
1395 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1397 Eigen::VectorXf center (feature_dimension);
1398 center = centers.row (i_cluster);
1399 float dist = computeDistance (sample, center);
1400 if (min_dist > dist)
1406 compactness += min_dist;
1407 labels (i_point, 0) = k_best;
1411 if (compactness < best_compactness)
1413 best_compactness = compactness;
1414 cluster_centers = centers;
1419 return (best_compactness);
1423 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1425 const Eigen::MatrixXf& data,
1426 Eigen::MatrixXf& out_centers,
1427 int number_of_clusters,
1430 std::size_t dimension = data.cols ();
1431 unsigned int number_of_points =
static_cast<unsigned int> (data.rows ());
1432 std::vector<int> centers_vec (number_of_clusters);
1433 int* centers = ¢ers_vec[0];
1434 std::vector<double> dist (number_of_points);
1435 std::vector<double> tdist (number_of_points);
1436 std::vector<double> tdist2 (number_of_points);
1439 unsigned int random_unsigned = rand ();
1440 centers[0] = random_unsigned % number_of_points;
1442 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1444 Eigen::VectorXf first (dimension);
1445 Eigen::VectorXf second (dimension);
1446 first = data.row (i_point);
1447 second = data.row (centers[0]);
1448 dist[i_point] = computeDistance (first, second);
1449 sum0 += dist[i_point];
1452 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1454 double best_sum = std::numeric_limits<double>::max ();
1455 int best_center = -1;
1456 for (
int i_trials = 0; i_trials < trials; i_trials++)
1458 unsigned int random_integer = rand () - 1;
1459 double random_double =
static_cast<double> (random_integer) /
static_cast<double> (std::numeric_limits<unsigned int>::max ());
1460 double p = random_double * sum0;
1462 unsigned int i_point;
1463 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1464 if ( (p -= dist[i_point]) <= 0.0)
1470 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1472 Eigen::VectorXf first (dimension);
1473 Eigen::VectorXf second (dimension);
1474 first = data.row (i_point);
1475 second = data.row (ci);
1476 tdist2[i_point] = std::min (
static_cast<double> (computeDistance (first, second)), dist[i_point]);
1477 s += tdist2[i_point];
1484 std::swap (tdist, tdist2);
1488 centers[i_cluster] = best_center;
1490 std::swap (dist, tdist);
1493 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1494 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1495 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1499 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1501 Eigen::VectorXf& center)
1503 std::size_t dimension = boxes.size ();
1504 float margin = 1.0f /
static_cast<float> (dimension);
1506 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1508 unsigned int random_integer = rand () - 1;
1509 float random_float =
static_cast<float> (random_integer) /
static_cast<float> (std::numeric_limits<unsigned int>::max ());
1510 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1515 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
1518 std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1520 for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1522 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(std::vector< int > &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.