40 #include <Eigen/src/Core/Matrix.h>
44 #include <pcl/point_representation.h>
45 #include <pcl/features/feature.h>
46 #include <pcl/kdtree/kdtree_flann.h>
69 template <
typename Po
intT>
74 using Ptr = shared_ptr<ISMVoteList<PointT> >;
75 using ConstPtr = shared_ptr<const ISMVoteList<PointT>>;
106 findStrongestPeaks (std::vector<
ISMPeak, Eigen::aligned_allocator<ISMPeak> > &out_peaks,
int in_class_id,
double in_non_maxima_radius,
double in_sigma);
113 getDensityAtPoint (
const PointT &point,
double sigma_dist);
126 shiftMean (
const Eigen::Vector3f& snapPt,
const double in_dSigmaDist);
157 using Ptr = shared_ptr<ISMModel>;
174 saveModelToFile (std::string& file_name);
180 loadModelFromfile (std::string& file_name);
240 template <
int FeatureSize,
typename Po
intT,
typename NormalT = pcl::Normal>
261 model_num_ (model_num),
262 dir_to_center_ (dir_to_center),
296 max_count_ (max_count),
297 epsilon_ (epsilon) {};
320 learned_weight_ (0.0f),
321 dir_to_center_ (0.0f, 0.0f, 0.0f) {};
343 std::vector<typename pcl::PointCloud<PointT>::Ptr>
344 getTrainingClouds ();
353 std::vector<unsigned int>
354 getTrainingClasses ();
360 setTrainingClasses (
const std::vector<unsigned int>& training_classes);
363 std::vector<typename pcl::PointCloud<NormalT>::Ptr>
364 getTrainingNormals ();
380 setSamplingSize (
float sampling_size);
384 getFeatureEstimator ();
395 getNumberOfClusters ();
401 setNumberOfClusters (
unsigned int num_of_clusters);
414 setSigmaDists (
const std::vector<float>& training_sigmas);
427 setNVotState (
bool state);
455 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations);
463 clusterDescriptors (std::vector<
pcl::Histogram<FeatureSize> >& histograms, Eigen::MatrixXi& labels, Eigen::MatrixXf& clusters_centers);
469 calculateSigmas (std::vector<float>& sigmas);
482 calculateWeights (
const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
483 const Eigen::MatrixXi &labels,
484 std::vector<float>& sigmas,
485 std::vector<std::vector<unsigned int> >& clusters,
486 std::vector<std::vector<float> >& statistical_weights,
487 std::vector<float>& learned_weights);
514 alignYCoordWithNormal (
const NormalT& in_normal);
521 applyTransform (Eigen::Vector3f& io_vec,
const Eigen::Matrix3f& in_transform);
544 computeKMeansClustering (
const Eigen::MatrixXf& points_to_cluster,
545 int number_of_clusters,
546 Eigen::MatrixXi& io_labels,
550 Eigen::MatrixXf& cluster_centers);
560 generateCentersPP (
const Eigen::MatrixXf& data,
561 Eigen::MatrixXf& out_centers,
562 int number_of_clusters,
570 generateRandomCenter (
const std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >& boxes, Eigen::VectorXf& center);
577 computeDistance (Eigen::VectorXf& vec_1, Eigen::VectorXf& vec_2);
614 static const int PP_CENTERS = 2;
618 static const int USE_INITIAL_LABELS = 1;
627 (
float, density, ism_density)
628 (
float, class_id, ism_class_id)
Feature represents the base feature class.
shared_ptr< Feature< PointInT, PointOutT > > Ptr
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
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.
shared_ptr< ISMVoteList< PointT > > Ptr
bool tree_is_valid_
Signalizes if the tree is valid.
std::vector< float > k_sqr_dist_
Stores square distances to the corresponding neighbours.
pcl::KdTreeFLANN< pcl::InterestPoint >::Ptr tree_
Stores the search tree.
std::vector< int > votes_class_
Stores classes for which every single vote was cast.
pcl::PointCloud< PointT >::Ptr votes_origins_
Stores the origins of the votes.
pcl::Indices k_ind_
Stores neighbours indices.
pcl::PointCloud< pcl::InterestPoint >::Ptr votes_
Stores all votes.
shared_ptr< const ISMVoteList< PointT > > ConstPtr
This class implements Implicit Shape Model algorithm described in "Hough Transforms and 3D SURF for r...
typename Feature::Ptr FeaturePtr
pcl::features::ISMModel::Ptr ISMModelPtr
std::vector< unsigned int > training_classes_
Stores the class number for each cloud from training_clouds_.
std::vector< typename pcl::PointCloud< NormalT >::Ptr > training_normals_
Stores the normals for each training cloud.
bool n_vot_ON_
If set to false then Nvot coeff from [Knopp et al., 2010, (4)] is equal 1.0.
unsigned int number_of_clusters_
Number of clusters, is used for clustering descriptors during the training.
Feature::Ptr feature_estimator_
Stores the feature estimator.
float sampling_size_
This value is used for the simplification.
std::vector< float > training_sigmas_
This array stores the sigma values for each training class.
std::vector< typename pcl::PointCloud< PointT >::Ptr > training_clouds_
Stores the clouds used for training.
Defines all the PCL implemented PointT point type structures.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Defines functions, macros and traits for allocating and using memory.
IndicesAllocator<> Indices
Type used for indices in PCL.
Defines all the PCL and non-PCL macros used.
A point structure representing an N-D histogram.
This struct is used for storing peak.
PCL_ADD_POINT4D
Point were this peak is located.
double density
Density of this peak.
int class_id
Determines which class this peak belongs.
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.
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...
shared_ptr< ISMModel > Ptr
unsigned int number_of_clusters_
Stores the number of clusters.
shared_ptr< const ISMModel > ConstPtr
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
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.
std::vector< std::vector< unsigned int > > clusters_
This is an array of clusters.
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.
NormalT normal_
Stores the normal of the initial point.
unsigned int model_num_
Tells from which training model this keypoint was extracted.
PointT point_
Stores the initial point.
LocationInfo(unsigned int model_num, const PointT &dir_to_center, const PointT &origin, const NormalT &normal)
Location info constructor.
PointT dir_to_center_
Expected direction to center for this keypoint.
This structure is used for determining the end of the k-means clustering process.
int type_
Flag that determines when the k-means clustering must be stopped.
float epsilon_
Defines the accuracy for k-means clustering.
TermCriteria(int type, int max_count, float epsilon)
Termination criteria constructor.
int max_count_
Defines maximum number of iterations for k-means clustering.
Structure for storing the visual word.
pcl::PointXYZ dir_to_center_
Expected direction to center.
float learned_weight_
Weight of the vote.
VisualWordStat()
Empty constructor with member variables initialization.
int class_
Which class this vote belongs.