41 #ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_
42 #define PCL_IMPLICIT_SHAPE_MODEL_HPP_
44 #include "../implicit_shape_model.h"
45 #include <pcl/filters/voxel_grid.h>
46 #include <pcl/filters/extract_indices.h>
51 template <
typename Po
intT>
54 tree_is_valid_ (false),
63 template <
typename Po
intT>
66 votes_class_.clear ();
67 votes_origins_.reset ();
75 template <
typename Po
intT>
void
79 tree_is_valid_ =
false;
80 votes_->points.insert (votes_->points.end (), vote);
82 votes_origins_->points.push_back (vote_origin);
83 votes_class_.push_back (votes_class);
93 colored_cloud->
width = 1;
101 for (
const auto& i_point: *cloud)
106 colored_cloud->
points.push_back (point);
113 for (
const auto &i_vote : votes_->points)
118 colored_cloud->
points.push_back (point);
120 colored_cloud->
height += votes_->size ();
122 return (colored_cloud);
126 template <
typename Po
intT>
void
128 std::vector<
pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
130 double in_non_maxima_radius,
135 const std::size_t n_vote_classes = votes_class_.size ();
136 if (n_vote_classes == 0)
138 for (std::size_t i = 0; i < n_vote_classes ; i++)
139 assert ( votes_class_[i] == in_class_id );
143 const int NUM_INIT_PTS = 100;
144 double SIGMA_DIST = in_sigma;
145 const double FINAL_EPS = SIGMA_DIST / 100;
147 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
148 std::vector<double> peak_densities (NUM_INIT_PTS);
149 double max_density = -1.0;
150 for (
int i = 0; i < NUM_INIT_PTS; i++)
152 Eigen::Vector3f old_center;
153 const auto idx = votes_->size() * i / NUM_INIT_PTS;
154 Eigen::Vector3f curr_center = (*votes_)[idx].getVector3fMap();
158 old_center = curr_center;
159 curr_center = shiftMean (old_center, SIGMA_DIST);
160 }
while ((old_center - curr_center).norm () > FINAL_EPS);
163 point.x = curr_center (0);
164 point.y = curr_center (1);
165 point.z = curr_center (2);
166 double curr_density = getDensityAtPoint (point, SIGMA_DIST);
167 assert (curr_density >= 0.0);
169 peaks[i] = curr_center;
170 peak_densities[i] = curr_density;
172 if ( max_density < curr_density )
173 max_density = curr_density;
177 std::vector<bool> peak_flag (NUM_INIT_PTS,
true);
178 for (
int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
181 double best_density = -1.0;
182 Eigen::Vector3f strongest_peak;
183 int best_peak_ind (-1);
184 int peak_counter (0);
185 for (
int i = 0; i < NUM_INIT_PTS; i++)
190 if ( peak_densities[i] > best_density)
192 best_density = peak_densities[i];
193 strongest_peak = peaks[i];
199 if( peak_counter == 0 )
203 peak.x = strongest_peak(0);
204 peak.y = strongest_peak(1);
205 peak.z = strongest_peak(2);
208 out_peaks.push_back ( peak );
211 peak_flag[best_peak_ind] =
false;
212 for (
int i = 0; i < NUM_INIT_PTS; i++)
218 double dist = (strongest_peak - peaks[i]).norm ();
219 if ( dist < in_non_maxima_radius )
220 peak_flag[i] =
false;
226 template <
typename Po
intT>
void
231 if (tree_ ==
nullptr)
233 tree_->setInputCloud (votes_);
234 k_ind_.resize ( votes_->size (), -1 );
235 k_sqr_dist_.resize ( votes_->size (), 0.0f );
236 tree_is_valid_ =
true;
241 template <
typename Po
intT> Eigen::Vector3f
246 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
253 std::size_t n_pts = tree_->radiusSearch (pt, 3*in_sigma_dist, k_ind_, k_sqr_dist_);
255 for (std::size_t j = 0; j < n_pts; j++)
257 double kernel = (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (in_sigma_dist * in_sigma_dist));
258 Eigen::Vector3f vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[k_ind_[j]].z);
259 wgh_sum += vote_vec *
static_cast<float> (
kernel);
262 assert (denom > 0.0);
264 return (wgh_sum /
static_cast<float> (denom));
268 template <
typename Po
intT>
double
270 const PointT &point,
double sigma_dist)
274 const std::size_t n_vote_classes = votes_class_.size ();
275 if (n_vote_classes == 0)
278 double sum_vote = 0.0;
284 std::size_t num_of_pts = tree_->radiusSearch (pt, 3 * sigma_dist, k_ind_, k_sqr_dist_);
286 for (std::size_t j = 0; j < num_of_pts; j++)
287 sum_vote += (*votes_)[k_ind_[j]].strength * std::exp (-k_sqr_dist_[j] / (sigma_dist * sigma_dist));
293 template <
typename Po
intT>
unsigned int
296 return (
static_cast<unsigned int> (votes_->size ()));
301 statistical_weights_ (0),
302 learned_weights_ (0),
306 number_of_classes_ (0),
307 number_of_visual_words_ (0),
308 number_of_clusters_ (0),
309 descriptors_dimension_ (0)
323 std::vector<float> vec;
324 vec.resize (this->number_of_clusters_, 0.0f);
325 this->statistical_weights_.resize (this->number_of_classes_, vec);
326 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
327 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
330 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
331 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
332 this->learned_weights_[i_visual_word] = copy.
learned_weights_[i_visual_word];
334 this->classes_.resize (this->number_of_visual_words_, 0);
335 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
336 this->classes_[i_visual_word] = copy.
classes_[i_visual_word];
338 this->sigmas_.resize (this->number_of_classes_, 0.0f);
339 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
340 this->sigmas_[i_class] = copy.
sigmas_[i_class];
342 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
343 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
344 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
345 this->directions_to_center_ (i_visual_word, i_dim) = copy.
directions_to_center_ (i_visual_word, i_dim);
347 this->clusters_centers_.resize (this->number_of_clusters_, 3);
348 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
349 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
350 this->clusters_centers_ (i_cluster, i_dim) = copy.
clusters_centers_ (i_cluster, i_dim);
363 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
366 output_file.close ();
370 output_file << number_of_classes_ <<
" ";
371 output_file << number_of_visual_words_ <<
" ";
372 output_file << number_of_clusters_ <<
" ";
373 output_file << descriptors_dimension_ <<
" ";
376 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
377 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
378 output_file << statistical_weights_[i_class][i_cluster] <<
" ";
381 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
382 output_file << learned_weights_[i_visual_word] <<
" ";
385 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
386 output_file << classes_[i_visual_word] <<
" ";
389 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
390 output_file << sigmas_[i_class] <<
" ";
393 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
394 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
395 output_file << directions_to_center_ (i_visual_word, i_dim) <<
" ";
398 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
399 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
400 output_file << clusters_centers_ (i_cluster, i_dim) <<
" ";
403 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
405 output_file << static_cast<unsigned int> (clusters_[i_cluster].size ()) <<
" ";
406 for (
const unsigned int &visual_word : clusters_[i_cluster])
407 output_file << visual_word <<
" ";
410 output_file.close ();
419 std::ifstream input_file (file_name.c_str ());
428 input_file.getline (line, 256,
' ');
429 number_of_classes_ =
static_cast<unsigned int> (strtol (line,
nullptr, 10));
430 input_file.getline (line, 256,
' '); number_of_visual_words_ = atoi (line);
431 input_file.getline (line, 256,
' '); number_of_clusters_ = atoi (line);
432 input_file.getline (line, 256,
' '); descriptors_dimension_ = atoi (line);
435 std::vector<float> vec;
436 vec.resize (number_of_clusters_, 0.0f);
437 statistical_weights_.resize (number_of_classes_, vec);
438 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
439 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
440 input_file >> statistical_weights_[i_class][i_cluster];
443 learned_weights_.resize (number_of_visual_words_, 0.0f);
444 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
445 input_file >> learned_weights_[i_visual_word];
448 classes_.resize (number_of_visual_words_, 0);
449 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
450 input_file >> classes_[i_visual_word];
453 sigmas_.resize (number_of_classes_, 0.0f);
454 for (
unsigned int i_class = 0; i_class < number_of_classes_; i_class++)
455 input_file >> sigmas_[i_class];
458 directions_to_center_.resize (number_of_visual_words_, 3);
459 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_visual_words_; i_visual_word++)
460 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
461 input_file >> directions_to_center_ (i_visual_word, i_dim);
464 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
465 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
466 for (
unsigned int i_dim = 0; i_dim < descriptors_dimension_; i_dim++)
467 input_file >> clusters_centers_ (i_cluster, i_dim);
470 std::vector<unsigned int> vect;
471 clusters_.resize (number_of_clusters_, vect);
472 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
474 unsigned int size_of_current_cluster = 0;
475 input_file >> size_of_current_cluster;
476 clusters_[i_cluster].resize (size_of_current_cluster, 0);
477 for (
unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
478 input_file >> clusters_[i_cluster][i_visual_word];
489 statistical_weights_.clear ();
490 learned_weights_.clear ();
493 directions_to_center_.resize (0, 0);
494 clusters_centers_.resize (0, 0);
496 number_of_classes_ = 0;
497 number_of_visual_words_ = 0;
498 number_of_clusters_ = 0;
499 descriptors_dimension_ = 0;
515 std::vector<float> vec;
516 vec.resize (number_of_clusters_, 0.0f);
517 this->statistical_weights_.resize (this->number_of_classes_, vec);
518 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
519 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
520 this->statistical_weights_[i_class][i_cluster] = other.
statistical_weights_[i_class][i_cluster];
522 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
523 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
524 this->learned_weights_[i_visual_word] = other.
learned_weights_[i_visual_word];
526 this->classes_.resize (this->number_of_visual_words_, 0);
527 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
528 this->classes_[i_visual_word] = other.
classes_[i_visual_word];
530 this->sigmas_.resize (this->number_of_classes_, 0.0f);
531 for (
unsigned int i_class = 0; i_class < this->number_of_classes_; i_class++)
532 this->sigmas_[i_class] = other.
sigmas_[i_class];
534 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
535 for (
unsigned int i_visual_word = 0; i_visual_word < this->number_of_visual_words_; i_visual_word++)
536 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
537 this->directions_to_center_ (i_visual_word, i_dim) = other.
directions_to_center_ (i_visual_word, i_dim);
539 this->clusters_centers_.resize (this->number_of_clusters_, 3);
540 for (
unsigned int i_cluster = 0; i_cluster < this->number_of_clusters_; i_cluster++)
541 for (
unsigned int i_dim = 0; i_dim < this->descriptors_dimension_; i_dim++)
542 this->clusters_centers_ (i_cluster, i_dim) = other.
clusters_centers_ (i_cluster, i_dim);
548 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
550 training_clouds_ (0),
551 training_classes_ (0),
552 training_normals_ (0),
553 training_sigmas_ (0),
554 sampling_size_ (0.1f),
555 feature_estimator_ (),
556 number_of_clusters_ (184),
562 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
565 training_clouds_.clear ();
566 training_classes_.clear ();
567 training_normals_.clear ();
568 training_sigmas_.clear ();
569 feature_estimator_.reset ();
573 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
576 return (training_clouds_);
580 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
584 training_clouds_.clear ();
585 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
586 training_clouds_.swap (clouds);
590 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
593 return (training_classes_);
597 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
600 training_classes_.clear ();
601 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
602 training_classes_.swap (classes);
606 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
609 return (training_normals_);
613 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
617 training_normals_.clear ();
618 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
619 training_normals_.swap (normals);
623 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
626 return (sampling_size_);
630 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
633 if (sampling_size >= std::numeric_limits<float>::epsilon ())
634 sampling_size_ = sampling_size;
641 return (feature_estimator_);
645 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
648 feature_estimator_ = feature;
652 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int
655 return (number_of_clusters_);
659 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
662 if (num_of_clusters > 0)
663 number_of_clusters_ = num_of_clusters;
667 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
670 return (training_sigmas_);
674 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
677 training_sigmas_.clear ();
678 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
679 training_sigmas_.swap (sigmas);
683 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
690 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
697 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
702 if (trained_model ==
nullptr)
704 trained_model->reset ();
706 std::vector<pcl::Histogram<FeatureSize> > histograms;
707 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
708 success = extractDescriptors (histograms, locations);
712 Eigen::MatrixXi labels;
713 success = clusterDescriptors(histograms, labels, trained_model->clusters_centers_);
717 std::vector<unsigned int> vec;
718 trained_model->clusters_.resize (number_of_clusters_, vec);
719 for (std::size_t i_label = 0; i_label < locations.size (); i_label++)
720 trained_model->clusters_[labels (i_label)].push_back (i_label);
722 calculateSigmas (trained_model->sigmas_);
727 trained_model->sigmas_,
728 trained_model->clusters_,
729 trained_model->statistical_weights_,
730 trained_model->learned_weights_);
732 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
733 trained_model->number_of_visual_words_ =
static_cast<unsigned int> (histograms.size ());
734 trained_model->number_of_clusters_ = number_of_clusters_;
735 trained_model->descriptors_dimension_ = FeatureSize;
737 trained_model->directions_to_center_.resize (locations.size (), 3);
738 trained_model->classes_.resize (locations.size ());
739 for (std::size_t i_dir = 0; i_dir < locations.size (); i_dir++)
741 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
742 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
743 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
744 trained_model->classes_[i_dir] = training_classes_[locations[i_dir].model_num_];
756 int in_class_of_interest)
760 if (in_cloud->
points.empty ())
765 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
766 if (sampled_point_cloud->
points.empty ())
770 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
773 const unsigned int n_key_points =
static_cast<unsigned int> (sampled_point_cloud->
size ());
774 std::vector<int> min_dist_inds (n_key_points, -1);
775 for (
unsigned int i_point = 0; i_point < n_key_points; i_point++)
777 Eigen::VectorXf curr_descriptor (FeatureSize);
778 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
779 curr_descriptor (i_dim) = (*feature_cloud)[i_point].histogram[i_dim];
781 float descriptor_sum = curr_descriptor.sum ();
782 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
785 unsigned int min_dist_idx = 0;
786 Eigen::VectorXf clusters_center (FeatureSize);
787 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
788 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
790 float best_dist = computeDistance (curr_descriptor, clusters_center);
791 for (
unsigned int i_clust_cent = 0; i_clust_cent < number_of_clusters_; i_clust_cent++)
793 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
794 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
795 float curr_dist = computeDistance (clusters_center, curr_descriptor);
796 if (curr_dist < best_dist)
798 min_dist_idx = i_clust_cent;
799 best_dist = curr_dist;
802 min_dist_inds[i_point] = min_dist_idx;
805 for (std::size_t i_point = 0; i_point < n_key_points; i_point++)
807 int min_dist_idx = min_dist_inds[i_point];
808 if (min_dist_idx == -1)
811 const unsigned int n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
813 Eigen::Matrix3f transform = alignYCoordWithNormal ((*sampled_normal_cloud)[i_point]);
814 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
816 unsigned int index = model->clusters_[min_dist_idx][i_word];
817 unsigned int i_class = model->classes_[index];
818 if (
static_cast<int> (i_class) != in_class_of_interest)
822 Eigen::Vector3f direction (
823 model->directions_to_center_(index, 0),
824 model->directions_to_center_(index, 1),
825 model->directions_to_center_(index, 2));
826 applyTransform (direction, transform.transpose ());
829 Eigen::Vector3f vote_pos = (*sampled_point_cloud)[i_point].getVector3fMap () + direction;
830 vote.x = vote_pos[0];
831 vote.y = vote_pos[1];
832 vote.z = vote_pos[2];
833 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
834 float learned_weight = model->learned_weights_[index];
835 float power = statistical_weight * learned_weight;
837 if (vote.
strength > std::numeric_limits<float>::epsilon ())
838 out_votes->
addVote (vote, (*sampled_point_cloud)[i_point], i_class);
846 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
849 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
854 int n_key_points = 0;
856 if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ ==
nullptr)
859 for (std::size_t i_cloud = 0; i_cloud < training_clouds_.size (); i_cloud++)
862 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
863 const auto num_of_points = training_clouds_[i_cloud]->size ();
864 for (
auto point_j = training_clouds_[i_cloud]->begin (); point_j != training_clouds_[i_cloud]->end (); point_j++)
865 models_center += point_j->getVector3fMap ();
866 models_center /=
static_cast<float> (num_of_points);
871 simplifyCloud (training_clouds_[i_cloud], training_normals_[i_cloud], sampled_point_cloud, sampled_normal_cloud);
872 if (sampled_point_cloud->
points.empty ())
875 shiftCloud (training_clouds_[i_cloud], models_center);
876 shiftCloud (sampled_point_cloud, models_center);
878 n_key_points +=
static_cast<int> (sampled_point_cloud->
size ());
881 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
884 for (
auto point_i = sampled_point_cloud->
points.cbegin (); point_i != sampled_point_cloud->
points.cend (); point_i++, point_index++)
886 float descriptor_sum = Eigen::VectorXf::Map ((*feature_cloud)[point_index].histogram, FeatureSize).sum ();
887 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
890 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
892 int dist =
static_cast<int> (
std::distance (sampled_point_cloud->
points.cbegin (), point_i));
893 Eigen::Matrix3f new_basis = alignYCoordWithNormal ((*sampled_normal_cloud)[dist]);
894 Eigen::Vector3f zero;
898 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
899 applyTransform (new_dir, new_basis);
901 PointT point (new_dir[0], new_dir[1], new_dir[2]);
902 LocationInfo info (
static_cast<unsigned int> (i_cloud), point, *point_i, (*sampled_normal_cloud)[dist]);
903 locations.insert(locations.end (), info);
911 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
914 Eigen::MatrixXi& labels,
915 Eigen::MatrixXf& clusters_centers)
917 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
919 for (std::size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
920 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
921 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
923 labels.resize (histograms.size(), 1);
924 computeKMeansClustering (
928 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
937 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
940 if (!training_sigmas_.empty ())
942 sigmas.resize (training_sigmas_.size (), 0.0f);
943 for (std::size_t i_sigma = 0; i_sigma < training_sigmas_.size (); i_sigma++)
944 sigmas[i_sigma] = training_sigmas_[i_sigma];
949 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
950 sigmas.resize (number_of_classes, 0.0f);
952 std::vector<float> vec;
953 std::vector<std::vector<float> > objects_sigmas;
954 objects_sigmas.resize (number_of_classes, vec);
956 unsigned int number_of_objects =
static_cast<unsigned int> (training_clouds_.size ());
957 for (
unsigned int i_object = 0; i_object < number_of_objects; i_object++)
959 float max_distance = 0.0f;
960 const auto number_of_points = training_clouds_[i_object]->size ();
961 for (
unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
962 for (
unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
964 float curr_distance = 0.0f;
965 curr_distance += (*training_clouds_[i_object])[i_point].x * (*training_clouds_[i_object])[j_point].x;
966 curr_distance += (*training_clouds_[i_object])[i_point].y * (*training_clouds_[i_object])[j_point].y;
967 curr_distance += (*training_clouds_[i_object])[i_point].z * (*training_clouds_[i_object])[j_point].z;
968 if (curr_distance > max_distance)
969 max_distance = curr_distance;
971 max_distance =
static_cast<float> (sqrt (max_distance));
972 unsigned int i_class = training_classes_[i_object];
973 objects_sigmas[i_class].push_back (max_distance);
976 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
979 unsigned int number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
980 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
981 sig += objects_sigmas[i_class][i_object];
982 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
983 sigmas[i_class] = sig;
988 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
990 const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
991 const Eigen::MatrixXi &labels,
992 std::vector<float>& sigmas,
993 std::vector<std::vector<unsigned int> >& clusters,
994 std::vector<std::vector<float> >& statistical_weights,
995 std::vector<float>& learned_weights)
997 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
999 std::vector<float> vec;
1000 vec.resize (number_of_clusters_, 0.0f);
1001 statistical_weights.clear ();
1002 learned_weights.clear ();
1003 statistical_weights.resize (number_of_classes, vec);
1004 learned_weights.resize (locations.size (), 0.0f);
1007 std::vector<int> vect;
1008 vect.resize (*std::max_element (training_classes_.begin (), training_classes_.end () ) + 1, 0);
1011 std::vector<int> n_ftr;
1014 std::vector<int> n_vot;
1017 std::vector<int> n_vw;
1020 std::vector<std::vector<int> > n_vot_2;
1022 n_vot_2.resize (number_of_clusters_, vect);
1023 n_vot.resize (number_of_clusters_, 0);
1024 n_ftr.resize (number_of_classes, 0);
1025 for (std::size_t i_location = 0; i_location < locations.size (); i_location++)
1027 int i_class = training_classes_[locations[i_location].model_num_];
1028 int i_cluster = labels (i_location);
1029 n_vot_2[i_cluster][i_class] += 1;
1030 n_vot[i_cluster] += 1;
1031 n_ftr[i_class] += 1;
1034 n_vw.resize (number_of_classes, 0);
1035 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1036 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1037 if (n_vot_2[i_cluster][i_class] > 0)
1041 learned_weights.resize (locations.size (), 0.0);
1042 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1044 unsigned int number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1045 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1047 unsigned int i_index = clusters[i_cluster][i_visual_word];
1048 int i_class = training_classes_[locations[i_index].model_num_];
1049 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1050 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1052 std::vector<float> calculated_sigmas;
1053 calculateSigmas (calculated_sigmas);
1054 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1055 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1058 Eigen::Matrix3f transform = alignYCoordWithNormal (locations[i_index].normal_);
1059 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1060 applyTransform (direction, transform);
1061 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1064 std::vector<float> gauss_dists;
1065 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1067 unsigned int j_index = clusters[i_cluster][j_visual_word];
1068 int j_class = training_classes_[locations[j_index].model_num_];
1069 if (i_class != j_class)
1072 Eigen::Matrix3f transform_2 = alignYCoordWithNormal (locations[j_index].normal_);
1073 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1074 applyTransform (direction_2, transform_2);
1075 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1076 float residual = (predicted_center - actual_center).norm ();
1077 float value = -residual * residual / square_sigma_dist;
1078 gauss_dists.push_back (
static_cast<float> (std::exp (value)));
1081 std::size_t mid_elem = (gauss_dists.size () - 1) / 2;
1082 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1083 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1088 for (
unsigned int i_cluster = 0; i_cluster < number_of_clusters_; i_cluster++)
1090 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1092 if (n_vot_2[i_cluster][i_class] == 0)
1094 if (n_vw[i_class] == 0)
1096 if (n_vot[i_cluster] == 0)
1098 if (n_ftr[i_class] == 0)
1100 float part_1 =
static_cast<float> (n_vw[i_class]);
1101 float part_2 =
static_cast<float> (n_vot[i_cluster]);
1102 float part_3 =
static_cast<float> (n_vot_2[i_cluster][i_class]) /
static_cast<float> (n_ftr[i_class]);
1103 float part_4 = 0.0f;
1108 for (
unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1109 if (n_ftr[j_class] != 0)
1110 part_4 +=
static_cast<float> (n_vot_2[i_cluster][j_class]) /
static_cast<float> (n_ftr[j_class]);
1112 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1118 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1127 grid.
setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1132 grid.
filter (temp_cloud);
1135 const float max_value = std::numeric_limits<float>::max ();
1137 const auto num_source_points = in_point_cloud->
size ();
1138 const auto num_sample_points = temp_cloud.
size ();
1140 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1141 std::vector<int> sampling_indices (num_sample_points, -1);
1143 for (std::size_t i_point = 0; i_point < num_source_points; i_point++)
1149 PointT pt_1 = (*in_point_cloud)[i_point];
1150 PointT pt_2 = temp_cloud[index];
1152 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);
1153 if (
distance < dist_to_grid_center[index])
1155 dist_to_grid_center[index] =
distance;
1156 sampling_indices[index] =
static_cast<int> (i_point);
1165 final_inliers_indices->indices.reserve (num_sample_points);
1166 for (std::size_t i_point = 0; i_point < num_sample_points; i_point++)
1168 if (sampling_indices[i_point] != -1)
1169 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1173 extract_points.
setIndices (final_inliers_indices);
1174 extract_points.
filter (*out_sampled_point_cloud);
1177 extract_normals.
setIndices (final_inliers_indices);
1178 extract_normals.
filter (*out_sampled_normal_cloud);
1182 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1185 Eigen::Vector3f shift_point)
1187 for (
auto point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1189 point_it->x -= shift_point.x ();
1190 point_it->y -= shift_point.y ();
1191 point_it->z -= shift_point.z ();
1196 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1199 Eigen::Matrix3f result;
1200 Eigen::Matrix3f rotation_matrix_X;
1201 Eigen::Matrix3f rotation_matrix_Z;
1207 float denom_X =
static_cast<float> (sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1208 A = in_normal.normal_y / denom_X;
1209 B = sign * in_normal.normal_z / denom_X;
1210 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1214 float denom_Z =
static_cast<float> (sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1215 A = in_normal.normal_y / denom_Z;
1216 B = sign * in_normal.normal_x / denom_Z;
1217 rotation_matrix_Z << A, -
B, 0.0f,
1221 result = rotation_matrix_X * rotation_matrix_Z;
1227 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1230 io_vec = in_transform * io_vec;
1234 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1243 feature_estimator_->setInputCloud (sampled_point_cloud->
makeShared ());
1245 feature_estimator_->setSearchMethod (tree);
1252 dynamic_pointer_cast<pcl::FeatureFromNormals<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);
1255 feature_estimator_->compute (*feature_cloud);
1259 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double
1261 const Eigen::MatrixXf& points_to_cluster,
1262 int number_of_clusters,
1263 Eigen::MatrixXi& io_labels,
1267 Eigen::MatrixXf& cluster_centers)
1269 const int spp_trials = 3;
1270 std::size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1271 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1273 attempts = std::max (attempts, 1);
1274 srand (
static_cast<unsigned int> (time (
nullptr)));
1276 Eigen::MatrixXi labels (number_of_points, 1);
1278 if (flags & USE_INITIAL_LABELS)
1283 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1284 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1285 std::vector<int> counters (number_of_clusters);
1286 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1287 Eigen::Vector2f* box = &boxes[0];
1289 double best_compactness = std::numeric_limits<double>::max ();
1290 double compactness = 0.0;
1292 if (criteria.
type_ & TermCriteria::EPS)
1295 criteria.
epsilon_ = std::numeric_limits<float>::epsilon ();
1299 if (criteria.
type_ & TermCriteria::COUNT)
1304 if (number_of_clusters == 1)
1310 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1311 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1313 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1314 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1316 float v = points_to_cluster (i_point, i_dim);
1317 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1318 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1321 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
1323 float max_center_shift = std::numeric_limits<float>::max ();
1324 for (
int iter = 0; iter < criteria.
max_count_ && max_center_shift > criteria.
epsilon_; iter++)
1326 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1328 centers = old_centers;
1331 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1333 if (flags & PP_CENTERS)
1334 generateCentersPP (points_to_cluster, centers, number_of_clusters, spp_trials);
1337 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1339 Eigen::VectorXf center (feature_dimension);
1340 generateRandomCenter (boxes, center);
1341 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1342 centers (i_cl_center, i_dim) = center (i_dim);
1349 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1350 counters[i_cluster] = 0;
1351 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1353 int i_label = labels (i_point, 0);
1354 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1355 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1356 counters[i_label]++;
1359 max_center_shift = 0.0f;
1360 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1362 if (counters[i_cl_center] != 0)
1364 float scale = 1.0f /
static_cast<float> (counters[i_cl_center]);
1365 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1366 centers (i_cl_center, i_dim) *= scale;
1370 Eigen::VectorXf center (feature_dimension);
1371 generateRandomCenter (boxes, center);
1372 for(
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1373 centers (i_cl_center, i_dim) = center (i_dim);
1379 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1381 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1382 dist += diff * diff;
1384 max_center_shift = std::max (max_center_shift, dist);
1389 for (std::size_t i_point = 0; i_point < number_of_points; i_point++)
1391 Eigen::VectorXf sample (feature_dimension);
1392 sample = points_to_cluster.row (i_point);
1395 float min_dist = std::numeric_limits<float>::max ();
1397 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1399 Eigen::VectorXf center (feature_dimension);
1400 center = centers.row (i_cluster);
1401 float dist = computeDistance (sample, center);
1402 if (min_dist > dist)
1408 compactness += min_dist;
1409 labels (i_point, 0) = k_best;
1413 if (compactness < best_compactness)
1415 best_compactness = compactness;
1416 cluster_centers = centers;
1421 return (best_compactness);
1425 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1427 const Eigen::MatrixXf& data,
1428 Eigen::MatrixXf& out_centers,
1429 int number_of_clusters,
1432 std::size_t dimension = data.cols ();
1433 unsigned int number_of_points =
static_cast<unsigned int> (data.rows ());
1434 std::vector<int> centers_vec (number_of_clusters);
1435 int* centers = ¢ers_vec[0];
1436 std::vector<double> dist (number_of_points);
1437 std::vector<double> tdist (number_of_points);
1438 std::vector<double> tdist2 (number_of_points);
1441 unsigned int random_unsigned = rand ();
1442 centers[0] = random_unsigned % number_of_points;
1444 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1446 Eigen::VectorXf first (dimension);
1447 Eigen::VectorXf second (dimension);
1448 first = data.row (i_point);
1449 second = data.row (centers[0]);
1450 dist[i_point] = computeDistance (first, second);
1451 sum0 += dist[i_point];
1454 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1456 double best_sum = std::numeric_limits<double>::max ();
1457 int best_center = -1;
1458 for (
int i_trials = 0; i_trials < trials; i_trials++)
1460 unsigned int random_integer = rand () - 1;
1461 double random_double =
static_cast<double> (random_integer) /
static_cast<double> (std::numeric_limits<unsigned int>::max ());
1462 double p = random_double * sum0;
1464 unsigned int i_point;
1465 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1466 if ( (p -= dist[i_point]) <= 0.0)
1472 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1474 Eigen::VectorXf first (dimension);
1475 Eigen::VectorXf second (dimension);
1476 first = data.row (i_point);
1477 second = data.row (ci);
1478 tdist2[i_point] = std::min (
static_cast<double> (computeDistance (first, second)), dist[i_point]);
1479 s += tdist2[i_point];
1486 std::swap (tdist, tdist2);
1490 centers[i_cluster] = best_center;
1492 std::swap (dist, tdist);
1495 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1496 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1497 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1501 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1503 Eigen::VectorXf& center)
1505 std::size_t dimension = boxes.size ();
1506 float margin = 1.0f /
static_cast<float> (dimension);
1508 for (std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1510 unsigned int random_integer = rand () - 1;
1511 float random_float =
static_cast<float> (random_integer) /
static_cast<float> (std::numeric_limits<unsigned int>::max ());
1512 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1517 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
1520 std::size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1522 for(std::size_t i_dim = 0; i_dim < dimension; i_dim++)
1524 float diff = vec_1 (i_dim) - vec_2 (i_dim);
1531 #endif //#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_