Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/filters/include/pcl/filters/convolution_3d.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, 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 the copyright holder(s) 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$
00037  *
00038  */
00039 
00040 #ifndef PCL_FILTERS_CONVOLUTION_3D_H
00041 #define PCL_FILTERS_CONVOLUTION_3D_H
00042 
00043 #include <pcl/pcl_base.h>
00044 #include <pcl/filters/boost.h>
00045 #include <pcl/search/pcl_search.h>
00046 
00047 namespace pcl
00048 {
00049   namespace filters
00050   {
00051     /** \brief Class ConvolvingKernel base class for all convolving kernels
00052       * \ingroup filters
00053       */
00054     template<typename PointInT, typename PointOutT>
00055     class ConvolvingKernel
00056     {
00057       public:
00058         typedef boost::shared_ptr<ConvolvingKernel<PointInT, PointOutT> > Ptr;
00059         typedef boost::shared_ptr<const ConvolvingKernel<PointInT, PointOutT> > ConstPtr;
00060  
00061         typedef typename PointCloud<PointInT>::ConstPtr PointCloudInConstPtr;
00062 
00063         /// \brief empty constructor
00064         ConvolvingKernel () {}
00065 
00066         /// \brief empty destructor
00067         virtual ~ConvolvingKernel () {}
00068 
00069         /** \brief Set input cloud
00070           * \param[in] input source point cloud
00071           */
00072         void
00073         setInputCloud (const PointCloudInConstPtr& input) { input_ = input; }
00074 
00075         /** \brief Convolve point at the center of this local information
00076           * \param[in] indices indices of the point in the source point cloud
00077           * \param[in] distances euclidean distance squared from the query point
00078           * \return the convolved point
00079           */
00080         virtual PointOutT
00081         operator() (const std::vector<int>& indices, const std::vector<float>& distances) = 0;
00082 
00083         /** \brief Must call this methode before doing any computation
00084           * \note make sure to override this with at least
00085           * \code
00086           * bool initCompute ()
00087           * {
00088           *   return (true);
00089           * }
00090           * \endcode
00091           * in your kernel interface, else you are going nowhere!
00092           */
00093         virtual bool
00094         initCompute () { return false; }
00095 
00096         /** \brief Utility function that annihilates a point making it fail the \ref pcl::isFinite test
00097           * \param[in/out] p point to annihilate
00098           */
00099         static void
00100         makeInfinite (PointOutT& p)
00101         {
00102           p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN ();
00103         }
00104 
00105       protected:
00106         /// source cloud
00107         PointCloudInConstPtr input_;
00108     };
00109 
00110     /** \brief Gaussian kernel implementation interface
00111       * Use this as implementation reference
00112       * \ingroup filters
00113       */
00114     template<typename PointInT, typename PointOutT>
00115     class GaussianKernel : public ConvolvingKernel <PointInT, PointOutT>
00116     {
00117       public:
00118         using ConvolvingKernel<PointInT, PointOutT>::initCompute;
00119         using ConvolvingKernel<PointInT, PointOutT>::input_;
00120         using ConvolvingKernel<PointInT, PointOutT>::operator ();
00121         using ConvolvingKernel<PointInT, PointOutT>::makeInfinite;
00122         typedef boost::shared_ptr<GaussianKernel<PointInT, PointOutT> > Ptr;
00123         typedef boost::shared_ptr<GaussianKernel<PointInT, PointOutT> > ConstPtr;
00124 
00125         /** Default constructor */
00126         GaussianKernel ()
00127           : ConvolvingKernel <PointInT, PointOutT> ()
00128           , sigma_ (0)
00129           , threshold_ (std::numeric_limits<float>::infinity ())
00130         {}
00131 
00132         virtual ~GaussianKernel () {}
00133 
00134         /** Set the sigma parameter of the Gaussian
00135           * \param[in] sigma
00136           */
00137         inline void
00138         setSigma (float sigma) { sigma_ = sigma; }
00139 
00140         /** Set the distance threshold relative to a sigma factor i.e. points such as
00141           * ||pi - q|| > sigma_coefficient^2 * sigma^2 are not considered.
00142           */
00143         inline void
00144         setThresholdRelativeToSigma (float sigma_coefficient)
00145         {
00146           sigma_coefficient_.reset (sigma_coefficient);
00147         }
00148 
00149         /** Set the distance threshold such as pi, ||pi - q|| > threshold are not considered. */
00150         inline void
00151         setThreshold (float threshold) { threshold_ = threshold; }
00152 
00153         /** Must call this methode before doing any computation */
00154         bool initCompute ();
00155 
00156         virtual PointOutT
00157         operator() (const std::vector<int>& indices, const std::vector<float>& distances);
00158 
00159       protected:
00160         float sigma_;
00161         float sigma_sqr_;
00162         float threshold_;
00163         boost::optional<float> sigma_coefficient_;
00164     };
00165 
00166     /** \brief Gaussian kernel implementation interface with RGB channel handling
00167       * Use this as implementation reference
00168       * \ingroup filters
00169       */
00170     template<typename PointInT, typename PointOutT>
00171     class GaussianKernelRGB : public GaussianKernel <PointInT, PointOutT>
00172     {
00173       public:
00174         using GaussianKernel<PointInT, PointOutT>::initCompute;
00175         using GaussianKernel<PointInT, PointOutT>::input_;
00176         using GaussianKernel<PointInT, PointOutT>::operator ();
00177         using GaussianKernel<PointInT, PointOutT>::makeInfinite;
00178         using GaussianKernel<PointInT, PointOutT>::sigma_sqr_;
00179         using GaussianKernel<PointInT, PointOutT>::threshold_;
00180         typedef boost::shared_ptr<GaussianKernelRGB<PointInT, PointOutT> > Ptr;
00181         typedef boost::shared_ptr<GaussianKernelRGB<PointInT, PointOutT> > ConstPtr;
00182 
00183         /** Default constructor */
00184         GaussianKernelRGB ()
00185           : GaussianKernel <PointInT, PointOutT> ()
00186         {}
00187 
00188         ~GaussianKernelRGB () {}
00189 
00190         PointOutT
00191         operator() (const std::vector<int>& indices, const std::vector<float>& distances);
00192     };
00193 
00194     /** Convolution3D handles the non organized case where width and height are unknown or if you
00195       * are only interested in convolving based on local neighborhood information.
00196       * The convolving kernel MUST be a radial symmetric and implement \ref ConvolvingKernel
00197       * interface.
00198       */
00199     template <typename PointIn, typename PointOut, typename KernelT>
00200     class Convolution3D : public pcl::PCLBase <PointIn>
00201     {
00202       public:
00203         typedef typename pcl::PointCloud<PointIn> PointCloudIn;
00204         typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00205         typedef typename pcl::search::Search<PointIn> KdTree;
00206         typedef typename pcl::search::Search<PointIn>::Ptr KdTreePtr;
00207         typedef typename pcl::PointCloud<PointOut> PointCloudOut;
00208         typedef boost::shared_ptr<Convolution3D<PointIn, PointOut, KernelT> > Ptr;
00209         typedef boost::shared_ptr<Convolution3D<PointIn, PointOut, KernelT> > ConstPtr;
00210 
00211         using pcl::PCLBase<PointIn>::indices_;
00212         using pcl::PCLBase<PointIn>::input_;
00213 
00214         /** \brief Constructor */
00215         Convolution3D ();
00216 
00217         /** \brief Empty destructor */
00218         ~Convolution3D () {}
00219 
00220         /** \brief Initialize the scheduler and set the number of threads to use.
00221           * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic)
00222           */
00223         inline void
00224         setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; }
00225 
00226         /** \brief Set convolving kernel
00227           * \param[in] kernel convolving element
00228           */
00229         inline void
00230         setKernel (const KernelT& kernel) { kernel_ = kernel; }
00231 
00232         /** \brief Provide a pointer to the input dataset that we need to estimate features at every point for.
00233           * \param cloud the const boost shared pointer to a PointCloud message
00234           */
00235         inline void
00236         setSearchSurface (const PointCloudInConstPtr &cloud) { surface_ = cloud; }
00237 
00238         /** \brief Get a pointer to the surface point cloud dataset. */
00239         inline PointCloudInConstPtr
00240         getSearchSurface () { return (surface_); }
00241 
00242         /** \brief Provide a pointer to the search object.
00243           * \param tree a pointer to the spatial search object.
00244           */
00245         inline void
00246         setSearchMethod (const KdTreePtr &tree) { tree_ = tree; }
00247 
00248         /** \brief Get a pointer to the search method used. */
00249         inline KdTreePtr
00250         getSearchMethod () { return (tree_); }
00251 
00252         /** \brief Set the sphere radius that is to be used for determining the nearest neighbors
00253           * \param radius the sphere radius used as the maximum distance to consider a point a neighbor
00254           */
00255         inline void
00256         setRadiusSearch (double radius) { search_radius_ = radius; }
00257 
00258         /** \brief Get the sphere radius used for determining the neighbors. */
00259         inline double
00260         getRadiusSearch () { return (search_radius_); }
00261 
00262         /** Convolve point cloud.
00263           * \param[out] output the convolved cloud
00264           */
00265         void
00266         convolve (PointCloudOut& output);
00267 
00268       protected:
00269         /** \brief initialize computation */
00270         bool initCompute ();
00271 
00272         /** \brief An input point cloud describing the surface that is to be used for nearest neighbors estimation. */
00273         PointCloudInConstPtr surface_;
00274 
00275         /** \brief A pointer to the spatial search object. */
00276         KdTreePtr tree_;
00277 
00278         /** \brief The nearest neighbors search radius for each point. */
00279         double search_radius_;
00280 
00281         /** \brief number of threads */
00282         unsigned int threads_;
00283 
00284         /** \brief convlving kernel */
00285         KernelT kernel_;
00286     };
00287   }
00288 }
00289 
00290 #include <pcl/filters/impl/convolution_3d.hpp>
00291 
00292 #endif // PCL_FILTERS_CONVOLUTION_3D_H