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),
59 template <
typename Po
intT>
62 votes_class_.clear ();
63 votes_origins_.reset ();
71 template <
typename Po
intT>
void
75 tree_is_valid_ =
false;
76 votes_->points.insert (votes_->points.end (), vote);
78 votes_origins_->points.push_back (vote_origin);
79 votes_class_.push_back (votes_class);
89 colored_cloud->
width = 1;
97 for (std::size_t i_point = 0; i_point < cloud->
points.size (); i_point++)
99 point.x = cloud->
points[i_point].x;
100 point.y = cloud->
points[i_point].y;
101 point.z = cloud->
points[i_point].z;
102 colored_cloud->
points.push_back (point);
109 for (
const auto &i_vote : votes_->points)
114 colored_cloud->
points.push_back (point);
118 return (colored_cloud);
122 template <
typename Po
intT>
void
124 std::vector<
pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
126 double in_non_maxima_radius,
131 const std::size_t n_vote_classes = votes_class_.size ();
132 if (n_vote_classes == 0)
134 for (std::size_t i = 0; i < n_vote_classes ; i++)
135 assert ( votes_class_[i] == in_class_id );
139 const int NUM_INIT_PTS = 100;
140 double SIGMA_DIST = in_sigma;
141 const double FINAL_EPS = SIGMA_DIST / 100;
143 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
144 std::vector<double> peak_densities (NUM_INIT_PTS);
145 double max_density = -1.0;
146 for (
int i = 0; i < NUM_INIT_PTS; i++)
148 Eigen::Vector3f old_center;
149 Eigen::Vector3f curr_center;
150 curr_center (0) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].x;
151 curr_center (1) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].y;
152 curr_center (2) = votes_->points[votes_->points.size () * i / NUM_INIT_PTS].z;
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_->points.size (), -1 );
233 k_sqr_dist_.resize ( votes_->points.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_->points[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
256 Eigen::Vector3f vote_vec (votes_->points[k_ind_[j]].x, votes_->points[k_ind_[j]].y, votes_->points[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_->points[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_->points.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->
points[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->
points[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->
points[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->
points[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 std::size_t num_of_points = training_clouds_[i_cloud]->points.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->
points[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->
points[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->
points[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 unsigned int number_of_points =
static_cast<unsigned int> (training_clouds_[i_object]->points.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]->points[i_point].x * training_clouds_[i_object]->points[j_point].x;
964 curr_distance += training_clouds_[i_object]->points[i_point].y * training_clouds_[i_object]->points[j_point].y;
965 curr_distance += training_clouds_[i_object]->points[i_point].z * training_clouds_[i_object]->points[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 std::size_t num_source_points = in_point_cloud->
points.size ();
1136 const std::size_t num_sample_points = temp_cloud.
points.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++)
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 boost::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);
1529 #endif //#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_