41 #ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_ 42 #define PCL_IMPLICIT_SHAPE_MODEL_HPP_ 44 #include "../implicit_shape_model.h" 47 template <
typename Po
intT>
50 tree_is_valid_ (false),
60 template <
typename Po
intT>
63 votes_class_.clear ();
64 votes_origins_.reset ();
72 template <
typename Po
intT>
void 76 tree_is_valid_ =
false;
77 votes_->points.insert (votes_->points.end (), vote);
79 votes_origins_->points.push_back (vote_origin);
80 votes_class_.push_back (votes_class);
90 colored_cloud->
width = 1;
94 colored_cloud->
height +=
static_cast<uint32_t
> (cloud->
points.size ());
98 for (
size_t i_point = 0; i_point < cloud->
points.size (); i_point++)
100 point.x = cloud->
points[i_point].x;
101 point.y = cloud->
points[i_point].y;
102 point.z = cloud->
points[i_point].z;
103 colored_cloud->
points.push_back (point);
110 for (
size_t i_vote = 0; i_vote < votes_->points.size (); i_vote++)
112 point.x = votes_->points[i_vote].x;
113 point.y = votes_->points[i_vote].y;
114 point.z = votes_->points[i_vote].z;
115 colored_cloud->
points.push_back (point);
117 colored_cloud->
height +=
static_cast<uint32_t
> (votes_->points.size ());
119 return (colored_cloud);
123 template <
typename Po
intT>
void 125 std::vector<
pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
127 double in_non_maxima_radius,
132 const size_t n_vote_classes = votes_class_.size ();
133 if (n_vote_classes == 0)
135 for (
size_t i = 0; i < n_vote_classes ; i++)
136 assert ( votes_class_[i] == in_class_id );
140 const int NUM_INIT_PTS = 100;
141 double SIGMA_DIST = in_sigma;
142 const double FINAL_EPS = SIGMA_DIST / 100;
144 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
145 std::vector<double> peak_densities (NUM_INIT_PTS);
146 double max_density = -1.0;
147 for (
int i = 0; i < NUM_INIT_PTS; i++)
149 Eigen::Vector3f old_center;
150 Eigen::Vector3f curr_center;
151 curr_center (0) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].x;
152 curr_center (1) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].y;
153 curr_center (2) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].z;
157 old_center = curr_center;
158 curr_center = shiftMean (old_center, SIGMA_DIST);
159 }
while ((old_center - curr_center).norm () > FINAL_EPS);
162 point.x = curr_center (0);
163 point.y = curr_center (1);
164 point.z = curr_center (2);
165 double curr_density = getDensityAtPoint (point, SIGMA_DIST);
166 assert (curr_density >= 0.0);
168 peaks[i] = curr_center;
169 peak_densities[i] = curr_density;
171 if ( max_density < curr_density )
172 max_density = curr_density;
176 std::vector<bool> peak_flag (NUM_INIT_PTS,
true);
177 for (
int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
180 double best_density = -1.0;
181 Eigen::Vector3f strongest_peak;
182 int best_peak_ind (-1);
183 int peak_counter (0);
184 for (
int i = 0; i < NUM_INIT_PTS; i++)
189 if ( peak_densities[i] > best_density)
191 best_density = peak_densities[i];
192 strongest_peak = peaks[i];
198 if( peak_counter == 0 )
202 peak.x = strongest_peak(0);
203 peak.y = strongest_peak(1);
204 peak.z = strongest_peak(2);
207 out_peaks.push_back ( peak );
210 peak_flag[best_peak_ind] =
false;
211 for (
int i = 0; i < NUM_INIT_PTS; i++)
217 double dist = (strongest_peak - peaks[i]).norm ();
218 if ( dist < in_non_maxima_radius )
219 peak_flag[i] =
false;
225 template <
typename Po
intT>
void 232 tree_->setInputCloud (votes_);
233 k_ind_.resize ( votes_->points.size (), -1 );
234 k_sqr_dist_.resize ( votes_->points.size (), 0.0f );
235 tree_is_valid_ =
true;
240 template <
typename Po
intT> Eigen::Vector3f
245 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
252 size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
254 for (
size_t j = 0; j < n_pts; j++)
256 double kernel = votes_->points[k_ind_[j]].strength * exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
257 Eigen::Vector3f vote_vec (votes_->points[k_ind_[j]].x, votes_->points[k_ind_[j]].y, votes_->points[k_ind_[j]].z);
258 wgh_sum += vote_vec *
static_cast<float> (
kernel);
261 assert (denom > 0.0);
263 return (wgh_sum / static_cast<float> (denom));
267 template <
typename Po
intT>
double 269 const PointT &point,
double sigma_dist)
273 const size_t n_vote_classes = votes_class_.size ();
274 if (n_vote_classes == 0)
277 double sum_vote = 0.0;
283 size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
285 for (
size_t j = 0; j < num_of_pts; j++)
286 sum_vote += votes_->points[k_ind_[j]].strength * exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
292 template <
typename Po
intT>
unsigned int 295 return (static_cast<unsigned int> (votes_->points.size ()));
300 statistical_weights_ (0),
301 learned_weights_ (0),
304 directions_to_center_ (),
305 clusters_centers_ (),
307 number_of_classes_ (0),
308 number_of_visual_words_ (0),
309 number_of_clusters_ (0),
310 descriptors_dimension_ (0)
324 std::vector<float> vec;
325 vec.resize (this->number_of_clusters_, 0.0f);
326 this->statistical_weights_.resize (this->number_of_classes_, vec);
327 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
328 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
331 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
332 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
333 this->learned_weights_[i_visual_word] = copy.
learned_weights_[i_visual_word];
335 this->classes_.resize (this->number_of_visual_words_, 0);
336 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
337 this->classes_[i_visual_word] = copy.
classes_[i_visual_word];
339 this->sigmas_.resize (this->number_of_classes_, 0.0f);
340 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
341 this->sigmas_[i_class] = copy.
sigmas_[i_class];
343 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
344 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
345 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
346 this->directions_to_center_ (i_visual_word, i_dim) = copy.
directions_to_center_ (i_visual_word, i_dim);
348 this->clusters_centers_.resize (this->number_of_clusters_, 3);
349 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
350 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
351 this->clusters_centers_ (i_cluster, i_dim) = copy.
clusters_centers_ (i_cluster, i_dim);
364 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
367 output_file.close ();
371 output_file << number_of_classes_ <<
" ";
372 output_file << number_of_visual_words_ <<
" ";
373 output_file << number_of_clusters_ <<
" ";
374 output_file << descriptors_dimension_ <<
" ";
377 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
378 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
379 output_file << statistical_weights_[i_class][i_cluster] <<
" ";
382 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
383 output_file << learned_weights_[i_visual_word] <<
" ";
386 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
387 output_file << classes_[i_visual_word] <<
" ";
390 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
391 output_file << sigmas_[i_class] <<
" ";
394 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
395 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
396 output_file << directions_to_center_ (i_visual_word, i_dim) <<
" ";
399 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
400 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
401 output_file << clusters_centers_ (i_cluster, i_dim) <<
" ";
404 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
406 output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) <<
" ";
407 for (
unsigned int i_visual_word = 0; i_visual_word < static_cast<unsigned int> (clusters_[i_cluster].size ()); i_visual_word++)
408 output_file << clusters_[i_cluster][i_visual_word] <<
" ";
411 output_file.close ();
420 std::ifstream input_file (file_name.c_str ());
429 input_file.getline (line, 256,
' ');
430 number_of_classes_ =
static_cast<unsigned int> (strtol (line, 0, 10));
431 input_file.getline (line, 256,
' '); number_of_visual_words_ = atoi (line);
432 input_file.getline (line, 256,
' '); number_of_clusters_ = atoi (line);
433 input_file.getline (line, 256,
' '); descriptors_dimension_ = atoi (line);
436 std::vector<float> vec;
437 vec.resize (number_of_clusters_, 0.0f);
438 statistical_weights_.resize (number_of_classes_, vec);
439 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
440 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
441 input_file >> statistical_weights_[i_class][i_cluster];
444 learned_weights_.resize (number_of_visual_words_, 0.0f);
445 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
446 input_file >> learned_weights_[i_visual_word];
449 classes_.resize (number_of_visual_words_, 0);
450 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
451 input_file >> classes_[i_visual_word];
454 sigmas_.resize (number_of_classes_, 0.0f);
455 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
456 input_file >> sigmas_[i_class];
459 directions_to_center_.resize (number_of_visual_words_, 3);
460 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
461 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
462 input_file >> directions_to_center_ (i_visual_word, i_dim);
465 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
466 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
467 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
468 input_file >> clusters_centers_ (i_cluster, i_dim);
471 std::vector<unsigned int> vect;
472 clusters_.resize (number_of_clusters_, vect);
473 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
475 unsigned int size_of_current_cluster = 0;
476 input_file >> size_of_current_cluster;
477 clusters_[i_cluster].resize (size_of_current_cluster, 0);
478 for (
unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
479 input_file >> clusters_[i_cluster][i_visual_word];
490 statistical_weights_.clear ();
491 learned_weights_.clear ();
494 directions_to_center_.resize (0, 0);
495 clusters_centers_.resize (0, 0);
497 number_of_classes_ = 0;
498 number_of_visual_words_ = 0;
499 number_of_clusters_ = 0;
500 descriptors_dimension_ = 0;
516 std::vector<float> vec;
517 vec.resize (number_of_clusters_, 0.0f);
518 this->statistical_weights_.resize (this->number_of_classes_, vec);
519 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
520 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
521 this->statistical_weights_[i_class][i_cluster] = other.
statistical_weights_[i_class][i_cluster];
523 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
524 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
525 this->learned_weights_[i_visual_word] = other.
learned_weights_[i_visual_word];
527 this->classes_.resize (this->number_of_visual_words_, 0);
528 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
529 this->classes_[i_visual_word] = other.
classes_[i_visual_word];
531 this->sigmas_.resize (this->number_of_classes_, 0.0f);
532 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
533 this->sigmas_[i_class] = other.
sigmas_[i_class];
535 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
536 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
537 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
538 this->directions_to_center_ (i_visual_word, i_dim) = other.
directions_to_center_ (i_visual_word, i_dim);
540 this->clusters_centers_.resize (this->number_of_clusters_, 3);
541 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
542 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
543 this->clusters_centers_ (i_cluster, i_dim) = other.
clusters_centers_ (i_cluster, i_dim);
549 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
551 training_clouds_ (0),
552 training_classes_ (0),
553 training_normals_ (0),
554 training_sigmas_ (0),
555 sampling_size_ (0.1f),
556 feature_estimator_ (),
557 number_of_clusters_ (184),
563 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
566 training_clouds_.clear ();
567 training_classes_.clear ();
568 training_normals_.clear ();
569 training_sigmas_.clear ();
570 feature_estimator_.reset ();
574 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
577 return (training_clouds_);
581 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 585 training_clouds_.clear ();
586 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
587 training_clouds_.swap (clouds);
591 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
594 return (training_classes_);
598 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 601 training_classes_.clear ();
602 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
603 training_classes_.swap (classes);
607 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
610 return (training_normals_);
614 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 618 training_normals_.clear ();
619 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
620 training_normals_.swap (normals);
624 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float 627 return (sampling_size_);
631 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 634 if (sampling_size >= std::numeric_limits<float>::epsilon ())
635 sampling_size_ = sampling_size;
639 template <
int FeatureSize,
typename Po
intT,
typename NormalT> boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > >
642 return (feature_estimator_);
646 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 650 feature_estimator_ = feature;
654 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int 657 return (number_of_clusters_);
661 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 664 if (num_of_clusters > 0)
665 number_of_clusters_ = num_of_clusters;
669 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
672 return (training_sigmas_);
676 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 679 training_sigmas_.clear ();
680 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
681 training_sigmas_.swap (sigmas);
685 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 692 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 699 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 704 if (trained_model == 0)
706 trained_model->reset ();
708 std::vector<pcl::Histogram<FeatureSize> > histograms;
709 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
710 success = extractDescriptors (histograms, locations);
714 Eigen::MatrixXi labels;
715 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
719 std::vector<unsigned int> vec;
720 trained_model->clusters_.resize (number_of_clusters_, vec);
721 for (
size_t i_label = 0; i_label < locations.size (); i_label++)
722 trained_model->clusters_[labels (i_label)].push_back (i_label);
724 calculateSigmas (trained_model->sigmas_);
729 trained_model->sigmas_,
730 trained_model->clusters_,
731 trained_model->statistical_weights_,
732 trained_model->learned_weights_);
734 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
735 trained_model->number_of_visual_words_ =
static_cast<unsigned int> (histograms.size ());
736 trained_model->number_of_clusters_ = number_of_clusters_;
737 trained_model->descriptors_dimension_ = FeatureSize;
739 trained_model->directions_to_center_.resize (locations.size (), 3);
740 trained_model->classes_.resize (locations.size ());
741 for (
size_t i_dir = 0; i_dir < locations.size (); i_dir++)
743 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
744 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
745 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
746 trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
753 template <
int FeatureSize,
typename Po
intT,
typename NormalT> boost::shared_ptr<pcl::features::ISMVoteList<PointT> >
758 int in_class_of_interest)
762 if (in_cloud->
points.size () == 0)
767 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
768 if (sampled_point_cloud->
points.size () == 0)
772 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
775 const unsigned int n_key_points =
static_cast<unsigned int> (sampled_point_cloud->
size ());
776 std::vector<int> min_dist_inds (n_key_points, -1);
777 for (
unsigned int i_point = 0; i_point < n_key_points; i_point++)
779 Eigen::VectorXf curr_descriptor (FeatureSize);
780 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
781 curr_descriptor (i_dim) = feature_cloud->
points[i_point].histogram[i_dim];
783 float descriptor_sum = curr_descriptor.sum ();
784 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
787 unsigned int min_dist_idx = 0;
788 Eigen::VectorXf clusters_center (FeatureSize);
789 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
790 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
792 float best_dist = computeDistance (curr_descriptor, clusters_center);
793 for (
unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
795 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
796 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
797 float curr_dist = computeDistance (clusters_center, curr_descriptor);
798 if (curr_dist < best_dist)
800 min_dist_idx = i_clust_cent;
801 best_dist = curr_dist;
804 min_dist_inds[i_point] = min_dist_idx;
807 for (
size_t i_point = 0; i_point < n_key_points; i_point++)
809 int min_dist_idx = min_dist_inds[i_point];
810 if (min_dist_idx == -1)
813 const unsigned int n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
815 Eigen::Matrix3f transform = alignYCoordWithNormal (sampled_normal_cloud->
points[i_point]);
816 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
818 unsigned int index = model->clusters_[min_dist_idx][i_word];
819 unsigned int i_class = model->classes_[index];
820 if (static_cast<int> (i_class) != in_class_of_interest)
824 Eigen::Vector3f direction (
825 model->directions_to_center_(index, 0),
826 model->directions_to_center_(index, 1),
827 model->directions_to_center_(index, 2));
828 applyTransform (direction, transform.transpose ());
831 Eigen::Vector3f vote_pos = sampled_point_cloud->
points[i_point].getVector3fMap () + direction;
832 vote.x = vote_pos[0];
833 vote.y = vote_pos[1];
834 vote.z = vote_pos[2];
835 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
836 float learned_weight = model->learned_weights_[index];
837 float power = statistical_weight * learned_weight;
838 vote.strength = power;
839 if (vote.strength > std::numeric_limits<float>::epsilon ())
840 out_votes->addVote (vote, sampled_point_cloud->
points[i_point], i_class);
848 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 851 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
856 int n_key_points = 0;
858 if (training_clouds_.size () == 0 || training_classes_.size () == 0 || feature_estimator_ == 0)
861 for (
size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
864 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
865 const size_t num_of_points = training_clouds_[i_cloud]->points.size ();
867 for (point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->
end (); point_j++)
868 models_center += point_j->getVector3fMap ();
869 models_center /=
static_cast<float> (num_of_points);
874 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
875 if (sampled_point_cloud->
points.size () == 0)
878 shiftCloud (training_clouds_[i_cloud], models_center);
879 shiftCloud (sampled_point_cloud, models_center);
881 n_key_points +=
static_cast<int> (sampled_point_cloud->
size ());
884 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
888 for (point_i = sampled_point_cloud->
points.begin (); point_i != sampled_point_cloud->
points.end (); point_i++, point_index++)
890 float descriptor_sum = Eigen::VectorXf::Map (feature_cloud->
points[point_index].histogram, FeatureSize).sum ();
891 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
894 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
896 int dist =
static_cast<int> (
std::distance (sampled_point_cloud->
points.begin (), point_i));
897 Eigen::Matrix3f new_basis = alignYCoordWithNormal (sampled_normal_cloud->
points[dist]);
898 Eigen::Vector3f zero;
902 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
903 applyTransform (new_dir, new_basis);
905 PointT point (new_dir[0], new_dir[1], new_dir[2]);
906 LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, sampled_normal_cloud->
points[dist]);
907 locations.insert(locations.end (), info);
915 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 918 Eigen::MatrixXi& labels,
919 Eigen::MatrixXf& clusters_centers)
921 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
923 for (
unsigned int i_feature = 0; i_feature < histograms.size (); i_feature++)
924 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
925 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
927 labels.resize (histograms.size(), 1);
928 computeKMeansClustering (
932 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
941 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 944 if (training_sigmas_.size () != 0)
946 sigmas.resize (training_sigmas_.size (), 0.0f);
947 for (
unsigned int i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
948 sigmas[i_sigma] = training_sigmas_[i_sigma];
953 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
954 sigmas.resize (number_of_classes, 0.0f);
956 std::vector<float> vec;
957 std::vector<std::vector<float> > objects_sigmas;
958 objects_sigmas.resize (number_of_classes, vec);
960 unsigned int number_of_objects =
static_cast<unsigned int> (training_clouds_.size ());
961 for (
unsigned int i_object = 0; i_object < number_of_objects; i_object++)
963 float max_distance = 0.0f;
964 unsigned int number_of_points =
static_cast<unsigned int> (training_clouds_[i_object]->points.size ());
965 for (
unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
966 for (
unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
968 float curr_distance = 0.0f;
969 curr_distance += training_clouds_[i_object]->points[i_point].x * training_clouds_[i_object]->points[j_point].x;
970 curr_distance += training_clouds_[i_object]->points[i_point].y * training_clouds_[i_object]->points[j_point].y;
971 curr_distance += training_clouds_[i_object]->points[i_point].z * training_clouds_[i_object]->points[j_point].z;
972 if (curr_distance > max_distance)
973 max_distance = curr_distance;
975 max_distance =
static_cast<float> (sqrt (max_distance));
976 unsigned int i_class = training_classes_[i_object];
977 objects_sigmas[i_class].push_back (max_distance);
980 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
983 unsigned int number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
984 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
985 sig += objects_sigmas[i_class][i_object];
986 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
987 sigmas[i_class] = sig;
992 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 994 const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
995 const Eigen::MatrixXi &labels,
996 std::vector<float>& sigmas,
997 std::vector<std::vector<unsigned int> >& clusters,
998 std::vector<std::vector<float> >& statistical_weights,
999 std::vector<float>& learned_weights)
1001 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
1003 std::vector<float> vec;
1004 vec.resize (number_of_clusters_, 0.0f);
1005 statistical_weights.clear ();
1006 learned_weights.clear ();
1007 statistical_weights.resize (number_of_classes, vec);
1008 learned_weights.resize (locations.size (), 0.0f);
1011 std::vector<int> vect;
1012 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
1015 std::vector<int> n_ftr;
1018 std::vector<int> n_vot;
1021 std::vector<int> n_vw;
1024 std::vector<std::vector<int> > n_vot_2;
1026 n_vot_2.resize (number_of_clusters_, vect);
1027 n_vot.resize (number_of_clusters_, 0);
1028 n_ftr.resize (number_of_classes, 0);
1029 for (
size_t i_location = 0; i_location < locations.size (); i_location++)
1031 int i_class = training_classes_[locations[i_location].model_num_];
1032 int i_cluster = labels (i_location);
1033 n_vot_2[i_cluster][i_class] += 1;
1034 n_vot[i_cluster] += 1;
1035 n_ftr[i_class] += 1;
1038 n_vw.resize (number_of_classes, 0);
1039 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1040 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1041 if (n_vot_2[i_cluster][i_class] > 0)
1045 learned_weights.resize (locations.size (), 0.0);
1046 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1048 unsigned int number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1049 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1051 unsigned int i_index = clusters[i_cluster][i_visual_word];
1052 int i_class = training_classes_[locations[i_index].model_num_];
1053 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1054 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1056 std::vector<float> calculated_sigmas;
1057 calculateSigmas (calculated_sigmas);
1058 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1059 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1062 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1063 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1064 applyTransform (direction, transform);
1065 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1068 std::vector<float> gauss_dists;
1069 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1071 unsigned int j_index = clusters[i_cluster][j_visual_word];
1072 int j_class = training_classes_[locations[j_index].model_num_];
1073 if (i_class != j_class)
1076 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1077 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1078 applyTransform (direction_2, transform_2);
1079 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1080 float residual = (predicted_center - actual_center).norm ();
1081 float value = -residual * residual / square_sigma_dist;
1082 gauss_dists.push_back (static_cast<float> (exp (value)));
1085 size_t mid_elem = (gauss_dists.size () - 1) / 2;
1086 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1087 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1092 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1094 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1096 if (n_vot_2[i_cluster][i_class] == 0)
1098 if (n_vw[i_class] == 0)
1100 if (n_vot[i_cluster] == 0)
1102 if (n_ftr[i_class] == 0)
1104 float part_1 =
static_cast<float> (n_vw[i_class]);
1105 float part_2 =
static_cast<float> (n_vot[i_cluster]);
1106 float part_3 =
static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1107 float part_4 = 0.0f;
1112 for (
unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1113 if (n_ftr[j_class] != 0)
1114 part_4 +=
static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1116 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1122 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1131 grid.
setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1136 grid.
filter (temp_cloud);
1139 const float max_value = std::numeric_limits<float>::max ();
1141 const size_t num_source_points = in_point_cloud->
points.size ();
1142 const size_t num_sample_points = temp_cloud.
points.size ();
1144 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1145 std::vector<int> sampling_indices (num_sample_points, -1);
1147 for (
size_t i_point = 0; i_point < num_source_points; i_point++)
1156 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);
1157 if (
distance < dist_to_grid_center[index])
1159 dist_to_grid_center[index] =
distance;
1160 sampling_indices[index] =
static_cast<int> (i_point);
1169 final_inliers_indices->indices.reserve (num_sample_points);
1170 for (
size_t i_point = 0; i_point < num_sample_points; i_point++)
1172 if (sampling_indices[i_point] != -1)
1173 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1177 extract_points.
setIndices (final_inliers_indices);
1178 extract_points.
filter (*out_sampled_point_cloud);
1181 extract_normals.
setIndices (final_inliers_indices);
1182 extract_normals.
filter (*out_sampled_normal_cloud);
1186 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1189 Eigen::Vector3f shift_point)
1192 for (point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1194 point_it->x -= shift_point.x ();
1195 point_it->y -= shift_point.y ();
1196 point_it->z -= shift_point.z ();
1201 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1204 Eigen::Matrix3f result;
1205 Eigen::Matrix3f rotation_matrix_X;
1206 Eigen::Matrix3f rotation_matrix_Z;
1212 float denom_X =
static_cast<float> (sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1213 A = in_normal.normal_y / denom_X;
1214 B = sign * in_normal.normal_z / denom_X;
1215 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1219 float denom_Z =
static_cast<float> (sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1220 A = in_normal.normal_y / denom_Z;
1221 B = sign * in_normal.normal_x / denom_Z;
1222 rotation_matrix_Z << A, -
B, 0.0f,
1226 result = rotation_matrix_X * rotation_matrix_Z;
1232 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1235 io_vec = in_transform * io_vec;
1239 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1248 feature_estimator_->setInputCloud (sampled_point_cloud->
makeShared ());
1250 feature_estimator_->setSearchMethod (tree);
1260 feature_estimator_->compute (*feature_cloud);
1264 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double 1266 const Eigen::MatrixXf& points_to_cluster,
1267 int number_of_clusters,
1268 Eigen::MatrixXi& io_labels,
1272 Eigen::MatrixXf& cluster_centers)
1274 const int spp_trials = 3;
1275 size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1276 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1278 attempts = std::max (attempts, 1);
1279 srand (static_cast<unsigned int> (time (0)));
1281 Eigen::MatrixXi labels (number_of_points, 1);
1283 if (flags & USE_INITIAL_LABELS)
1288 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1289 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1290 std::vector<int> counters (number_of_clusters);
1291 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1292 Eigen::Vector2f* box = &boxes[0];
1294 double best_compactness = std::numeric_limits<double>::max ();
1295 double compactness = 0.0;
1297 if (criteria.type_ & TermCriteria::EPS)
1298 criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f);
1300 criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1302 criteria.epsilon_ *= criteria.epsilon_;
1304 if (criteria.type_ & TermCriteria::COUNT)
1305 criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100);
1307 criteria.max_count_ = 100;
1309 if (number_of_clusters == 1)
1312 criteria.max_count_ = 2;
1315 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1316 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1318 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1319 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1321 float v = points_to_cluster (i_point, i_dim);
1322 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1323 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1326 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
1328 float max_center_shift = std::numeric_limits<float>::max ();
1329 for (
int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++)
1331 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1333 centers = old_centers;
1336 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1338 if (flags & PP_CENTERS)
1339 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1342 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1344 Eigen::VectorXf center (feature_dimension);
1345 generateRandomCenter (boxes, center);
1346 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1347 centers (i_cl_center, i_dim) = center (i_dim);
1354 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1355 counters[i_cluster] = 0;
1356 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1358 int i_label = labels (i_point, 0);
1359 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1360 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1361 counters[i_label]++;
1364 max_center_shift = 0.0f;
1365 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1367 if (counters[i_cl_center] != 0)
1369 float scale = 1.0f /
static_cast<float> (counters[i_cl_center]);
1370 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1371 centers (i_cl_center, i_dim) *= scale;
1375 Eigen::VectorXf center (feature_dimension);
1376 generateRandomCenter (boxes, center);
1377 for(
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1378 centers (i_cl_center, i_dim) = center (i_dim);
1384 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1386 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1387 dist += diff * diff;
1389 max_center_shift = std::max (max_center_shift, dist);
1394 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1396 Eigen::VectorXf sample (feature_dimension);
1397 sample = points_to_cluster.row (i_point);
1400 float min_dist = std::numeric_limits<float>::max ();
1402 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1404 Eigen::VectorXf center (feature_dimension);
1405 center = centers.row (i_cluster);
1406 float dist = computeDistance (sample, center);
1407 if (min_dist > dist)
1413 compactness += min_dist;
1414 labels (i_point, 0) = k_best;
1418 if (compactness < best_compactness)
1420 best_compactness = compactness;
1421 cluster_centers = centers;
1426 return (best_compactness);
1430 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1432 const Eigen::MatrixXf& data,
1433 Eigen::MatrixXf& out_centers,
1434 int number_of_clusters,
1437 size_t dimension = data.cols ();
1438 unsigned int number_of_points =
static_cast<unsigned int> (data.rows ());
1439 std::vector<int> centers_vec (number_of_clusters);
1440 int* centers = ¢ers_vec[0];
1441 std::vector<double> dist (number_of_points);
1442 std::vector<double> tdist (number_of_points);
1443 std::vector<double> tdist2 (number_of_points);
1446 unsigned int random_unsigned = rand ();
1447 centers[0] = random_unsigned % number_of_points;
1449 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1451 Eigen::VectorXf first (dimension);
1452 Eigen::VectorXf second (dimension);
1453 first = data.row (i_point);
1454 second = data.row (centers[0]);
1455 dist[i_point] = computeDistance (first, second);
1456 sum0 += dist[i_point];
1459 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1461 double best_sum = std::numeric_limits<double>::max ();
1462 int best_center = -1;
1463 for (
int i_trials = 0; i_trials < trials; i_trials++)
1465 unsigned int random_integer = rand () - 1;
1466 double random_double =
static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1467 double p = random_double * sum0;
1469 unsigned int i_point;
1470 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1471 if ( (p -= dist[i_point]) <= 0.0)
1477 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1479 Eigen::VectorXf first (dimension);
1480 Eigen::VectorXf second (dimension);
1481 first = data.row (i_point);
1482 second = data.row (ci);
1483 tdist2[i_point] = std::min (static_cast<double> (computeDistance (first, second)), dist[i_point]);
1484 s += tdist2[i_point];
1491 std::swap (tdist, tdist2);
1495 centers[i_cluster] = best_center;
1497 std::swap (dist, tdist);
1500 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1501 for (
unsigned int i_dim = 0; i_dim < dimension; i_dim++)
1502 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1506 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1508 Eigen::VectorXf& center)
1510 size_t dimension = boxes.size ();
1511 float margin = 1.0f /
static_cast<float> (dimension);
1513 for (
unsigned int i_dim = 0; i_dim < dimension; i_dim++)
1515 unsigned int random_integer = rand () - 1;
1516 float random_float =
static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1517 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1522 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float 1525 size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1527 for(
unsigned int i_dim = 0; i_dim < dimension; i_dim++)
1529 float diff = vec_1 (i_dim) - vec_2 (i_dim);
1536 #endif //#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_ A point structure representing normal coordinates and the surface curvature estimate.
void generateRandomCenter(const std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > &boxes, Eigen::VectorXf ¢er)
Generates random center for cluster.
std::vector< typename pcl::PointCloud< PointT >::Ptr > getTrainingClouds()
This method simply returns the clouds that were set as the training clouds.
std::vector< float > learned_weights_
Stores learned weights.
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
ISMVoteList()
Empty constructor with member variables initialization.
unsigned int getNumberOfVotes()
This method simply returns the number of votes.
void generateCentersPP(const Eigen::MatrixXf &data, Eigen::MatrixXf &out_centers, int number_of_clusters, int trials)
Generates centers for clusters as described in Arthur, David and Sergei Vassilvitski (2007) k-means++...
bool loadModelFromfile(std::string &file_name)
This method loads the trained model from file.
The assignment of this structure is to store the statistical/learned weights and other information of...
void setTrainingNormals(const std::vector< typename pcl::PointCloud< NormalT >::Ptr > &training_normals)
Allows to set normals for the training clouds that were passed through setTrainingClouds method...
void simplifyCloud(typename pcl::PointCloud< PointT >::ConstPtr in_point_cloud, typename pcl::PointCloud< NormalT >::ConstPtr in_normal_cloud, typename pcl::PointCloud< PointT >::Ptr out_sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr out_sampled_normal_cloud)
Simplifies the cloud using voxel grid principles.
struct PCL_EXPORTS pcl::ism::ImplicitShapeModelEstimation::TC TermCriteria
This structure is used for determining the end of the k-means clustering process. ...
std::vector< float > getSigmaDists()
Returns the array of sigma values.
virtual ~ISMVoteList()
virtual descriptor.
unsigned int descriptors_dimension_
Stores descriptors dimension.
unsigned int number_of_classes_
Stores the number of classes.
Eigen::MatrixXf directions_to_center_
Stores the directions to objects center for each visual word.
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
Eigen::Matrix3f alignYCoordWithNormal(const NormalT &in_normal)
This method simply computes the rotation matrix, so that the given normal would match the Y axis afte...
ISMModel()
Simple constructor that initializes the structure.
void setFeatureEstimator(boost::shared_ptr< pcl::Feature< PointT, pcl::Histogram< FeatureSize > > > feature)
Changes the feature estimator.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
VectorType::iterator iterator
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
void setNVotState(bool state)
Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
uint32_t height
The point cloud height (if organized as an image-structure).
void setSigmaDists(const std::vector< float > &training_sigmas)
This method allows to set the value of sigma used for calculating the learned weights for every singl...
virtual ~ISMModel()
Destructor that frees memory.
boost::shared_ptr< PointCloud< PointT > > Ptr
A point structure representing an N-D histogram.
uint32_t width
The point cloud width (if organized as an image-structure).
bool trainISM(ISMModelPtr &trained_model)
This method performs training and forms a visual vocabulary.
ImplicitShapeModelEstimation()
Simple constructor that initializes everything.
unsigned int number_of_clusters_
Stores the number of clusters.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
double computeKMeansClustering(const Eigen::MatrixXf &points_to_cluster, int number_of_clusters, Eigen::MatrixXi &io_labels, TermCriteria criteria, int attempts, int flags, Eigen::MatrixXf &cluster_centers)
Performs K-means clustering.
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
A point structure representing Euclidean xyz coordinates.
float computeDistance(Eigen::VectorXf &vec_1, Eigen::VectorXf &vec_2)
Computes the square distance beetween two vectors.
std::vector< std::vector< float > > statistical_weights_
Stores statistical weights.
A point structure representing an interest point with Euclidean xyz coordinates, and an interest valu...
int getCentroidIndex(const PointT &p)
Returns the index in the resulting downsampled cloud of the specified point.
float distance(const PointT &p1, const PointT &p2)
boost::shared_ptr< pcl::features::ISMVoteList< PointT > > 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...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
void reset()
this method resets all variables and frees memory.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
double getDensityAtPoint(const PointT &point, double sigma_dist)
Returns the density at the specified point.
double density
Density of this peak.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void calculateWeights(const std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations, const Eigen::MatrixXi &labels, std::vector< float > &sigmas, std::vector< std::vector< unsigned int > > &clusters, std::vector< std::vector< float > > &statistical_weights, std::vector< float > &learned_weights)
This function forms a visual vocabulary and evaluates weights described in [Knopp et al...
void applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
void filter(PointCloud &output)
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the coresponding cloud of normals for every training point cloud.
std::vector< unsigned int > getTrainingClasses()
Returns the array of classes that indicates which class the corresponding training cloud belongs...
boost::shared_ptr< ::pcl::PointIndices > Ptr
bool clusterDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, Eigen::MatrixXi &labels, Eigen::MatrixXf &clusters_centers)
This method performs descriptor clustering.
void estimateFeatures(typename pcl::PointCloud< PointT >::Ptr sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr normal_cloud, typename pcl::PointCloud< pcl::Histogram< FeatureSize > >::Ptr feature_cloud)
This method estimates features for the given point cloud.
ISMModel & operator=(const ISMModel &other)
Operator overloading for deep copy.
int class_id
Determines which class this peak belongs.
void findStrongestPeaks(std::vector< ISMPeak, Eigen::aligned_allocator< ISMPeak > > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma)
This method finds the strongest peaks (points were density has most higher values).
boost::shared_ptr< pcl::Feature< PointT, pcl::Histogram< FeatureSize > > > getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
This struct is used for storing peak.
unsigned int number_of_visual_words_
Stores the number of visual words.
std::vector< unsigned int > classes_
Stores the class label for every direction.
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed, so avoid using this function on non-empty clouds.
Feature represents the base feature class.
void addVote(pcl::InterestPoint &in_vote, const PointT &vote_origin, int in_class)
This method simply adds another vote to the list.
std::vector< float > sigmas_
Stores the sigma value for each class.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool extractDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations)
Extracts the descriptors from the input clouds.
float getSamplingSize()
Returns the sampling size used for cloud simplification.
boost::shared_ptr< pcl::features::ISMModel > ISMModelPtr
unsigned int getNumberOfClusters()
Returns the number of clusters used for descriptor clustering.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
void validateTree()
this method is simply setting up the search tree.
This structure stores the information about the keypoint.
void setSamplingSize(float sampling_size)
Changes the sampling size used for cloud simplification.
void shiftCloud(typename pcl::PointCloud< PointT >::Ptr in_cloud, Eigen::Vector3f shift_point)
This method simply shifts the clouds points relative to the passed point.
void setTrainingClasses(const std::vector< unsigned int > &training_classes)
Allows to set the class labels for the corresponding training clouds.
void setTrainingClouds(const std::vector< typename pcl::PointCloud< PointT >::Ptr > &training_clouds)
Allows to set clouds for training the ISM model.
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud(typename pcl::PointCloud< PointT >::Ptr cloud=0)
Returns the colored cloud that consists of votes for center (blue points) and initial point cloud (if...
void calculateSigmas(std::vector< float > &sigmas)
This method calculates the value of sigma used for calculating the learned weights for every single c...
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm...