gaussian.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: gaussian.h 5294 2012-03-25 18:10:50Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_GAUSSIAN_KERNEL
00041 #define PCL_GAUSSIAN_KERNEL
00042 
00043 #include <sstream>
00044 #include <pcl/common/eigen.h>
00045 #include <pcl/point_cloud.h>
00046 #include <boost/function.hpp>
00047 
00048 namespace pcl
00049 {
00057   class PCL_EXPORTS GaussianKernel
00058   {
00059     public:
00060 
00061       GaussianKernel () {}
00062 
00063       static const unsigned MAX_KERNEL_WIDTH = 71;
00071       void
00072       compute (float sigma, 
00073                Eigen::VectorXf &kernel,
00074                unsigned kernel_width = MAX_KERNEL_WIDTH) const;
00075 
00084       void
00085       compute (float sigma, 
00086                Eigen::VectorXf &kernel, 
00087                Eigen::VectorXf &derivative, 
00088                unsigned kernel_width = MAX_KERNEL_WIDTH) const;
00089 
00097       void
00098       convolveRows (const pcl::PointCloud<float> &input,
00099                     const Eigen::VectorXf &kernel,
00100                     pcl::PointCloud<float> &output) const;
00101 
00110      template <typename PointT> void
00111      convolveRows (const pcl::PointCloud<PointT> &input,
00112                    boost::function <float (const PointT& p)> field_accessor,
00113                    const Eigen::VectorXf &kernel,
00114                    pcl::PointCloud<float> &output) const;
00115 
00123       void
00124       convolveCols (const pcl::PointCloud<float> &input,
00125                     const Eigen::VectorXf &kernel,
00126                     pcl::PointCloud<float> &output) const;
00127 
00136       template <typename PointT> void
00137       convolveCols (const pcl::PointCloud<PointT> &input,
00138                     boost::function <float (const PointT& p)> field_accessor,
00139                     const Eigen::VectorXf &kernel,
00140                     pcl::PointCloud<float> &output) const;
00141 
00150       inline void
00151       convolve (const pcl::PointCloud<float> &input,
00152                 const Eigen::VectorXf &horiz_kernel,
00153                 const Eigen::VectorXf &vert_kernel,
00154                 pcl::PointCloud<float> &output) const
00155       {
00156         std::cout << ">>> convolve cpp" << std::endl;
00157         pcl::PointCloud<float> tmp (input.width, input.height) ;
00158         convolveRows (input, horiz_kernel, tmp);        
00159         convolveCols (tmp, vert_kernel, output);
00160         std::cout << "<<< convolve cpp" << std::endl;
00161       }
00162 
00172       template <typename PointT> inline void
00173       convolve (const pcl::PointCloud<PointT> &input,
00174                 boost::function <float (const PointT& p)> field_accessor,
00175                 const Eigen::VectorXf &horiz_kernel,
00176                 const Eigen::VectorXf &vert_kernel,
00177                 pcl::PointCloud<float> &output) const
00178       {
00179         std::cout << ">>> convolve hpp" << std::endl;
00180         pcl::PointCloud<float> tmp (input.width, input.height);
00181         convolveRows<PointT>(input, field_accessor, horiz_kernel, tmp);
00182         convolveCols(tmp, vert_kernel, output);
00183         std::cout << "<<< convolve hpp" << std::endl;
00184       }
00185       
00196       inline void
00197       computeGradients (const pcl::PointCloud<float> &input,
00198                         const Eigen::VectorXf &gaussian_kernel,
00199                         const Eigen::VectorXf &gaussian_kernel_derivative,
00200                         pcl::PointCloud<float> &grad_x,
00201                         pcl::PointCloud<float> &grad_y) const
00202       {
00203         convolve (input, gaussian_kernel_derivative, gaussian_kernel, grad_x);
00204         convolve (input, gaussian_kernel, gaussian_kernel_derivative, grad_y);
00205       }
00206 
00218       template <typename PointT> inline void
00219       computeGradients (const pcl::PointCloud<PointT> &input,
00220                         boost::function <float (const PointT& p)> field_accessor,
00221                         const Eigen::VectorXf &gaussian_kernel,
00222                         const Eigen::VectorXf &gaussian_kernel_derivative,
00223                         pcl::PointCloud<float> &grad_x,
00224                         pcl::PointCloud<float> &grad_y) const
00225       {
00226         convolve<PointT> (input, field_accessor, gaussian_kernel_derivative, gaussian_kernel, grad_x);
00227         convolve<PointT> (input, field_accessor, gaussian_kernel, gaussian_kernel_derivative, grad_y);
00228       }
00229       
00237       inline void
00238       smooth (const pcl::PointCloud<float> &input,
00239               const Eigen::VectorXf &gaussian_kernel,
00240               pcl::PointCloud<float> &output) const
00241       {
00242         convolve (input, gaussian_kernel, gaussian_kernel, output);
00243       }
00244 
00253       template <typename PointT> inline void
00254       smooth (const pcl::PointCloud<PointT> &input,
00255               boost::function <float (const PointT& p)> field_accessor,
00256               const Eigen::VectorXf &gaussian_kernel,
00257               pcl::PointCloud<float> &output) const
00258       {
00259         convolve<PointT> (input, field_accessor, gaussian_kernel, gaussian_kernel, output);
00260       }
00261   };
00262 }
00263 
00264 #include <pcl/common/impl/gaussian.hpp>
00265 
00266 #endif // PCL_GAUSSIAN_KERNEL


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:13