Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/features/include/pcl/features/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$
00038  *
00039  */
00040 
00041 #ifndef PCL_FEATURES_CVFH_H_
00042 #define PCL_FEATURES_CVFH_H_
00043 
00044 #include <pcl/features/feature.h>
00045 #include <pcl/features/vfh.h>
00046 #include <pcl/search/pcl_search.h>
00047 #include <pcl/common/common.h>
00048 
00049 namespace pcl
00050 {
00051   /** \brief CVFHEstimation estimates the Clustered Viewpoint Feature Histogram (CVFH) descriptor for a given 
00052     * point cloud dataset containing XYZ data and normals, as presented in: 
00053     *   - CAD-Model Recognition and 6 DOF Pose Estimation
00054     *     A. Aldoma, N. Blodow, D. Gossow, S. Gedikli, R.B. Rusu, M. Vincze and G. Bradski
00055     *     ICCV 2011, 3D Representation and Recognition (3dRR11) workshop
00056     *     Barcelona, Spain, (2011) 
00057     *
00058     * The suggested PointOutT is pcl::VFHSignature308.
00059     *
00060     * \author Aitor Aldoma
00061     * \ingroup features
00062     */
00063   template<typename PointInT, typename PointNT, typename PointOutT = pcl::VFHSignature308>
00064   class CVFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
00065   {
00066     public:
00067       typedef boost::shared_ptr<CVFHEstimation<PointInT, PointNT, PointOutT> > Ptr;
00068       typedef boost::shared_ptr<const CVFHEstimation<PointInT, PointNT, PointOutT> > ConstPtr;
00069 
00070       using Feature<PointInT, PointOutT>::feature_name_;
00071       using Feature<PointInT, PointOutT>::getClassName;
00072       using Feature<PointInT, PointOutT>::indices_;
00073       using Feature<PointInT, PointOutT>::k_;
00074       using Feature<PointInT, PointOutT>::search_radius_;
00075       using Feature<PointInT, PointOutT>::surface_;
00076       using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
00077 
00078       typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00079       typedef typename pcl::search::Search<PointNormal>::Ptr KdTreePtr;
00080       typedef typename pcl::VFHEstimation<PointInT, PointNT, pcl::VFHSignature308> VFHEstimator;
00081 
00082       /** \brief Empty constructor. */
00083       CVFHEstimation () :
00084         vpx_ (0), vpy_ (0), vpz_ (0), 
00085         leaf_size_ (0.005f), 
00086         normalize_bins_ (false),
00087         curv_threshold_ (0.03f), 
00088         cluster_tolerance_ (leaf_size_ * 3), 
00089         eps_angle_threshold_ (0.125f), 
00090         min_points_ (50),
00091         radius_normals_ (leaf_size_ * 3),
00092         centroids_dominant_orientations_ (),
00093         dominant_normals_ ()
00094       {
00095         search_radius_ = 0;
00096         k_ = 1;
00097         feature_name_ = "CVFHEstimation";
00098       }
00099       ;
00100 
00101       /** \brief Removes normals with high curvature caused by real edges or noisy data
00102         * \param[in] cloud pointcloud to be filtered
00103         * \param[out] indices_out the indices of the points with higher curvature than threshold
00104         * \param[out] indices_in the indices of the remaining points after filtering
00105         * \param[in] threshold threshold value for curvature
00106         */
00107       void
00108       filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, std::vector<int> & indices_to_use, std::vector<int> &indices_out,
00109                                       std::vector<int> &indices_in, float threshold);
00110 
00111       /** \brief Set the viewpoint.
00112         * \param[in] vpx the X coordinate of the viewpoint
00113         * \param[in] vpy the Y coordinate of the viewpoint
00114         * \param[in] vpz the Z coordinate of the viewpoint
00115         */
00116       inline void
00117       setViewPoint (float vpx, float vpy, float vpz)
00118       {
00119         vpx_ = vpx;
00120         vpy_ = vpy;
00121         vpz_ = vpz;
00122       }
00123 
00124       /** \brief Set the radius used to compute normals
00125         * \param[in] radius_normals the radius
00126         */
00127       inline void
00128       setRadiusNormals (float radius_normals)
00129       {
00130         radius_normals_ = radius_normals;
00131       }
00132 
00133       /** \brief Get the viewpoint. 
00134         * \param[out] vpx the X coordinate of the viewpoint
00135         * \param[out] vpy the Y coordinate of the viewpoint
00136         * \param[out] vpz the Z coordinate of the viewpoint
00137         */
00138       inline void
00139       getViewPoint (float &vpx, float &vpy, float &vpz)
00140       {
00141         vpx = vpx_;
00142         vpy = vpy_;
00143         vpz = vpz_;
00144       }
00145 
00146       /** \brief Get the centroids used to compute different CVFH descriptors
00147         * \param[out] centroids vector to hold the centroids
00148         */
00149       inline void
00150       getCentroidClusters (std::vector<Eigen::Vector3f> & centroids)
00151       {
00152         for (size_t i = 0; i < centroids_dominant_orientations_.size (); ++i)
00153           centroids.push_back (centroids_dominant_orientations_[i]);
00154       }
00155 
00156       /** \brief Get the normal centroids used to compute different CVFH descriptors
00157         * \param[out] centroids vector to hold the normal centroids
00158         */
00159       inline void
00160       getCentroidNormalClusters (std::vector<Eigen::Vector3f> & centroids)
00161       {
00162         for (size_t i = 0; i < dominant_normals_.size (); ++i)
00163           centroids.push_back (dominant_normals_[i]);
00164       }
00165 
00166       /** \brief Sets max. Euclidean distance between points to be added to the cluster 
00167         * \param[in] d the maximum Euclidean distance 
00168         */
00169 
00170       inline void
00171       setClusterTolerance (float d)
00172       {
00173         cluster_tolerance_ = d;
00174       }
00175 
00176       /** \brief Sets max. deviation of the normals between two points so they can be clustered together
00177         * \param[in] d the maximum deviation 
00178         */
00179       inline void
00180       setEPSAngleThreshold (float d)
00181       {
00182         eps_angle_threshold_ = d;
00183       }
00184 
00185       /** \brief Sets curvature threshold for removing normals
00186         * \param[in] d the curvature threshold 
00187         */
00188       inline void
00189       setCurvatureThreshold (float d)
00190       {
00191         curv_threshold_ = d;
00192       }
00193 
00194       /** \brief Set minimum amount of points for a cluster to be considered
00195         * \param[in] min the minimum amount of points to be set 
00196         */
00197       inline void
00198       setMinPoints (size_t min)
00199       {
00200         min_points_ = min;
00201       }
00202 
00203       /** \brief Sets wether if the CVFH signatures should be normalized or not
00204         * \param[in] normalize true if normalization is required, false otherwise 
00205         */
00206       inline void
00207       setNormalizeBins (bool normalize)
00208       {
00209         normalize_bins_ = normalize;
00210       }
00211 
00212       /** \brief Overloaded computed method from pcl::Feature.
00213         * \param[out] output the resultant point cloud model dataset containing the estimated features
00214         */
00215       void
00216       compute (PointCloudOut &output);
00217 
00218     private:
00219       /** \brief Values describing the viewpoint ("pinhole" camera model assumed). 
00220         * By default, the viewpoint is set to 0,0,0.
00221         */
00222       float vpx_, vpy_, vpz_;
00223 
00224       /** \brief Size of the voxels after voxel gridding. IMPORTANT: Must match the voxel 
00225         * size of the training data or the normalize_bins_ flag must be set to true.
00226         */
00227       float leaf_size_;
00228 
00229       /** \brief Wether to normalize the signatures or not. Default: false. */
00230       bool normalize_bins_;
00231 
00232       /** \brief Curvature threshold for removing normals. */
00233       float curv_threshold_;
00234 
00235       /** \brief allowed Euclidean distance between points to be added to the cluster. */
00236       float cluster_tolerance_;
00237 
00238       /** \brief deviation of the normals between two points so they can be clustered together. */
00239       float eps_angle_threshold_;
00240 
00241       /** \brief Minimum amount of points in a clustered region to be considered stable for CVFH
00242         * computation.
00243         */
00244       size_t min_points_;
00245 
00246       /** \brief Radius for the normals computation. */
00247       float radius_normals_;
00248 
00249       /** \brief Estimate the Clustered Viewpoint Feature Histograms (CVFH) descriptors at 
00250         * a set of points given by <setInputCloud (), setIndices ()> using the surface in
00251         * setSearchSurface ()
00252         *
00253         * \param[out] output the resultant point cloud model dataset that contains the CVFH
00254         * feature estimates
00255         */
00256       void
00257       computeFeature (PointCloudOut &output);
00258 
00259       /** \brief Region growing method using Euclidean distances and neighbors normals to 
00260         * add points to a region.
00261         * \param[in] cloud point cloud to split into regions
00262         * \param[in] normals are the normals of cloud
00263         * \param[in] tolerance is the allowed Euclidean distance between points to be added to
00264         * the cluster
00265         * \param[in] tree is the spatial search structure for nearest neighbour search
00266         * \param[out] clusters vector of indices representing the clustered regions
00267         * \param[in] eps_angle deviation of the normals between two points so they can be
00268         * clustered together
00269         * \param[in] min_pts_per_cluster minimum cluster size. (default: 1 point)
00270         * \param[in] max_pts_per_cluster maximum cluster size. (default: all the points)
00271         */
00272       void
00273       extractEuclideanClustersSmooth (const pcl::PointCloud<pcl::PointNormal> &cloud,
00274                                       const pcl::PointCloud<pcl::PointNormal> &normals, float tolerance,
00275                                       const pcl::search::Search<pcl::PointNormal>::Ptr &tree,
00276                                       std::vector<pcl::PointIndices> &clusters, double eps_angle,
00277                                       unsigned int min_pts_per_cluster = 1,
00278                                       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
00279 
00280     protected:
00281       /** \brief Centroids that were used to compute different CVFH descriptors */
00282       std::vector<Eigen::Vector3f> centroids_dominant_orientations_;
00283       /** \brief Normal centroids that were used to compute different CVFH descriptors */
00284       std::vector<Eigen::Vector3f> dominant_normals_;
00285   };
00286 }
00287 
00288 #ifdef PCL_NO_PRECOMPILE
00289 #include <pcl/features/impl/cvfh.hpp>
00290 #endif
00291 
00292 #endif  //#ifndef PCL_FEATURES_CVFH_H_