Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/recognition/include/pcl/recognition/implicit_shape_model.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 
00036 #ifndef PCL_IMPLICIT_SHAPE_MODEL_H_
00037 #define PCL_IMPLICIT_SHAPE_MODEL_H_
00038 
00039 #include <vector>
00040 #include <fstream>
00041 #include <limits>
00042 #include <Eigen/src/Core/Matrix.h>
00043 #include <pcl/pcl_base.h>
00044 #include <pcl/point_types.h>
00045 #include <pcl/point_representation.h>
00046 #include <pcl/features/feature.h>
00047 #include <pcl/features/spin_image.h>
00048 #include <pcl/filters/voxel_grid.h>
00049 #include <pcl/filters/extract_indices.h>
00050 #include <pcl/search/search.h>
00051 #include <pcl/kdtree/kdtree.h>
00052 #include <pcl/kdtree/kdtree_flann.h>
00053 #include <pcl/kdtree/impl/kdtree_flann.hpp>
00054 
00055 namespace pcl
00056 {
00057   /** \brief This struct is used for storing peak. */
00058   struct ISMPeak
00059   {
00060     /** \brief Point were this peak is located. */
00061     PCL_ADD_POINT4D;
00062 
00063     /** \brief Density of this peak. */
00064     double density;
00065 
00066     /** \brief Determines which class this peak belongs. */
00067     int class_id;
00068 
00069     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00070   } EIGEN_ALIGN16;
00071 
00072   namespace features
00073   {
00074     /** \brief This class is used for storing, analyzing and manipulating votes
00075       * obtained from ISM algorithm. */
00076     template <typename PointT>
00077     class PCL_EXPORTS ISMVoteList
00078     {
00079       public:
00080 
00081         /** \brief Empty constructor with member variables initialization. */
00082         ISMVoteList ();
00083 
00084         /** \brief virtual descriptor. */
00085         virtual
00086         ~ISMVoteList ();
00087 
00088         /** \brief This method simply adds another vote to the list.
00089           * \param[in] in_vote vote to add
00090           * \param[in] vote_origin origin of the added vote
00091           * \param[in] in_class class for which this vote is cast
00092           */
00093         void
00094         addVote (pcl::InterestPoint& in_vote, const PointT &vote_origin, int in_class);
00095 
00096         /** \brief Returns the colored cloud that consists of votes for center (blue points) and
00097           * initial point cloud (if it was passed).
00098           * \param[in] cloud cloud that needs to be merged with votes for visualizing. */
00099         typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr
00100         getColoredCloud (typename pcl::PointCloud<PointT>::Ptr cloud = 0);
00101 
00102         /** \brief This method finds the strongest peaks (points were density has most higher values).
00103           * It is based on the non maxima supression principles.
00104           * \param[out] out_peaks it will contain the strongest peaks
00105           * \param[in] in_class_id class of interest for which peaks are evaluated
00106           * \param[in] in_non_maxima_radius non maxima supression radius. The shapes radius is recommended for this value.
00107           */
00108         void
00109         findStrongestPeaks (std::vector<ISMPeak, Eigen::aligned_allocator<ISMPeak> > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma);
00110 
00111         /** \brief Returns the density at the specified point.
00112           * \param[in] point point of interest
00113           * \param[in] sigma_dist
00114           */
00115         double
00116         getDensityAtPoint (const PointT &point, double sigma_dist);
00117 
00118         /** \brief This method simply returns the number of votes. */
00119         unsigned int
00120         getNumberOfVotes ();
00121 
00122       protected:
00123 
00124         /** \brief this method is simply setting up the search tree. */
00125         void
00126         validateTree ();
00127 
00128         Eigen::Vector3f
00129         shiftMean (const Eigen::Vector3f& snapPt, const double in_dSigmaDist);
00130 
00131       protected:
00132 
00133         /** \brief Stores all votes. */
00134         pcl::PointCloud<pcl::InterestPoint>::Ptr votes_;
00135 
00136         /** \brief Signalizes if the tree is valid. */
00137         bool tree_is_valid_;
00138 
00139         /** \brief Stores the origins of the votes. */
00140         typename pcl::PointCloud<PointT>::Ptr votes_origins_;
00141 
00142         /** \brief Stores classes for which every single vote was cast. */
00143         std::vector<int> votes_class_;
00144 
00145         /** \brief Stores the search tree. */
00146         pcl::KdTreeFLANN<pcl::InterestPoint>::Ptr tree_;
00147 
00148         /** \brief Stores neighbours indices. */
00149         std::vector<int> k_ind_;
00150 
00151         /** \brief Stores square distances to the corresponding neighbours. */
00152         std::vector<float> k_sqr_dist_;
00153     };
00154  
00155     /** \brief The assignment of this structure is to store the statistical/learned weights and other information
00156       * of the trained Implict Shape Model algorithm.
00157       */
00158     struct PCL_EXPORTS ISMModel
00159     {
00160       /** \brief Simple constructor that initializes the structure. */
00161       ISMModel ();
00162 
00163       /** \brief Copy constructor for deep copy. */
00164       ISMModel (ISMModel const & copy);
00165 
00166       /** Destructor that frees memory. */
00167       virtual
00168       ~ISMModel ();
00169 
00170       /** \brief This method simply saves the trained model for later usage.
00171         * \param[in] file_name path to file for saving model
00172         */
00173       bool
00174       saveModelToFile (std::string& file_name);
00175 
00176       /** \brief This method loads the trained model from file.
00177         * \param[in] file_name path to file which stores trained model
00178         */
00179       bool
00180       loadModelFromfile (std::string& file_name);
00181 
00182       /** \brief this method resets all variables and frees memory. */
00183       void
00184       reset ();
00185 
00186       /** Operator overloading for deep copy. */
00187       ISMModel & operator = (const ISMModel& other);
00188 
00189       /** \brief Stores statistical weights. */
00190       std::vector<std::vector<float> > statistical_weights_;
00191 
00192       /** \brief Stores learned weights. */
00193       std::vector<float> learned_weights_;
00194 
00195       /** \brief Stores the class label for every direction. */
00196       std::vector<unsigned int> classes_;
00197 
00198       /** \brief Stores the sigma value for each class. This values were used to compute the learned weights. */
00199       std::vector<float> sigmas_;
00200 
00201       /** \brief Stores the directions to objects center for each visual word. */
00202       Eigen::MatrixXf directions_to_center_;
00203 
00204       /** \brief Stores the centers of the clusters that were obtained during the visual words clusterization. */
00205       Eigen::MatrixXf clusters_centers_;
00206 
00207       /** \brief This is an array of clusters. Each cluster stores the indices of the visual words that it contains. */
00208       std::vector<std::vector<unsigned int> > clusters_;
00209 
00210       /** \brief Stores the number of classes. */
00211       unsigned int number_of_classes_;
00212 
00213       /** \brief Stores the number of visual words. */
00214       unsigned int number_of_visual_words_;
00215 
00216       /** \brief Stores the number of clusters. */
00217       unsigned int number_of_clusters_;
00218 
00219       /** \brief Stores descriptors dimension. */
00220       unsigned int descriptors_dimension_;
00221 
00222       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00223     };
00224   }
00225 
00226   namespace ism
00227   {
00228     /** \brief This class implements Implicit Shape Model algorithm described in
00229       * "Hough Transforms and 3D SURF for robust three dimensional classication"
00230       * by Jan Knopp1, Mukta Prasad, Geert Willems1, Radu Timofte, and Luc Van Gool.
00231       * It has two main member functions. One for training, using the data for which we know
00232       * which class it belongs to. And second for investigating a cloud for the presence
00233       * of the class of interest.
00234       * Implementation of the ISM algorithm described in "Hough Transforms and 3D SURF for robust three dimensional classication"
00235       * by Jan Knopp, Mukta Prasad, Geert Willems, Radu Timofte, and Luc Van Gool
00236       *
00237       * Authors: Roman Shapovalov, Alexander Velizhev, Sergey Ushakov
00238       */
00239     template <int FeatureSize, typename PointT, typename NormalT = pcl::Normal>
00240     class PCL_EXPORTS ImplicitShapeModelEstimation
00241     {
00242       public:
00243 
00244         typedef boost::shared_ptr<pcl::features::ISMModel> ISMModelPtr;
00245 
00246       protected:
00247 
00248         /** \brief This structure stores the information about the keypoint. */
00249         struct PCL_EXPORTS LocationInfo
00250         {
00251           /** \brief Location info constructor.
00252             * \param[in] model_num number of training model.
00253             * \param[in] dir_to_center expected direction to center
00254             * \param[in] origin initial point
00255             * \param[in] normal normal of the initial point
00256             */
00257           LocationInfo (unsigned int model_num, const PointT& dir_to_center, const PointT& origin, const NormalT& normal) :
00258             model_num_ (model_num),
00259             dir_to_center_ (dir_to_center),
00260             point_ (origin),
00261             normal_ (normal) {};
00262 
00263           /** \brief Tells from which training model this keypoint was extracted. */
00264           unsigned int model_num_;
00265 
00266           /** \brief Expected direction to center for this keypoint. */
00267           PointT dir_to_center_;
00268 
00269           /** \brief Stores the initial point. */
00270           PointT point_;
00271 
00272           /** \brief Stores the normal of the initial point. */
00273           NormalT normal_;
00274         };
00275 
00276         /** \brief This structure is used for determining the end of the
00277           * k-means clustering process. */
00278         typedef struct PCL_EXPORTS TC
00279         {
00280           enum
00281           {
00282             COUNT = 1,
00283             EPS = 2
00284           };
00285 
00286           /** \brief Termination criteria constructor.
00287             * \param[in] type defines the condition of termination(max iter., desired accuracy)
00288             * \param[in] max_count defines the max number of iterations
00289             * \param[in] epsilon defines the desired accuracy
00290             */
00291           TC(int type, int max_count, float epsilon) :
00292             type_ (type),
00293             max_count_ (max_count),
00294             epsilon_ (epsilon) {};
00295 
00296           /** \brief Flag that determines when the k-means clustering must be stopped.
00297             * If type_ equals COUNT then it must be stopped when the max number of iterations will be
00298             * reached. If type_ eaquals EPS then it must be stopped when the desired accuracy will be reached.
00299             * These flags can be used together, in that case the clustering will be finished when one of these
00300             * conditions will be reached.
00301             */
00302           int type_;
00303 
00304           /** \brief Defines maximum number of iterations for k-means clustering. */
00305           int max_count_;
00306 
00307           /** \brief Defines the accuracy for k-means clustering. */
00308           float epsilon_;
00309         } TermCriteria;
00310 
00311         /** \brief Structure for storing the visual word. */
00312         struct PCL_EXPORTS VisualWordStat
00313         {
00314           /** \brief Empty constructor with member variables initialization. */
00315           VisualWordStat () :
00316             class_ (-1),
00317             learned_weight_ (0.0f),
00318             dir_to_center_ (0.0f, 0.0f, 0.0f) {};
00319 
00320           /** \brief Which class this vote belongs. */
00321           int class_;
00322 
00323           /** \brief Weight of the vote. */
00324           float learned_weight_;
00325 
00326           /** \brief Expected direction to center. */
00327           pcl::PointXYZ dir_to_center_;
00328         };
00329 
00330       public:
00331 
00332         /** \brief Simple constructor that initializes everything. */
00333         ImplicitShapeModelEstimation ();
00334 
00335         /** \brief Simple destructor. */
00336         virtual
00337         ~ImplicitShapeModelEstimation ();
00338 
00339         /** \brief This method simply returns the clouds that were set as the training clouds. */
00340         std::vector<typename pcl::PointCloud<PointT>::Ptr>
00341     getTrainingClouds ();
00342 
00343         /** \brief Allows to set clouds for training the ISM model.
00344           * \param[in] training_clouds array of point clouds for training
00345           */
00346         void
00347     setTrainingClouds (const std::vector< typename pcl::PointCloud<PointT>::Ptr >& training_clouds);
00348 
00349         /** \brief Returns the array of classes that indicates which class the corresponding training cloud belongs. */
00350         std::vector<unsigned int>
00351         getTrainingClasses ();
00352 
00353         /** \brief Allows to set the class labels for the corresponding training clouds.
00354           * \param[in] training_classes array of class labels
00355           */
00356         void
00357         setTrainingClasses (const std::vector<unsigned int>& training_classes);
00358 
00359         /** \brief This method returns the coresponding cloud of normals for every training point cloud. */
00360         std::vector<typename pcl::PointCloud<NormalT>::Ptr>
00361         getTrainingNormals ();
00362 
00363         /** \brief Allows to set normals for the training clouds that were passed through setTrainingClouds method.
00364           * \param[in] training_normals array of clouds, each cloud is the cloud of normals
00365           */
00366         void
00367         setTrainingNormals (const std::vector< typename pcl::PointCloud<NormalT>::Ptr >& training_normals);
00368 
00369         /** \brief Returns the sampling size used for cloud simplification. */
00370         float
00371         getSamplingSize ();
00372 
00373         /** \brief Changes the sampling size used for cloud simplification.
00374           * \param[in] sampling_size desired size of grid bin
00375           */
00376         void
00377         setSamplingSize (float sampling_size);
00378 
00379         /** \brief Returns the current feature estimator used for extraction of the descriptors. */
00380         boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > >
00381         getFeatureEstimator ();
00382 
00383         /** \brief Changes the feature estimator.
00384           * \param[in] feature feature estimator that will be used to extract the descriptors.
00385           * Note that it must be fully initialized and configured.
00386           */
00387         void
00388         setFeatureEstimator (boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > > feature);
00389 
00390         /** \brief Returns the number of clusters used for descriptor clustering. */
00391         unsigned int
00392         getNumberOfClusters ();
00393 
00394         /** \brief Changes the number of clusters.
00395           * \param num_of_clusters desired number of clusters
00396           */
00397         void
00398         setNumberOfClusters (unsigned int num_of_clusters);
00399 
00400         /** \brief Returns the array of sigma values. */
00401         std::vector<float>
00402         getSigmaDists ();
00403 
00404         /** \brief This method allows to set the value of sigma used for calculating the learned weights for every single class.
00405           * \param[in] training_sigmas new sigmas for every class. If you want these values to be computed automatically,
00406           * just pass the empty array. The automatic regime calculates the maximum distance between the objects points and takes 10% of
00407           * this value as recomended in the article. If there are several objects of the same class,
00408           * then it computes the average maximum distance and takes 10%. Note that each class has its own sigma value.
00409           */
00410         void
00411         setSigmaDists (const std::vector<float>& training_sigmas);
00412 
00413         /** \brief Returns the state of Nvot coeff from [Knopp et al., 2010, (4)],
00414           * if set to false then coeff is taken as 1.0. It is just a kind of heuristic.
00415           * The default behavior is as in the article. So you can ignore this if you want.
00416           */
00417         bool
00418         getNVotState ();
00419 
00420         /** \brief Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
00421           * \param[in] state desired state, if false then Nvot is taken as 1.0
00422           */
00423         void
00424         setNVotState (bool state);
00425 
00426         /** \brief This method performs training and forms a visual vocabulary. It returns a trained model that
00427           * can be saved to file for later usage.
00428           * \param[out] model trained model
00429           */
00430         bool
00431         trainISM (ISMModelPtr& trained_model);
00432 
00433         /** \brief This function is searching for the class of interest in a given cloud
00434           * and returns the list of votes.
00435           * \param[in] model trained model which will be used for searching the objects
00436           * \param[in] in_cloud input cloud that need to be investigated
00437           * \param[in] in_normals cloud of normals coresponding to the input cloud
00438           * \param[in] in_class_of_interest class which we are looking for
00439           */
00440         boost::shared_ptr<pcl::features::ISMVoteList<PointT> >
00441         findObjects (ISMModelPtr model, typename pcl::PointCloud<PointT>::Ptr in_cloud, typename pcl::PointCloud<Normal>::Ptr in_normals, int in_class_of_interest);
00442 
00443       protected:
00444 
00445         /** \brief Extracts the descriptors from the input clouds.
00446           * \param[out] histograms it will store the descriptors for each key point
00447           * \param[out] locations it will contain the comprehensive information (such as direction, initial keypoint)
00448           * for the corresponding descriptors
00449           */
00450         bool
00451         extractDescriptors (std::vector<pcl::Histogram<FeatureSize> >& histograms,
00452                             std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations);
00453 
00454         /** \brief This method performs descriptor clustering.
00455           * \param[in] histograms descriptors to cluster
00456           * \param[out] labels it contains labels for each descriptor
00457           * \param[out] cluster_centers stores the centers of clusters
00458           */
00459         bool
00460         clusterDescriptors (std::vector< pcl::Histogram<FeatureSize> >& histograms, Eigen::MatrixXi& labels, Eigen::MatrixXf& clusters_centers);
00461 
00462         /** \brief This method calculates the value of sigma used for calculating the learned weights for every single class.
00463           * \param[out] sigmas computed sigmas.
00464           */
00465         void
00466         calculateSigmas (std::vector<float>& sigmas);
00467 
00468         /** \brief This function forms a visual vocabulary and evaluates weights
00469           * described in [Knopp et al., 2010, (5)].
00470           * \param[in] classes classes that we want to learn
00471           * \param[in] locations array containing description of each keypoint: its position, which cloud belongs
00472           * and expected direction to center
00473           * \param[in] labels labels that were obtained during k-means clustering
00474           * \param[in] sigmas array of sigmas for each class
00475           * \param[in] clusters clusters that were obtained during k-means clustering
00476           * \param[out] statistical_weights stores the computed statistical weights
00477           * \param[out] learned_weights stores the computed learned weights
00478           */
00479         void
00480         calculateWeights (const std::vector< LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
00481                           const Eigen::MatrixXi &labels,
00482                           std::vector<float>& sigmas,
00483                           std::vector<std::vector<unsigned int> >& clusters,
00484                           std::vector<std::vector<float> >& statistical_weights,
00485                           std::vector<float>& learned_weights);
00486 
00487         /** \brief Simplifies the cloud using voxel grid principles.
00488           * \param[in] in_point_cloud cloud that need to be simplified
00489           * \param[in] in_normal_cloud normals of the cloud that need to be simplified
00490           * \param[out] out_sampled_point_cloud simplified cloud
00491           * \param[out] out_sampled_normal_cloud and the corresponding normals
00492           */
00493         void
00494         simplifyCloud (typename pcl::PointCloud<PointT>::ConstPtr in_point_cloud,
00495                        typename pcl::PointCloud<NormalT>::ConstPtr in_normal_cloud,
00496                        typename pcl::PointCloud<PointT>::Ptr out_sampled_point_cloud,
00497                        typename pcl::PointCloud<NormalT>::Ptr out_sampled_normal_cloud);
00498 
00499         /** \brief This method simply shifts the clouds points relative to the passed point.
00500           * \param[in] in_cloud cloud to shift
00501           * \param[in] shift_point point relative to which the cloud will be shifted
00502           */
00503         void
00504         shiftCloud (typename pcl::PointCloud<PointT>::Ptr in_cloud, Eigen::Vector3f shift_point);
00505 
00506         /** \brief This method simply computes the rotation matrix, so that the given normal
00507           * would match the Y axis after the transformation. This is done because the algorithm needs to be invariant
00508           * to the affine transformations.
00509           * \param[in] in_normal normal for which the rotation matrix need to be computed
00510           */
00511         Eigen::Matrix3f
00512         alignYCoordWithNormal (const NormalT& in_normal);
00513 
00514         /** \brief This method applies transform set in in_transform to vector io_vector.
00515           * \param[in] io_vec vector that need to be transformed
00516           * \param[in] in_transform matrix that contains the transformation
00517           */
00518         void
00519         applyTransform (Eigen::Vector3f& io_vec, const Eigen::Matrix3f& in_transform);
00520 
00521         /** \brief This method estimates features for the given point cloud.
00522           * \param[in] sampled_point_cloud sampled point cloud for which the features must be computed
00523           * \param[in] point_cloud original point cloud
00524           * \param[in] normal_cloud normals for the original point cloud
00525           * \param[out] feature_cloud it will store the computed histograms (features) for the given cloud
00526           */
00527         void
00528         estimateFeatures (typename pcl::PointCloud<PointT>::Ptr sampled_point_cloud,
00529                           typename pcl::PointCloud<NormalT>::Ptr normal_cloud,
00530                           typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud);
00531 
00532         /** \brief Performs K-means clustering.
00533           * \param[in] points_to_cluster points to cluster
00534           * \param[in] number_of_clusters desired number of clusters
00535           * \param[out] io_labels output parameter, which stores the label for each point
00536           * \param[in] criteria defines when the computational process need to be finished. For example if the
00537           * desired accuracy is achieved or the iteration number exceeds given value
00538           * \param[in] attempts number of attempts to compute clustering
00539           * \param[in] flags if set to USE_INITIAL_LABELS then initial approximation of labels is taken from io_labels
00540           * \param[out] cluster_centers it will store the cluster centers
00541           */
00542         double
00543         computeKMeansClustering (const Eigen::MatrixXf& points_to_cluster,
00544                                  int number_of_clusters,
00545                                  Eigen::MatrixXi& io_labels,
00546                                  TermCriteria criteria,
00547                                  int attempts,
00548                                  int flags,
00549                                  Eigen::MatrixXf& cluster_centers);
00550 
00551         /** \brief Generates centers for clusters as described in 
00552           * Arthur, David and Sergei Vassilvitski (2007) k-means++: The Advantages of Careful Seeding.
00553           * \param[in] data points to cluster
00554           * \param[out] out_centers it will contain generated centers
00555           * \param[in] number_of_clusters defines the number of desired cluster centers
00556           * \param[in] trials number of trials to generate a center
00557           */
00558         void
00559         generateCentersPP (const Eigen::MatrixXf& data,
00560                            Eigen::MatrixXf& out_centers,
00561                            int number_of_clusters,
00562                            int trials);
00563 
00564         /** \brief Generates random center for cluster.
00565           * \param[in] boxes contains min and max values for each dimension
00566           * \param[out] center it will the contain generated center
00567           */
00568         void
00569         generateRandomCenter (const std::vector<Eigen::Vector2f>& boxes, Eigen::VectorXf& center);
00570 
00571         /** \brief Computes the square distance beetween two vectors.
00572           * \param[in] vec_1 first vector
00573           * \param[in] vec_2 second vector
00574           */
00575         float
00576         computeDistance (Eigen::VectorXf& vec_1, Eigen::VectorXf& vec_2);
00577 
00578         /** \brief Forbids the assignment operator. */
00579         ImplicitShapeModelEstimation&
00580         operator= (const ImplicitShapeModelEstimation&);
00581 
00582       protected:
00583 
00584         /** \brief Stores the clouds used for training. */
00585         std::vector<typename pcl::PointCloud<PointT>::Ptr> training_clouds_;
00586 
00587         /** \brief Stores the class number for each cloud from training_clouds_. */
00588         std::vector<unsigned int> training_classes_;
00589 
00590         /** \brief Stores the normals for each training cloud. */
00591         std::vector<typename pcl::PointCloud<NormalT>::Ptr> training_normals_;
00592 
00593         /** \brief This array stores the sigma values for each training class. If this array has a size equals 0, then
00594           * sigma values will be calculated automatically.
00595           */
00596         std::vector<float> training_sigmas_;
00597 
00598         /** \brief This value is used for the simplification. It sets the size of grid bin. */
00599         float sampling_size_;
00600 
00601         /** \brief Stores the feature estimator. */
00602         boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > > feature_estimator_;
00603 
00604         /** \brief Number of clusters, is used for clustering descriptors during the training. */
00605         unsigned int number_of_clusters_;
00606 
00607         /** \brief If set to false then Nvot coeff from [Knopp et al., 2010, (4)] is equal 1.0. */
00608         bool n_vot_ON_;
00609 
00610         /** \brief This const value is used for indicating that for k-means clustering centers must
00611           * be generated as described in
00612           * Arthur, David and Sergei Vassilvitski (2007) k-means++: The Advantages of Careful Seeding. */
00613         static const int PP_CENTERS = 2;
00614 
00615         /** \brief This const value is used for indicating that input labels must be taken as the
00616           * initial approximation for k-means clustering. */
00617         static const int USE_INITIAL_LABELS = 1;
00618     };
00619   }
00620 }
00621 
00622 POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ISMPeak,
00623   (float, x, x)
00624   (float, y, y)
00625   (float, z, z)
00626   (float, density, ism_density)
00627   (float, class_id, ism_class_id)
00628 )
00629 
00630 #endif  //#ifndef PCL_IMPLICIT_SHAPE_MODEL_H_