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;
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);
1257 boost::dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
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_