Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/features/include/pcl/features/feature.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_FEATURE_H_
00042 #define PCL_FEATURE_H_
00043 
00044 #if defined __GNUC__
00045 #  pragma GCC system_header 
00046 #endif
00047 
00048 #include <boost/function.hpp>
00049 #include <boost/bind.hpp>
00050 // PCL includes
00051 #include <pcl/pcl_base.h>
00052 #include <pcl/search/search.h>
00053 
00054 namespace pcl
00055 {
00056   /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares
00057     * plane normal and surface curvature.
00058     * \param covariance_matrix the 3x3 covariance matrix
00059     * \param point a point lying on the least-squares plane (SSE aligned)
00060     * \param plane_parameters the resultant plane parameters as: a, b, c, d (ax + by + cz + d = 0)
00061     * \param curvature the estimated surface curvature as a measure of
00062     * \f[
00063     * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
00064     * \f]
00065     * \ingroup features
00066     */
00067   inline void
00068   solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
00069                         const Eigen::Vector4f &point,
00070                         Eigen::Vector4f &plane_parameters, float &curvature);
00071 
00072   /** \brief Solve the eigenvalues and eigenvectors of a given 3x3 covariance matrix, and estimate the least-squares
00073     * plane normal and surface curvature.
00074     * \param covariance_matrix the 3x3 covariance matrix
00075     * \param nx the resultant X component of the plane normal
00076     * \param ny the resultant Y component of the plane normal
00077     * \param nz the resultant Z component of the plane normal
00078     * \param curvature the estimated surface curvature as a measure of
00079     * \f[
00080     * \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
00081     * \f]
00082     * \ingroup features
00083     */
00084   inline void
00085   solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
00086                         float &nx, float &ny, float &nz, float &curvature);
00087 
00088   ////////////////////////////////////////////////////////////////////////////////////////////
00089   ////////////////////////////////////////////////////////////////////////////////////////////
00090   ////////////////////////////////////////////////////////////////////////////////////////////
00091   /** \brief Feature represents the base feature class. Some generic 3D operations that
00092     * are applicable to all features are defined here as static methods.
00093     *
00094     * \attention
00095     * The convention for a feature descriptor is:
00096     *   - if the nearest neighbors for the query point at which the descriptor is to be computed cannot be
00097     *     determined, the descriptor values will be set to NaN (not a number)
00098     *   - it is impossible to estimate a feature descriptor for a point that doesn't have finite 3D coordinates.
00099     *     Therefore, any point that has NaN data on x, y, or z, will most likely have its descriptor set to NaN.
00100     *
00101     * \author Radu B. Rusu
00102     * \ingroup features
00103     */
00104   template <typename PointInT, typename PointOutT>
00105   class Feature : public PCLBase<PointInT>
00106   {
00107     public:
00108       using PCLBase<PointInT>::indices_;
00109       using PCLBase<PointInT>::input_;
00110 
00111       typedef PCLBase<PointInT> BaseClass;
00112 
00113       typedef boost::shared_ptr< Feature<PointInT, PointOutT> > Ptr;
00114       typedef boost::shared_ptr< const Feature<PointInT, PointOutT> > ConstPtr;
00115 
00116       typedef typename pcl::search::Search<PointInT> KdTree;
00117       typedef typename pcl::search::Search<PointInT>::Ptr KdTreePtr;
00118 
00119       typedef pcl::PointCloud<PointInT> PointCloudIn;
00120       typedef typename PointCloudIn::Ptr PointCloudInPtr;
00121       typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00122 
00123       typedef pcl::PointCloud<PointOutT> PointCloudOut;
00124 
00125       typedef boost::function<int (size_t, double, std::vector<int> &, std::vector<float> &)> SearchMethod;
00126       typedef boost::function<int (const PointCloudIn &cloud, size_t index, double, std::vector<int> &, std::vector<float> &)> SearchMethodSurface;
00127 
00128     public:
00129       /** \brief Empty constructor. */
00130       Feature () :
00131         feature_name_ (), search_method_surface_ (),
00132         surface_(), tree_(),
00133         search_parameter_(0), search_radius_(0), k_(0),
00134         fake_surface_(false)
00135       {}
00136             
00137       /** \brief Empty destructor */
00138       virtual ~Feature () {}
00139 
00140       /** \brief Provide a pointer to a dataset to add additional information
00141         * to estimate the features for every point in the input dataset.  This
00142         * is optional, if this is not set, it will only use the data in the
00143         * input cloud to estimate the features.  This is useful when you only
00144         * need to compute the features for a downsampled cloud.
00145         * \param[in] cloud a pointer to a PointCloud message
00146         */
00147       inline void
00148       setSearchSurface (const PointCloudInConstPtr &cloud)
00149       {
00150         surface_ = cloud;
00151         fake_surface_ = false;
00152         //use_surface_  = true;
00153       }
00154 
00155       /** \brief Get a pointer to the surface point cloud dataset. */
00156       inline PointCloudInConstPtr
00157       getSearchSurface () const
00158       {
00159         return (surface_);
00160       }
00161 
00162       /** \brief Provide a pointer to the search object.
00163         * \param[in] tree a pointer to the spatial search object.
00164         */
00165       inline void
00166       setSearchMethod (const KdTreePtr &tree) { tree_ = tree; }
00167 
00168       /** \brief Get a pointer to the search method used. */
00169       inline KdTreePtr
00170       getSearchMethod () const
00171       {
00172         return (tree_);
00173       }
00174 
00175       /** \brief Get the internal search parameter. */
00176       inline double
00177       getSearchParameter () const
00178       {
00179         return (search_parameter_);
00180       }
00181 
00182       /** \brief Set the number of k nearest neighbors to use for the feature estimation.
00183         * \param[in] k the number of k-nearest neighbors
00184         */
00185       inline void
00186       setKSearch (int k) { k_ = k; }
00187 
00188       /** \brief get the number of k nearest neighbors used for the feature estimation. */
00189       inline int
00190       getKSearch () const
00191       {
00192         return (k_);
00193       }
00194 
00195       /** \brief Set the sphere radius that is to be used for determining the nearest neighbors used for the feature
00196         * estimation.
00197         * \param[in] radius the sphere radius used as the maximum distance to consider a point a neighbor
00198         */
00199       inline void
00200       setRadiusSearch (double radius)
00201       {
00202         search_radius_ = radius;
00203       }
00204 
00205       /** \brief Get the sphere radius used for determining the neighbors. */
00206       inline double
00207       getRadiusSearch () const
00208       {
00209         return (search_radius_);
00210       }
00211 
00212       /** \brief Base method for feature estimation for all points given in
00213         * <setInputCloud (), setIndices ()> using the surface in setSearchSurface ()
00214         * and the spatial locator in setSearchMethod ()
00215         * \param[out] output the resultant point cloud model dataset containing the estimated features
00216         */
00217       void
00218       compute (PointCloudOut &output);
00219 
00220     protected:
00221       /** \brief The feature name. */
00222       std::string feature_name_;
00223 
00224       /** \brief The search method template for points. */
00225       SearchMethodSurface search_method_surface_;
00226 
00227       /** \brief An input point cloud describing the surface that is to be used
00228         * for nearest neighbors estimation.
00229         */
00230       PointCloudInConstPtr surface_;
00231 
00232       /** \brief A pointer to the spatial search object. */
00233       KdTreePtr tree_;
00234 
00235       /** \brief The actual search parameter (from either \a search_radius_ or \a k_). */
00236       double search_parameter_;
00237 
00238       /** \brief The nearest neighbors search radius for each point. */
00239       double search_radius_;
00240 
00241       /** \brief The number of K nearest neighbors to use for each point. */
00242       int k_;
00243 
00244       /** \brief Get a string representation of the name of this class. */
00245       inline const std::string&
00246       getClassName () const { return (feature_name_); }
00247 
00248       /** \brief This method should get called before starting the actual computation. */
00249       virtual bool
00250       initCompute ();
00251 
00252       /** \brief This method should get called after ending the actual computation. */
00253       virtual bool
00254       deinitCompute ();
00255 
00256       /** \brief If no surface is given, we use the input PointCloud as the surface. */
00257       bool fake_surface_;
00258 
00259       /** \brief Search for k-nearest neighbors using the spatial locator from
00260         * \a setSearchmethod, and the given surface from \a setSearchSurface.
00261         * \param[in] index the index of the query point
00262         * \param[in] parameter the search parameter (either k or radius)
00263         * \param[out] indices the resultant vector of indices representing the k-nearest neighbors
00264         * \param[out] distances the resultant vector of distances representing the distances from the query point to the
00265         * k-nearest neighbors
00266         *
00267         * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0.
00268         */
00269       inline int
00270       searchForNeighbors (size_t index, double parameter,
00271                           std::vector<int> &indices, std::vector<float> &distances) const
00272       {
00273         return (search_method_surface_ (*input_, index, parameter, indices, distances));
00274       }
00275 
00276       /** \brief Search for k-nearest neighbors using the spatial locator from
00277         * \a setSearchmethod, and the given surface from \a setSearchSurface.
00278         * \param[in] cloud the query point cloud
00279         * \param[in] index the index of the query point in \a cloud
00280         * \param[in] parameter the search parameter (either k or radius)
00281         * \param[out] indices the resultant vector of indices representing the k-nearest neighbors
00282         * \param[out] distances the resultant vector of distances representing the distances from the query point to the
00283         * k-nearest neighbors
00284         *
00285         * \return the number of neighbors found. If no neighbors are found or an error occurred, return 0.
00286         */
00287       inline int
00288       searchForNeighbors (const PointCloudIn &cloud, size_t index, double parameter,
00289                           std::vector<int> &indices, std::vector<float> &distances) const
00290       {
00291         return (search_method_surface_ (cloud, index, parameter, indices, distances));
00292       }
00293 
00294     private:
00295       /** \brief Abstract feature estimation method.
00296         * \param[out] output the resultant features
00297         */
00298       virtual void
00299       computeFeature (PointCloudOut &output) = 0;
00300 
00301     public:
00302       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00303   };
00304 
00305 
00306   ////////////////////////////////////////////////////////////////////////////////////////////
00307   ////////////////////////////////////////////////////////////////////////////////////////////
00308   ////////////////////////////////////////////////////////////////////////////////////////////
00309   template <typename PointInT, typename PointNT, typename PointOutT>
00310   class FeatureFromNormals : public Feature<PointInT, PointOutT>
00311   {
00312     typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn;
00313     typedef typename PointCloudIn::Ptr PointCloudInPtr;
00314     typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00315     typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00316 
00317     public:
00318       typedef typename pcl::PointCloud<PointNT> PointCloudN;
00319       typedef typename PointCloudN::Ptr PointCloudNPtr;
00320       typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
00321 
00322       typedef boost::shared_ptr< FeatureFromNormals<PointInT, PointNT, PointOutT> > Ptr;
00323       typedef boost::shared_ptr< const FeatureFromNormals<PointInT, PointNT, PointOutT> > ConstPtr;
00324 
00325       // Members derived from the base class
00326       using Feature<PointInT, PointOutT>::input_;
00327       using Feature<PointInT, PointOutT>::surface_;
00328       using Feature<PointInT, PointOutT>::getClassName;
00329 
00330       /** \brief Empty constructor. */
00331       FeatureFromNormals () : normals_ () {}
00332       
00333       /** \brief Empty destructor */
00334       virtual ~FeatureFromNormals () {}
00335 
00336       /** \brief Provide a pointer to the input dataset that contains the point normals of
00337         * the XYZ dataset.
00338         * In case of search surface is set to be different from the input cloud,
00339         * normals should correspond to the search surface, not the input cloud!
00340         * \param[in] normals the const boost shared pointer to a PointCloud of normals.
00341         * By convention, L2 norm of each normal should be 1.
00342         */
00343       inline void
00344       setInputNormals (const PointCloudNConstPtr &normals) { normals_ = normals; }
00345 
00346       /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
00347       inline PointCloudNConstPtr
00348       getInputNormals () const { return (normals_); }
00349 
00350     protected:
00351       /** \brief A pointer to the input dataset that contains the point normals of the XYZ
00352         * dataset.
00353         */
00354       PointCloudNConstPtr normals_;
00355 
00356       /** \brief This method should get called before starting the actual computation. */
00357       virtual bool
00358       initCompute ();
00359 
00360     public:
00361       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00362   };
00363 
00364   ////////////////////////////////////////////////////////////////////////////////////////////
00365   ////////////////////////////////////////////////////////////////////////////////////////////
00366   ////////////////////////////////////////////////////////////////////////////////////////////
00367   template <typename PointInT, typename PointLT, typename PointOutT>
00368   class FeatureFromLabels : public Feature<PointInT, PointOutT>
00369   {
00370     typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn;
00371     typedef typename PointCloudIn::Ptr PointCloudInPtr;
00372     typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00373 
00374     typedef typename pcl::PointCloud<PointLT> PointCloudL;
00375     typedef typename PointCloudL::Ptr PointCloudNPtr;
00376     typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
00377 
00378     typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00379 
00380     public:
00381       typedef boost::shared_ptr< FeatureFromLabels<PointInT, PointLT, PointOutT> > Ptr;
00382       typedef boost::shared_ptr< const FeatureFromLabels<PointInT, PointLT, PointOutT> > ConstPtr;
00383 
00384       // Members derived from the base class
00385       using Feature<PointInT, PointOutT>::input_;
00386       using Feature<PointInT, PointOutT>::surface_;
00387       using Feature<PointInT, PointOutT>::getClassName;
00388       using Feature<PointInT, PointOutT>::k_;
00389 
00390       /** \brief Empty constructor. */
00391       FeatureFromLabels () : labels_ ()
00392       {
00393         k_ = 1; // Search tree is not always used here.
00394       }
00395       
00396       /** \brief Empty destructor */
00397       virtual ~FeatureFromLabels () {}
00398 
00399       /** \brief Provide a pointer to the input dataset that contains the point labels of
00400         * the XYZ dataset.
00401         * In case of search surface is set to be different from the input cloud,
00402         * labels should correspond to the search surface, not the input cloud!
00403         * \param[in] labels the const boost shared pointer to a PointCloud of labels.
00404         */
00405       inline void
00406       setInputLabels (const PointCloudLConstPtr &labels)
00407       {
00408         labels_ = labels;
00409       }
00410 
00411       /** \brief Get a pointer to the labels of the input XYZ point cloud dataset. */
00412       inline PointCloudLConstPtr
00413       getInputLabels () const
00414       {
00415         return (labels_);
00416       }
00417 
00418     protected:
00419       /** \brief A pointer to the input dataset that contains the point labels of the XYZ
00420         * dataset.
00421         */
00422       PointCloudLConstPtr labels_;
00423 
00424       /** \brief This method should get called before starting the actual computation. */
00425       virtual bool
00426       initCompute ();
00427 
00428     public:
00429       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00430   };
00431 
00432   ////////////////////////////////////////////////////////////////////////////////////////////
00433   ////////////////////////////////////////////////////////////////////////////////////////////
00434   ////////////////////////////////////////////////////////////////////////////////////////////
00435   /** \brief FeatureWithLocalReferenceFrames provides a public interface for descriptor
00436     * extractor classes which need a local reference frame at each input keypoint.
00437     *
00438     * \attention
00439     * This interface is for backward compatibility with existing code and in the future it could be
00440     * merged with pcl::Feature. Subclasses should call the protected method initLocalReferenceFrames ()
00441     * to correctly initialize the frames_ member.
00442     *
00443     * \author Nicola Fioraio
00444     * \ingroup features
00445     */
00446   template <typename PointInT, typename PointRFT>
00447   class FeatureWithLocalReferenceFrames
00448   {
00449     public:
00450       typedef pcl::PointCloud<PointRFT> PointCloudLRF;
00451       typedef typename PointCloudLRF::Ptr PointCloudLRFPtr;
00452       typedef typename PointCloudLRF::ConstPtr PointCloudLRFConstPtr;
00453 
00454       /** \brief Empty constructor. */
00455       FeatureWithLocalReferenceFrames () : frames_ (), frames_never_defined_ (true) {}
00456 
00457        /** \brief Empty destructor. */
00458       virtual ~FeatureWithLocalReferenceFrames () {}
00459 
00460       /** \brief Provide a pointer to the input dataset that contains the local
00461         * reference frames of the XYZ dataset.
00462         * In case of search surface is set to be different from the input cloud,
00463         * local reference frames should correspond to the input cloud, not the search surface!
00464         * \param[in] frames the const boost shared pointer to a PointCloud of reference frames.
00465         */
00466       inline void
00467       setInputReferenceFrames (const PointCloudLRFConstPtr &frames)
00468       {
00469         frames_ = frames;
00470         frames_never_defined_ = false;
00471       }
00472 
00473       /** \brief Get a pointer to the local reference frames. */
00474       inline PointCloudLRFConstPtr
00475       getInputReferenceFrames () const
00476       {
00477         return (frames_);
00478       }
00479 
00480     protected:
00481       /** \brief A boost shared pointer to the local reference frames. */
00482       PointCloudLRFConstPtr frames_;
00483       /** \brief The user has never set the frames. */
00484       bool frames_never_defined_;
00485 
00486       /** \brief Check if frames_ has been correctly initialized and compute it if needed.
00487         * \param input the subclass' input cloud dataset.
00488         * \param lrf_estimation a pointer to a local reference frame estimation class to be used as default.
00489         * \return true if frames_ has been correctly initialized.
00490         */
00491       typedef typename Feature<PointInT, PointRFT>::Ptr LRFEstimationPtr;
00492       virtual bool
00493       initLocalReferenceFrames (const size_t& indices_size,
00494                                 const LRFEstimationPtr& lrf_estimation = LRFEstimationPtr());
00495   };
00496 }
00497 
00498 #include <pcl/features/impl/feature.hpp>
00499 
00500 #endif  //#ifndef PCL_FEATURE_H_