our_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 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 {
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;
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 
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 
00130       void
00131       computeRFAndShapeDistribution (PointInTPtr & processed, PointCloudOut &output, std::vector<pcl::PointIndices> & cluster_indices);
00132 
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 
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 
00160       inline void
00161       setViewPoint (float vpx, float vpy, float vpz)
00162       {
00163         vpx_ = vpx;
00164         vpy_ = vpy;
00165         vpz_ = vpz;
00166       }
00167 
00171       inline void
00172       setRadiusNormals (float radius_normals)
00173       {
00174         radius_normals_ = radius_normals;
00175       }
00176 
00182       inline void
00183       getViewPoint (float &vpx, float &vpy, float &vpz)
00184       {
00185         vpx = vpx_;
00186         vpy = vpy_;
00187         vpz = vpz_;
00188       }
00189 
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 
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 
00214       inline void
00215       setClusterTolerance (float d)
00216       {
00217         cluster_tolerance_ = d;
00218       }
00219 
00223       inline void
00224       setEPSAngleThreshold (float d)
00225       {
00226         eps_angle_threshold_ = d;
00227       }
00228 
00232       inline void
00233       setCurvatureThreshold (float d)
00234       {
00235         curv_threshold_ = d;
00236       }
00237 
00241       inline void
00242       setMinPoints (size_t min)
00243       {
00244         min_points_ = min;
00245       }
00246 
00250       inline void
00251       setNormalizeBins (bool normalize)
00252       {
00253         normalize_bins_ = normalize;
00254       }
00255 
00259       inline void
00260       getClusterIndices (std::vector<pcl::PointIndices> & indices)
00261       {
00262         indices = clusters_;
00263       }
00264 
00268       void
00269       setRefineClusters (float rc)
00270       {
00271         refine_clusters_ = rc;
00272       }
00273 
00277       void
00278       getTransforms (std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & trans)
00279       {
00280         trans = transforms_;
00281       }
00282 
00287       void
00288       getValidTransformsVec (std::vector<bool> & valid)
00289       {
00290         valid = valid_transforms_;
00291       }
00292 
00296       void
00297       setAxisRatio (float f)
00298       {
00299         axis_ratio_ = f;
00300       }
00301 
00305       void
00306       setMinAxisValue (float f)
00307       {
00308         min_axis_value_ = f;
00309       }
00310 
00314       void
00315       compute (PointCloudOut &output);
00316 
00317     private:
00321       float vpx_, vpy_, vpz_;
00322 
00326       float leaf_size_;
00327 
00329       bool normalize_bins_;
00330 
00332       float curv_threshold_;
00333 
00335       float cluster_tolerance_;
00336 
00338       float eps_angle_threshold_;
00339 
00343       size_t min_points_;
00344 
00346       float radius_normals_;
00347 
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 
00364       void
00365       computeFeature (PointCloudOut &output);
00366 
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:
00388       std::vector<Eigen::Vector3f> centroids_dominant_orientations_;
00390       std::vector<Eigen::Vector3f> dominant_normals_;
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_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:27:30