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>
51template <
typename Po
intT>
54 tree_is_valid_ (
false),
63template <
typename Po
intT>
66 votes_class_.clear ();
67 votes_origins_.
reset ();
75template <
typename Po
intT>
void
79 tree_is_valid_ =
false;
80 votes_->points.insert (votes_->points.end (), vote);
101 for (
const auto&
i_point: *cloud)
113 for (
const auto &
i_vote : votes_->points)
126template <
typename Po
intT>
void
147 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> >
peaks (
NUM_INIT_PTS);
154 Eigen::Vector3f
curr_center = (*votes_)[idx].getVector3fMap();
226template <
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;
241template <
typename Po
intT> Eigen::Vector3f
246 Eigen::Vector3f
wgh_sum (0.0, 0.0, 0.0);
255 for (std::size_t j = 0; j <
n_pts; j++)
258 Eigen::Vector3f
vote_vec ((*votes_)[k_ind_[j]].x, (*votes_)[k_ind_[j]].y, (*votes_)[k_ind_[j]].z);
268template <
typename Po
intT>
double
293template <
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);
330 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
334 this->classes_.resize (this->number_of_visual_words_, 0);
338 this->sigmas_.resize (this->number_of_classes_, 0.0f);
342 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
347 this->clusters_centers_.resize (this->number_of_clusters_, 3);
349 for (
unsigned int i_dim = 0;
i_dim < this->descriptors_dimension_;
i_dim++)
363 std::ofstream
output_file (file_name.c_str (), std::ios::trunc);
419 std::ifstream
input_file (file_name.c_str ());
429 number_of_classes_ =
static_cast<unsigned int> (
strtol (
line,
nullptr, 10));
435 std::vector<float>
vec;
436 vec.resize (number_of_clusters_, 0.0f);
437 statistical_weights_.resize (number_of_classes_,
vec);
443 learned_weights_.resize (number_of_visual_words_, 0.0f);
448 classes_.resize (number_of_visual_words_, 0);
453 sigmas_.resize (number_of_classes_, 0.0f);
458 directions_to_center_.resize (number_of_visual_words_, 3);
464 clusters_centers_.resize (number_of_clusters_, descriptors_dimension_);
470 std::vector<unsigned int>
vect;
471 clusters_.resize (number_of_clusters_,
vect);
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;
510 this->number_of_classes_ =
other.number_of_classes_;
511 this->number_of_visual_words_ =
other.number_of_visual_words_;
512 this->number_of_clusters_ =
other.number_of_clusters_;
513 this->descriptors_dimension_ =
other.descriptors_dimension_;
515 std::vector<float>
vec;
516 vec.resize (number_of_clusters_, 0.0f);
517 this->statistical_weights_.resize (this->number_of_classes_,
vec);
522 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
526 this->classes_.resize (this->number_of_visual_words_, 0);
530 this->sigmas_.resize (this->number_of_classes_, 0.0f);
534 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
539 this->clusters_centers_.resize (this->number_of_clusters_, 3);
541 for (
unsigned int i_dim = 0;
i_dim < this->descriptors_dimension_;
i_dim++)
548template <
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),
562template <
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 ();
573template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
576 return (training_clouds_);
580template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
584 training_clouds_.clear ();
586 training_clouds_.swap (
clouds);
590template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
593 return (training_classes_);
597template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
600 training_classes_.clear ();
602 training_classes_.swap (
classes);
606template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
609 return (training_normals_);
613template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
617 training_normals_.clear ();
619 training_normals_.swap (normals);
623template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
626 return (sampling_size_);
630template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
641 return (feature_estimator_);
645template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
648 feature_estimator_ = feature;
652template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int
655 return (number_of_clusters_);
659template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
667template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
670 return (training_sigmas_);
674template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
677 training_sigmas_.clear ();
679 training_sigmas_.swap (
sigmas);
683template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
690template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
697template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
706 std::vector<pcl::Histogram<FeatureSize> >
histograms;
707 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> >
locations;
712 Eigen::MatrixXi labels;
717 std::vector<unsigned int>
vec;
732 trained_model->number_of_classes_ = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
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;
837 if (vote.
strength > std::numeric_limits<float>::epsilon ())
846template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
856 if (training_clouds_.empty () || training_classes_.empty () || feature_estimator_ ==
nullptr)
894 Eigen::Vector3f zero;
911template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool
914 Eigen::MatrixXi& labels,
924 computeKMeansClustering (
928 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
937template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
940 if (!training_sigmas_.empty ())
942 sigmas.resize (training_sigmas_.size (), 0.0f);
949 unsigned int number_of_classes = *std::max_element (training_classes_.begin (), training_classes_.end () ) + 1;
952 std::vector<float>
vec;
956 unsigned int number_of_objects =
static_cast<unsigned int> (training_clouds_.size ());
988template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
991 const Eigen::MatrixXi &labels,
992 std::vector<float>&
sigmas,
993 std::vector<std::vector<unsigned int> >&
clusters,
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);
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;
1023 n_vot.resize (number_of_clusters_, 0);
1058 Eigen::Matrix3f transform = alignYCoordWithNormal (
locations[
i_index].normal_);
1059 Eigen::Vector3f direction =
locations[
i_index].dir_to_center_.getVector3fMap ();
1060 applyTransform (direction, transform);
1078 gauss_dists.push_back (
static_cast<float> (std::exp (value)));
1118template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1127 grid.setLeafSize (sampling_size_, sampling_size_, sampling_size_);
1128 grid.setSaveLeafLayout (
true);
1135 const float max_value = std::numeric_limits<float>::max ();
1182template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1196template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1227template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1234template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1245 feature_estimator_->setSearchMethod (tree);
1259template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double
1274 srand (
static_cast<unsigned int> (time (
nullptr)));
1278 if (flags & USE_INITIAL_LABELS)
1292 if (
criteria.type_ & TermCriteria::EPS)
1295 criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1299 if (
criteria.type_ & TermCriteria::COUNT)
1331 if ( iter == 0 && (
i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1333 if (flags & PP_CENTERS)
1395 float min_dist = std::numeric_limits<float>::max ();
1401 float dist = computeDistance (sample,
center);
1425template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1427 const Eigen::MatrixXf& data,
1449 second = data.row (
centers[0]);
1456 double best_sum = std::numeric_limits<double>::max ();
1477 second = data.row (
ci);
1501template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void
1517template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float
1521 float distance = 0.0f;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm.
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
shared_ptr< ISMVoteList< PointT > > Ptr
void addVote(pcl::InterestPoint &in_vote, const PointT &vote_origin, int in_class)
This method simply adds another vote to the list.
void findStrongestPeaks(std::vector< ISMPeak, Eigen::aligned_allocator< ISMPeak > > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma)
This method finds the strongest peaks (points were density has most higher values).
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud(typename pcl::PointCloud< PointT >::Ptr cloud=0)
Returns the colored cloud that consists of votes for center (blue points) and initial point cloud (if...
unsigned int getNumberOfVotes()
This method simply returns the number of votes.
void validateTree()
this method is simply setting up the search tree.
double getDensityAtPoint(const PointT &point, double sigma_dist)
Returns the density at the specified point.
virtual ~ISMVoteList()
virtual descriptor.
ISMVoteList()
Empty constructor with member variables initialization.
bool trainISM(ISMModelPtr &trained_model)
This method performs training and forms a visual vocabulary.
typename Feature::Ptr FeaturePtr
void applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
Eigen::Matrix3f alignYCoordWithNormal(const NormalT &in_normal)
This method simply computes the rotation matrix, so that the given normal would match the Y axis afte...
std::vector< typename pcl::PointCloud< PointT >::Ptr > getTrainingClouds()
This method simply returns the clouds that were set as the training clouds.
void generateRandomCenter(const std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > &boxes, Eigen::VectorXf ¢er)
Generates random center for cluster.
void setFeatureEstimator(FeaturePtr feature)
Changes the feature estimator.
void setTrainingClouds(const std::vector< typename pcl::PointCloud< PointT >::Ptr > &training_clouds)
Allows to set clouds for training the ISM model.
void setNVotState(bool state)
Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
float computeDistance(Eigen::VectorXf &vec_1, Eigen::VectorXf &vec_2)
Computes the square distance between two vectors.
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
void shiftCloud(typename pcl::PointCloud< PointT >::Ptr in_cloud, Eigen::Vector3f shift_point)
This method simply shifts the clouds points relative to the passed point.
pcl::features::ISMModel::Ptr ISMModelPtr
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
void setTrainingNormals(const std::vector< typename pcl::PointCloud< NormalT >::Ptr > &training_normals)
Allows to set normals for the training clouds that were passed through setTrainingClouds method.
void generateCentersPP(const Eigen::MatrixXf &data, Eigen::MatrixXf &out_centers, int number_of_clusters, int trials)
Generates centers for clusters as described in Arthur, David and Sergei Vassilvitski (2007) k-means++...
ImplicitShapeModelEstimation()
Simple constructor that initializes everything.
double computeKMeansClustering(const Eigen::MatrixXf &points_to_cluster, int number_of_clusters, Eigen::MatrixXi &io_labels, TermCriteria criteria, int attempts, int flags, Eigen::MatrixXf &cluster_centers)
Performs K-means clustering.
std::vector< unsigned int > getTrainingClasses()
Returns the array of classes that indicates which class the corresponding training cloud belongs.
void calculateWeights(const std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations, const Eigen::MatrixXi &labels, std::vector< float > &sigmas, std::vector< std::vector< unsigned int > > &clusters, std::vector< std::vector< float > > &statistical_weights, std::vector< float > &learned_weights)
This function forms a visual vocabulary and evaluates weights described in [Knopp et al....
void estimateFeatures(typename pcl::PointCloud< PointT >::Ptr sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr normal_cloud, typename pcl::PointCloud< pcl::Histogram< FeatureSize > >::Ptr feature_cloud)
This method estimates features for the given point cloud.
bool extractDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations)
Extracts the descriptors from the input clouds.
FeaturePtr getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
unsigned int getNumberOfClusters()
Returns the number of clusters used for descriptor clustering.
void calculateSigmas(std::vector< float > &sigmas)
This method calculates the value of sigma used for calculating the learned weights for every single c...
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
bool clusterDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, Eigen::MatrixXi &labels, Eigen::MatrixXf &clusters_centers)
This method performs descriptor clustering.
std::vector< float > getSigmaDists()
Returns the array of sigma values.
void setSamplingSize(float sampling_size)
Changes the sampling size used for cloud simplification.
pcl::features::ISMVoteList< PointT >::Ptr findObjects(ISMModelPtr model, typename pcl::PointCloud< PointT >::Ptr in_cloud, typename pcl::PointCloud< Normal >::Ptr in_normals, int in_class_of_interest)
This function is searching for the class of interest in a given cloud and returns the list of votes.
void simplifyCloud(typename pcl::PointCloud< PointT >::ConstPtr in_point_cloud, typename pcl::PointCloud< NormalT >::ConstPtr in_normal_cloud, typename pcl::PointCloud< PointT >::Ptr out_sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr out_sampled_normal_cloud)
Simplifies the cloud using voxel grid principles.
void setSigmaDists(const std::vector< float > &training_sigmas)
This method allows to set the value of sigma used for calculating the learned weights for every singl...
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the corresponding cloud of normals for every training point cloud.
void setTrainingClasses(const std::vector< unsigned int > &training_classes)
Allows to set the class labels for the corresponding training clouds.
float getSamplingSize()
Returns the sampling size used for cloud simplification.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
shared_ptr< pcl::search::Search< PointT > > Ptr
Defines functions, macros and traits for allocating and using memory.
This struct is used for storing peak.
A point structure representing an interest point with Euclidean xyz coordinates, and an interest valu...
A point structure representing normal coordinates and the surface curvature estimate.
shared_ptr< ::pcl::PointIndices > Ptr
A point structure representing Euclidean xyz coordinates.
A point structure representing Euclidean xyz coordinates, and the RGB color.
The assignment of this structure is to store the statistical/learned weights and other information of...
unsigned int number_of_clusters_
Stores the number of clusters.
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
ISMModel()
Simple constructor that initializes the structure.
bool loadModelFromfile(std::string &file_name)
This method loads the trained model from file.
ISMModel & operator=(const ISMModel &other)
Operator overloading for deep copy.
virtual ~ISMModel()
Destructor that frees memory.
unsigned int descriptors_dimension_
Stores descriptors dimension.
std::vector< float > sigmas_
Stores the sigma value for each class.
std::vector< float > learned_weights_
Stores learned weights.
void reset()
this method resets all variables and frees memory.
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
std::vector< unsigned int > classes_
Stores the class label for every direction.
Eigen::MatrixXf directions_to_center_
Stores the directions to objects center for each visual word.
unsigned int number_of_classes_
Stores the number of classes.
std::vector< std::vector< float > > statistical_weights_
Stores statistical weights.
unsigned int number_of_visual_words_
Stores the number of visual words.
This structure stores the information about the keypoint.
This structure is used for determining the end of the k-means clustering process.