cvfh.h
Go to the documentation of this file.
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 6089 2012-07-02 18:38:08Z rusu $
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/normal_3d.h>
00046 #include <pcl/features/vfh.h>
00047 #include <pcl/search/pcl_search.h>
00048 #include <pcl/common/common.h>
00049 
00050 namespace pcl
00051 {
00064   template<typename PointInT, typename PointNT, typename PointOutT = pcl::VFHSignature308>
00065   class CVFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
00066   {
00067     public:
00068       using Feature<PointInT, PointOutT>::feature_name_;
00069       using Feature<PointInT, PointOutT>::getClassName;
00070       using Feature<PointInT, PointOutT>::indices_;
00071       using Feature<PointInT, PointOutT>::k_;
00072       using Feature<PointInT, PointOutT>::search_radius_;
00073       using Feature<PointInT, PointOutT>::surface_;
00074       using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
00075 
00076       typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00077       typedef typename pcl::search::Search<PointNormal>::Ptr KdTreePtr;
00078       typedef typename pcl::NormalEstimation<PointNormal, PointNormal> NormalEstimator;
00079       typedef typename pcl::VFHEstimation<PointInT, PointNT, pcl::VFHSignature308> VFHEstimator;
00080 
00082       CVFHEstimation () :
00083         vpx_ (0), vpy_ (0), vpz_ (0), 
00084         leaf_size_ (0.005f), 
00085         normalize_bins_ (false),
00086         curv_threshold_ (0.03f), 
00087         cluster_tolerance_ (leaf_size_ * 3), 
00088         eps_angle_threshold_ (0.125f), 
00089         min_points_ (50),
00090         radius_normals_ (leaf_size_ * 3),
00091         centroids_dominant_orientations_ (),
00092         dominant_normals_ ()
00093       {
00094         search_radius_ = 0;
00095         k_ = 1;
00096         feature_name_ = "CVFHEstimation";
00097       }
00098       ;
00099 
00106       void
00107       filterNormalsWithHighCurvature (const pcl::PointCloud<PointNT> & cloud, std::vector<int> & indices_to_use, std::vector<int> &indices_out,
00108                                       std::vector<int> &indices_in, float threshold);
00109 
00115       inline void
00116       setViewPoint (float vpx, float vpy, float vpz)
00117       {
00118         vpx_ = vpx;
00119         vpy_ = vpy;
00120         vpz_ = vpz;
00121       }
00122 
00126       inline void
00127       setRadiusNormals (float radius_normals)
00128       {
00129         radius_normals_ = radius_normals;
00130       }
00131 
00137       inline void
00138       getViewPoint (float &vpx, float &vpy, float &vpz)
00139       {
00140         vpx = vpx_;
00141         vpy = vpy_;
00142         vpz = vpz_;
00143       }
00144 
00148       inline void
00149       getCentroidClusters (std::vector<Eigen::Vector3f> & centroids)
00150       {
00151         for (size_t i = 0; i < centroids_dominant_orientations_.size (); ++i)
00152           centroids.push_back (centroids_dominant_orientations_[i]);
00153       }
00154 
00158       inline void
00159       getCentroidNormalClusters (std::vector<Eigen::Vector3f> & centroids)
00160       {
00161         for (size_t i = 0; i < dominant_normals_.size (); ++i)
00162           centroids.push_back (dominant_normals_[i]);
00163       }
00164 
00169       inline void
00170       setClusterTolerance (float d)
00171       {
00172         cluster_tolerance_ = d;
00173       }
00174 
00178       inline void
00179       setEPSAngleThreshold (float d)
00180       {
00181         eps_angle_threshold_ = d;
00182       }
00183 
00187       inline void
00188       setCurvatureThreshold (float d)
00189       {
00190         curv_threshold_ = d;
00191       }
00192 
00196       inline void
00197       setMinPoints (size_t min)
00198       {
00199         min_points_ = min;
00200       }
00201 
00205       inline void
00206       setNormalizeBins (bool normalize)
00207       {
00208         normalize_bins_ = normalize;
00209       }
00210 
00214       void
00215       compute (PointCloudOut &output);
00216 
00217     private:
00221       float vpx_, vpy_, vpz_;
00222 
00226       float leaf_size_;
00227 
00229       bool normalize_bins_;
00230 
00232       float curv_threshold_;
00233 
00235       float cluster_tolerance_;
00236 
00238       float eps_angle_threshold_;
00239 
00243       size_t min_points_;
00244 
00246       float radius_normals_;
00247 
00255       void
00256       computeFeature (PointCloudOut &output);
00257 
00271       void
00272       extractEuclideanClustersSmooth (const pcl::PointCloud<pcl::PointNormal> &cloud,
00273                                       const pcl::PointCloud<pcl::PointNormal> &normals, float tolerance,
00274                                       const pcl::search::Search<pcl::PointNormal>::Ptr &tree,
00275                                       std::vector<pcl::PointIndices> &clusters, double eps_angle,
00276                                       unsigned int min_pts_per_cluster = 1,
00277                                       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
00278 
00279     protected:
00281       std::vector<Eigen::Vector3f> centroids_dominant_orientations_;
00283       std::vector<Eigen::Vector3f> dominant_normals_;
00284 
00285     private:
00289       void 
00290       computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &) {}
00291   };
00292 
00293 }
00294 
00295 #endif  //#ifndef PCL_FEATURES_VFH_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:48