Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/filters/include/pcl/filters/convolution.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  *
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_H_
00041 #define PCL_FILTERS_CONVOLUTION_H_
00042 
00043 #include <pcl/common/eigen.h>
00044 #include <pcl/common/point_operators.h>
00045 #include <pcl/point_cloud.h>
00046 #include <pcl/exceptions.h>
00047 #include <pcl/pcl_base.h>
00048 
00049 namespace pcl
00050 {
00051   namespace filters
00052   {
00053     /** Convolution is a mathematical operation on two functions f and g,
00054       * producing a third function that is typically viewed as a modified
00055       * version of one of the original functions.
00056       * see http://en.wikipedia.org/wiki/Convolution.
00057       *
00058       * The class provides rows, column and separate convolving operations
00059       * of a point cloud.
00060       * Columns and separate convolution is only allowed on organised
00061       * point clouds.
00062       *
00063       * When convolving, computing the rows and cols elements at 1/2 kernel
00064       * width distance from the borders is not defined. We allow for 3
00065       * policies:
00066       * - Ignoring: elements at special locations are filled with zero
00067       * (default behaviour)
00068       * - Mirroring: the missing rows or columns are obtained throug mirroring
00069       * - Duplicating: the missing rows or columns are obtained throug
00070       * duplicating
00071       *
00072       * \author Nizar Sallem
00073       * \ingroup filters
00074       */
00075 
00076     template <typename PointIn, typename PointOut>
00077     class Convolution
00078     {
00079       public:
00080         typedef typename pcl::PointCloud<PointIn> PointCloudIn;
00081         typedef typename PointCloudIn::Ptr PointCloudInPtr;
00082         typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00083         typedef typename pcl::PointCloud<PointOut> PointCloudOut;
00084         typedef boost::shared_ptr< Convolution<PointIn, PointOut> > Ptr;
00085         typedef boost::shared_ptr< const Convolution<PointIn, PointOut> > ConstPtr;
00086 
00087 
00088         /// The borders policy available
00089         enum BORDERS_POLICY
00090         {
00091           BORDERS_POLICY_IGNORE = -1,
00092           BORDERS_POLICY_MIRROR = 0,
00093           BORDERS_POLICY_DUPLICATE = 1
00094         };
00095         /// Constructor
00096         Convolution ();
00097         /// Empty destructor
00098         ~Convolution () {}
00099         /** \brief Provide a pointer to the input dataset
00100           * \param cloud the const boost shared pointer to a PointCloud message
00101           * \remark Will perform a deep copy
00102           */
00103         inline void
00104         setInputCloud (const PointCloudInConstPtr& cloud) { input_ = cloud; }
00105         /** Set convolving kernel
00106           * \param[in] kernel convolving element
00107           */
00108         inline void
00109         setKernel (const Eigen::ArrayXf& kernel) { kernel_ = kernel; }
00110         /// Set the borders policy
00111         void
00112         setBordersPolicy (int policy) { borders_policy_ = policy; }
00113         /// Get the borders policy
00114         int
00115         getBordersPolicy () { return (borders_policy_); }
00116         /** \remark this is critical so please read it carefully.
00117           * In 3D the next point in (u,v) coordinate can be really far so a distance
00118           * threshold is used to keep us from ghost points.
00119           * The value you set here is strongly related to the sensor. A good value for
00120           * kinect data is 0.001 \default is std::numeric<float>::infinity ()
00121           * \param[in] threshold maximum allowed distance between 2 juxtaposed points
00122           */
00123         inline void
00124         setDistanceThreshold (const float& threshold) { distance_threshold_ = threshold; }
00125         /// \return the distance threshold
00126         inline const float &
00127         getDistanceThreshold () const { return (distance_threshold_); }
00128         /** \brief Initialize the scheduler and set the number of threads to use.
00129           * \param nr_threads the number of hardware threads to use (0 sets the value back to automatic)
00130           */
00131         inline void
00132         setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; }
00133         /** Convolve a float image rows by a given kernel.
00134           * \param[in] kernel convolution kernel
00135           * \param[out] output the convolved cloud
00136           * \note if output doesn't fit in input i.e. output.rows () < input.rows () or
00137           * output.cols () < input.cols () then output is resized to input sizes.
00138           */
00139         inline void
00140         convolveRows (PointCloudOut& output);
00141         /** Convolve a float image columns by a given kernel.
00142           * \param[in] kernel convolution kernel
00143           * \param[out] output the convolved image
00144           * \note if output doesn't fit in input i.e. output.rows () < input.rows () or
00145           * output.cols () < input.cols () then output is resized to input sizes.
00146           */
00147         inline void
00148         convolveCols (PointCloudOut& output);
00149         /** Convolve point cloud with an horizontal kernel along rows
00150           * then vertical kernel along columns : convolve separately.
00151           * \param[in] h_kernel kernel for convolving rows
00152           * \param[in] v_kernel kernel for convolving columns
00153           * \param[out] output the convolved cloud
00154           * \note if output doesn't fit in input i.e. output.rows () < input.rows () or
00155           * output.cols () < input.cols () then output is resized to input sizes.
00156           */
00157         inline void
00158         convolve (const Eigen::ArrayXf& h_kernel, const Eigen::ArrayXf& v_kernel, PointCloudOut& output);
00159         /** Convolve point cloud with same kernel along rows and columns separately.
00160           * \param[out] output the convolved cloud
00161           * \note if output doesn't fit in input i.e. output.rows () < input.rows () or
00162           * output.cols () < input.cols () then output is resized to input sizes.
00163           */
00164         inline void
00165         convolve (PointCloudOut& output);
00166 
00167       protected:
00168         /// \brief convolve rows and ignore borders
00169         void
00170         convolve_rows (PointCloudOut& output);
00171         /// \brief convolve cols and ignore borders
00172         void
00173         convolve_cols (PointCloudOut& output);
00174         /// \brief convolve rows and mirror borders
00175         void
00176         convolve_rows_mirror (PointCloudOut& output);
00177         /// \brief convolve cols and mirror borders
00178         void
00179         convolve_cols_mirror (PointCloudOut& output);
00180         /// \brief convolve rows and duplicate borders
00181         void
00182         convolve_rows_duplicate (PointCloudOut& output);
00183         /// \brief convolve cols and duplicate borders
00184         void
00185         convolve_cols_duplicate (PointCloudOut& output);
00186         /** init compute is an internal method called before computation
00187           * \param[in] kernel convolution kernel to be used
00188           * \throw pcl::InitFailedException
00189           */
00190         void
00191         initCompute (PointCloudOut& output);
00192       private:
00193         /** \return the result of convolution of point at (\ai, \aj)
00194           * \note no test on finity is performed
00195           */
00196         inline PointOut
00197         convolveOneRowDense (int i, int j);
00198         /** \return the result of convolution of point at (\ai, \aj)
00199           * \note no test on finity is performed
00200           */
00201         inline PointOut
00202         convolveOneColDense (int i, int j);
00203         /** \return the result of convolution of point at (\ai, \aj)
00204           * \note only finite points within \a distance_threshold_ are accounted
00205           */
00206         inline PointOut
00207         convolveOneRowNonDense (int i, int j);
00208         /** \return the result of convolution of point at (\ai, \aj)
00209           * \note only finite points within \a distance_threshold_ are accounted
00210           */
00211         inline PointOut
00212         convolveOneColNonDense (int i, int j);
00213 
00214         /// Border policy
00215         int borders_policy_;
00216         /// Threshold distance between adjacent points
00217         float distance_threshold_;
00218         /// Pointer to the input cloud
00219         PointCloudInConstPtr input_;
00220         /// convolution kernel
00221         Eigen::ArrayXf kernel_;
00222         /// half kernel size
00223         int half_width_;
00224         /// kernel size - 1
00225         int kernel_width_;
00226       protected:
00227         /** \brief The number of threads the scheduler should use. */
00228         unsigned int threads_;
00229 
00230         void
00231         makeInfinite (PointOut& p)
00232         {
00233           p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN ();
00234         }      
00235     };
00236   }
00237 }
00238 
00239 #include <pcl/filters/impl/convolution.hpp>
00240 
00241 #endif