Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/features/include/pcl/features/our_cvfh.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id: cvfh.h 4936 2012-03-07 11:12:45Z aaldoma $
00038  *
00039  */
00040 
00041 #ifndef PCL_FEATURES_OURCVFH_H_
00042 #define PCL_FEATURES_OURCVFH_H_
00043 
00044 #include <pcl/features/feature.h>
00045 #include <pcl/search/pcl_search.h>
00046 #include <pcl/common/common.h>
00047 
00048 namespace pcl
00049 {
00050   /** \brief OURCVFHEstimation estimates the Oriented, Unique and Repetable Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given
00051    * point cloud dataset given XYZ data and normals, as presented in:
00052    *     - OUR-CVFH – Oriented, Unique and Repeatable Clustered Viewpoint Feature Histogram for Object Recognition and 6DOF Pose Estimation
00053    *     A. Aldoma, F. Tombari, R.B. Rusu and M. Vincze
00054    *     DAGM-OAGM 2012
00055    *     Graz, Austria
00056    * The suggested PointOutT is pcl::VFHSignature308.
00057    *
00058    * \author Aitor Aldoma
00059    * \ingroup features
00060    */
00061   template<typename PointInT, typename PointNT, typename PointOutT = pcl::VFHSignature308>
00062   class OURCVFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
00063   {
00064     public:
00065       typedef boost::shared_ptr<OURCVFHEstimation<PointInT, PointNT, PointOutT> > Ptr;
00066       typedef boost::shared_ptr<const OURCVFHEstimation<PointInT, PointNT, PointOutT> > ConstPtr;
00067       using Feature<PointInT, PointOutT>::feature_name_;
00068       using Feature<PointInT, PointOutT>::getClassName;
00069       using Feature<PointInT, PointOutT>::indices_;
00070       using Feature<PointInT, PointOutT>::k_;
00071       using Feature<PointInT, PointOutT>::search_radius_;
00072       using Feature<PointInT, PointOutT>::surface_;
00073       using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
00074 
00075       typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00076       typedef typename pcl::search::Search<PointNormal>::Ptr KdTreePtr;
00077       typedef typename pcl::PointCloud<PointInT>::Ptr PointInTPtr;
00078       /** \brief Empty constructor. */
00079       OURCVFHEstimation () :
00080         vpx_ (0), vpy_ (0), vpz_ (0), leaf_size_ (0.005f), normalize_bins_ (false), curv_threshold_ (0.03f), cluster_tolerance_ (leaf_size_ * 3),
00081             eps_angle_threshold_ (0.125f), min_points_ (50), radius_normals_ (leaf_size_ * 3), centroids_dominant_orientations_ (),
00082             dominant_normals_ ()
00083       {
00084         search_radius_ = 0;
00085         k_ = 1;
00086         feature_name_ = "OURCVFHEstimation";
00087         refine_clusters_ = 1.f;
00088         min_axis_value_ = 0.925f;
00089         axis_ratio_ = 0.8f;
00090       }
00091       ;
00092 
00093       /** \brief Creates an affine transformation from the RF axes
00094        * \param[in] evx the x-axis
00095        * \param[in] evy the z-axis
00096        * \param[in] evz the z-axis
00097        * \param[out] transformPC the resulting transformation
00098        * \param[in] center_mat 4x4 matrix concatenated to the resulting transformation
00099        */
00100       inline Eigen::Matrix4f
00101       createTransFromAxes (Eigen::Vector3f & evx, Eigen::Vector3f & evy, Eigen::Vector3f & evz, Eigen::Affine3f & transformPC,
00102                            Eigen::Matrix4f & center_mat)
00103       {
00104         Eigen::Matrix4f trans;
00105         trans.setIdentity (4, 4);
00106         trans (0, 0) = evx (0, 0);
00107         trans (1, 0) = evx (1, 0);
00108         trans (2, 0) = evx (2, 0);
00109         trans (0, 1) = evy (0, 0);
00110         trans (1, 1) = evy (1, 0);
00111         trans (2, 1) = evy (2, 0);
00112         trans (0, 2) = evz (0, 0);
00113         trans (1, 2) = evz (1, 0);
00114         trans (2, 2) = evz (2, 0);
00115 
00116         Eigen::Matrix4f homMatrix = Eigen::Matrix4f ();
00117         homMatrix.setIdentity (4, 4);
00118         homMatrix = transformPC.matrix ();
00119 
00120         Eigen::Matrix4f trans_copy = trans.inverse ();
00121         trans = trans_copy * center_mat * homMatrix;
00122         return trans;
00123       }
00124 
00125       /** \brief Computes SGURF and the shape distribution based on the selected SGURF
00126        * \param[in] processed the input cloud
00127        * \param[out] output the resulting signature
00128        * \param[in] cluster_indices the indices of the stable cluster
00129        */
00130       void
00131       computeRFAndShapeDistribution (PointInTPtr & processed, PointCloudOut &output, std::vector<pcl::PointIndices> & cluster_indices);
00132 
00133       /** \brief Computes SGURF
00134        * \param[in] centroid the centroid of the cluster
00135        * \param[in] normal_centroid the average of the normals
00136        * \param[in] processed the input cloud
00137        * \param[out] transformations the transformations aligning the cloud to the SGURF axes
00138        * \param[out] grid the cloud transformed internally
00139        * \param[in] indices the indices of the stable cluster
00140        */
00141       bool
00142       sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid, PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
00143              PointInTPtr & grid, pcl::PointIndices & indices);
00144 
00145       /** \brief Removes normals with high curvature caused by real edges or noisy data
00146        * \param[in] cloud pointcloud to be filtered
00147        * \param[out] indices_out the indices of the points with higher curvature than threshold
00148        * \param[out] indices_in the indices of the remaining points after filtering
00149        * \param[in] threshold threshold value for curvature
00150        */
00151       void
00152       filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, std::vector<int> & indices_to_use, std::vector<int> &indices_out,
00153                                       std::vector<int> &indices_in, float threshold);
00154 
00155       /** \brief Set the viewpoint.
00156        * \param[in] vpx the X coordinate of the viewpoint
00157        * \param[in] vpy the Y coordinate of the viewpoint
00158        * \param[in] vpz the Z coordinate of the viewpoint
00159        */
00160       inline void
00161       setViewPoint (float vpx, float vpy, float vpz)
00162       {
00163         vpx_ = vpx;
00164         vpy_ = vpy;
00165         vpz_ = vpz;
00166       }
00167 
00168       /** \brief Set the radius used to compute normals
00169        * \param[in] radius_normals the radius
00170        */
00171       inline void
00172       setRadiusNormals (float radius_normals)
00173       {
00174         radius_normals_ = radius_normals;
00175       }
00176 
00177       /** \brief Get the viewpoint. 
00178        * \param[out] vpx the X coordinate of the viewpoint
00179        * \param[out] vpy the Y coordinate of the viewpoint
00180        * \param[out] vpz the Z coordinate of the viewpoint
00181        */
00182       inline void
00183       getViewPoint (float &vpx, float &vpy, float &vpz)
00184       {
00185         vpx = vpx_;
00186         vpy = vpy_;
00187         vpz = vpz_;
00188       }
00189 
00190       /** \brief Get the centroids used to compute different CVFH descriptors
00191        * \param[out] centroids vector to hold the centroids
00192        */
00193       inline void
00194       getCentroidClusters (std::vector<Eigen::Vector3f> & centroids)
00195       {
00196         for (size_t i = 0; i < centroids_dominant_orientations_.size (); ++i)
00197           centroids.push_back (centroids_dominant_orientations_[i]);
00198       }
00199 
00200       /** \brief Get the normal centroids used to compute different CVFH descriptors
00201        * \param[out] centroids vector to hold the normal centroids
00202        */
00203       inline void
00204       getCentroidNormalClusters (std::vector<Eigen::Vector3f> & centroids)
00205       {
00206         for (size_t i = 0; i < dominant_normals_.size (); ++i)
00207           centroids.push_back (dominant_normals_[i]);
00208       }
00209 
00210       /** \brief Sets max. Euclidean distance between points to be added to the cluster 
00211        * \param[in] d the maximum Euclidean distance
00212        */
00213 
00214       inline void
00215       setClusterTolerance (float d)
00216       {
00217         cluster_tolerance_ = d;
00218       }
00219 
00220       /** \brief Sets max. deviation of the normals between two points so they can be clustered together
00221        * \param[in] d the maximum deviation
00222        */
00223       inline void
00224       setEPSAngleThreshold (float d)
00225       {
00226         eps_angle_threshold_ = d;
00227       }
00228 
00229       /** \brief Sets curvature threshold for removing normals
00230        * \param[in] d the curvature threshold
00231        */
00232       inline void
00233       setCurvatureThreshold (float d)
00234       {
00235         curv_threshold_ = d;
00236       }
00237 
00238       /** \brief Set minimum amount of points for a cluster to be considered
00239        * \param[in] min the minimum amount of points to be set
00240        */
00241       inline void
00242       setMinPoints (size_t min)
00243       {
00244         min_points_ = min;
00245       }
00246 
00247       /** \brief Sets wether if the signatures should be normalized or not
00248        * \param[in] normalize true if normalization is required, false otherwise
00249        */
00250       inline void
00251       setNormalizeBins (bool normalize)
00252       {
00253         normalize_bins_ = normalize;
00254       }
00255 
00256       /** \brief Gets the indices of the original point cloud used to compute the signatures
00257        * \param[out] indices vector of point indices
00258        */
00259       inline void
00260       getClusterIndices (std::vector<pcl::PointIndices> & indices)
00261       {
00262         indices = clusters_;
00263       }
00264 
00265       /** \brief Sets the refinement factor for the clusters
00266        * \param[in] rc the factor used to decide if a point is used to estimate a stable cluster
00267        */
00268       void
00269       setRefineClusters (float rc)
00270       {
00271         refine_clusters_ = rc;
00272       }
00273 
00274       /** \brief Returns the transformations aligning the point cloud to the corresponding SGURF
00275        * \param[out] trans vector of transformations
00276        */
00277       void
00278       getTransforms (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & trans)
00279       {
00280         trans = transforms_;
00281       }
00282 
00283       /** \brief Returns a boolean vector indicating of the transformation obtained by getTransforms() represents
00284        * a valid SGURF
00285        * \param[out] valid vector of booleans
00286        */
00287       void
00288       getValidTransformsVec (std::vector<bool> & valid)
00289       {
00290         valid = valid_transforms_;
00291       }
00292 
00293       /** \brief Sets the min axis ratio between the SGURF axes to decide if disambiguition is feasible
00294        * \param[in] f the ratio between axes
00295        */
00296       void
00297       setAxisRatio (float f)
00298       {
00299         axis_ratio_ = f;
00300       }
00301 
00302       /** \brief Sets the min disambiguition axis value to generate several SGURFs for the cluster when disambiguition is difficult
00303        * \param[in] f the min axis value
00304        */
00305       void
00306       setMinAxisValue (float f)
00307       {
00308         min_axis_value_ = f;
00309       }
00310 
00311       /** \brief Overloaded computed method from pcl::Feature.
00312        * \param[out] output the resultant point cloud model dataset containing the estimated features
00313        */
00314       void
00315       compute (PointCloudOut &output);
00316 
00317     private:
00318       /** \brief Values describing the viewpoint ("pinhole" camera model assumed). 
00319        * By default, the viewpoint is set to 0,0,0.
00320        */
00321       float vpx_, vpy_, vpz_;
00322 
00323       /** \brief Size of the voxels after voxel gridding. IMPORTANT: Must match the voxel 
00324        * size of the training data or the normalize_bins_ flag must be set to true.
00325        */
00326       float leaf_size_;
00327 
00328       /** \brief Wether to normalize the signatures or not. Default: false. */
00329       bool normalize_bins_;
00330 
00331       /** \brief Curvature threshold for removing normals. */
00332       float curv_threshold_;
00333 
00334       /** \brief allowed Euclidean distance between points to be added to the cluster. */
00335       float cluster_tolerance_;
00336 
00337       /** \brief deviation of the normals between two points so they can be clustered together. */
00338       float eps_angle_threshold_;
00339 
00340       /** \brief Minimum amount of points in a clustered region to be considered stable for CVFH
00341        * computation.
00342        */
00343       size_t min_points_;
00344 
00345       /** \brief Radius for the normals computation. */
00346       float radius_normals_;
00347 
00348       /** \brief Factor for the cluster refinement */
00349       float refine_clusters_;
00350 
00351       std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_;
00352       std::vector<bool> valid_transforms_;
00353 
00354       float axis_ratio_;
00355       float min_axis_value_;
00356 
00357       /** \brief Estimate the OUR-CVFH descriptors at
00358        * a set of points given by <setInputCloud (), setIndices ()> using the surface in
00359        * setSearchSurface ()
00360        *
00361        * \param[out] output the resultant point cloud model dataset that contains the OUR-CVFH
00362        * feature estimates
00363        */
00364       void
00365       computeFeature (PointCloudOut &output);
00366 
00367       /** \brief Region growing method using Euclidean distances and neighbors normals to 
00368        * add points to a region.
00369        * \param[in] cloud point cloud to split into regions
00370        * \param[in] normals are the normals of cloud
00371        * \param[in] tolerance is the allowed Euclidean distance between points to be added to
00372        * the cluster
00373        * \param[in] tree is the spatial search structure for nearest neighbour search
00374        * \param[out] clusters vector of indices representing the clustered regions
00375        * \param[in] eps_angle deviation of the normals between two points so they can be
00376        * clustered together
00377        * \param[in] min_pts_per_cluster minimum cluster size. (default: 1 point)
00378        * \param[in] max_pts_per_cluster maximum cluster size. (default: all the points)
00379        */
00380       void
00381       extractEuclideanClustersSmooth (const pcl::PointCloud<pcl::PointNormal> &cloud, const pcl::PointCloud<pcl::PointNormal> &normals,
00382                                       float tolerance, const pcl::search::Search<pcl::PointNormal>::Ptr &tree,
00383                                       std::vector<pcl::PointIndices> &clusters, double eps_angle, unsigned int min_pts_per_cluster = 1,
00384                                       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
00385 
00386     protected:
00387       /** \brief Centroids that were used to compute different OUR-CVFH descriptors */
00388       std::vector<Eigen::Vector3f> centroids_dominant_orientations_;
00389       /** \brief Normal centroids that were used to compute different OUR-CVFH descriptors */
00390       std::vector<Eigen::Vector3f> dominant_normals_;
00391       /** \brief Indices to the points representing the stable clusters */
00392       std::vector<pcl::PointIndices> clusters_;
00393   };
00394 }
00395 
00396 #ifdef PCL_NO_PRECOMPILE
00397 #include <pcl/features/impl/our_cvfh.hpp>
00398 #endif
00399 
00400 #endif  //#ifndef PCL_FEATURES_VFH_H_