feature.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  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: feature.h 3022 2011-11-01 03:42:22Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_FEATURE_H_
00041 #define PCL_FEATURE_H_
00042 
00043 #include <boost/function.hpp>
00044 #include <boost/bind.hpp>
00045 #include <boost/mpl/size.hpp>
00046 
00047 // PCL includes
00048 #include "pcl/pcl_base.h"
00049 #include "pcl/common/eigen.h"
00050 #include "pcl/common/centroid.h"
00051 
00052 #include "pcl/search/search.h"
00053 
00054 namespace pcl
00055 {
00067   inline void 
00068   solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 
00069                         const Eigen::Vector4f &point, 
00070                         Eigen::Vector4f &plane_parameters, float &curvature);
00071 
00084   inline void 
00085   solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 
00086                         float &nx, float &ny, float &nz, float &curvature);
00087 
00091 
00096   template <typename PointInT, typename PointOutT>
00097   class Feature : public PCLBase<PointInT>
00098   {
00099     public:
00100       using PCLBase<PointInT>::indices_;
00101       using PCLBase<PointInT>::input_;
00102 
00103       typedef PCLBase<PointInT> BaseClass;
00104 
00105       typedef boost::shared_ptr< Feature<PointInT, PointOutT> > Ptr;
00106       typedef boost::shared_ptr< const Feature<PointInT, PointOutT> > ConstPtr;
00107       
00108       typedef typename pcl::search::Search<PointInT> KdTree;
00109       typedef typename pcl::search::Search<PointInT>::Ptr KdTreePtr;
00110 
00111       typedef pcl::PointCloud<PointInT> PointCloudIn;
00112       typedef typename PointCloudIn::Ptr PointCloudInPtr;
00113       typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00114 
00115       typedef pcl::PointCloud<PointOutT> PointCloudOut;
00116 
00117       typedef boost::function<int (size_t, double, std::vector<int> &, std::vector<float> &)> SearchMethod;
00118       typedef boost::function<int (const PointCloudIn &cloud, size_t index, double, std::vector<int> &, std::vector<float> &)> SearchMethodSurface;
00119     
00120     public:
00122       Feature () : surface_(), tree_(), search_parameter_(0), search_radius_(0), k_(0), fake_surface_(false)
00123       {};
00124 
00132       inline void
00133       setSearchSurface (const PointCloudInConstPtr &cloud)
00134       {
00135         surface_ = cloud;
00136         fake_surface_ = false;
00137         //use_surface_  = true;
00138       }
00139 
00141       inline PointCloudInConstPtr 
00142       getSearchSurface () { return (surface_); }
00143 
00147       inline void 
00148       setSearchMethod (const KdTreePtr &tree) { tree_ = tree; }
00149 
00151       inline KdTreePtr 
00152       getSearchMethod () { return (tree_); }
00153 
00155       inline double 
00156       getSearchParameter () { return (search_parameter_); }
00157 
00161       inline void 
00162       setKSearch (int k) { k_ = k; }
00163 
00165       inline int 
00166       getKSearch () { return (k_); }
00167 
00172       inline void 
00173       setRadiusSearch (double radius) { search_radius_ = radius; }
00174 
00176       inline double 
00177       getRadiusSearch () { return (search_radius_); }
00178 
00184       void 
00185       compute (PointCloudOut &output);
00186 
00195       inline int
00196       searchForNeighbors (size_t index, double parameter, 
00197                           std::vector<int> &indices, std::vector<float> &distances) const
00198       {
00199         if (surface_ == input_)       // if the two surfaces are the same
00200           return (search_method_ (index, parameter, indices, distances));
00201         else
00202           return (search_method_surface_ (*input_, index, parameter, indices, distances));
00203       }
00204 
00214       inline int
00215       searchForNeighbors (const PointCloudIn &cloud, size_t index, double parameter, 
00216                           std::vector<int> &indices, std::vector<float> &distances) const
00217       {
00218         return (search_method_surface_ (cloud, index, parameter, indices, distances));
00219       }
00220 
00221     protected:
00223       std::string feature_name_;
00224 
00226       SearchMethod search_method_;
00227 
00229       SearchMethodSurface search_method_surface_;
00230 
00234       PointCloudInConstPtr surface_;
00235 
00237       KdTreePtr tree_;
00238 
00240       double search_parameter_;
00241 
00243       double search_radius_;
00244 
00246       int k_;
00247 
00249       inline const std::string& 
00250       getClassName () const { return (feature_name_); }
00251 
00253       virtual inline bool
00254       initCompute ();
00255 
00257       virtual inline bool
00258       deinitCompute ();
00259 
00261       bool fake_surface_;
00262 
00263     private:
00265       virtual void 
00266       computeFeature (PointCloudOut &output) = 0;
00267       
00268     public:
00269       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00270   };
00271 
00272 
00276   template <typename PointInT, typename PointNT, typename PointOutT>
00277   class FeatureFromNormals : public Feature<PointInT, PointOutT>
00278   {
00279     typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn;
00280     typedef typename PointCloudIn::Ptr PointCloudInPtr;
00281     typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00282     typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00283 
00284     public:
00285       typedef typename pcl::PointCloud<PointNT> PointCloudN;
00286       typedef typename PointCloudN::Ptr PointCloudNPtr;
00287       typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
00288 
00289       typedef boost::shared_ptr< FeatureFromNormals<PointInT, PointNT, PointOutT> > Ptr;
00290       typedef boost::shared_ptr< const FeatureFromNormals<PointInT, PointNT, PointOutT> > ConstPtr;
00291 
00292       // Members derived from the base class
00293       using Feature<PointInT, PointOutT>::input_;
00294       using Feature<PointInT, PointOutT>::surface_;
00295       using Feature<PointInT, PointOutT>::getClassName;
00296 
00298       FeatureFromNormals () {};
00299 
00307       inline void 
00308       setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
00309 
00311       inline PointCloudNConstPtr 
00312       getInputNormals () { return (normals_); }
00313 
00314     protected:     
00318       PointCloudNConstPtr normals_;
00319 
00321       virtual bool
00322       initCompute ();
00323 
00324     public:
00325       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00326   };
00327 }
00328 
00329 #include "pcl/features/impl/feature.hpp"
00330 
00331 #endif  //#ifndef PCL_FEATURE_H_