gaussian.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, wwww.pointclouds.org
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: gaussian.cpp 4874 2012-03-01 07:22:00Z rusu $
00035  *
00036  */
00037 
00038 #include <pcl/common/gaussian.h>
00039 
00040 void 
00041 pcl::GaussianKernel::compute (float sigma, 
00042                               Eigen::VectorXf &kernel, 
00043                               unsigned kernel_width) const
00044 {
00045   assert (kernel_width %2 == 1);
00046   assert (sigma >= 0);
00047   kernel.resize (kernel_width);
00048   static const float factor = 0.01f;
00049   static const float max_gauss = 1.0f;
00050   const int hw = kernel_width / 2;
00051   float sigma_sqr = 1.0f / (2.0f * sigma * sigma);
00052   for (int i = -hw, j = 0, k = kernel_width - 1; i < 0 ; i++, j++, k--)
00053     kernel[k] = kernel[j] = expf (-static_cast<float>(i) * static_cast<float>(i) * sigma_sqr);
00054   kernel[hw] = 1;
00055   unsigned g_width = kernel_width;
00056   for (unsigned i = 0; fabs (kernel[i]/max_gauss) < factor; i++, g_width-= 2);
00057   if (g_width == kernel_width)
00058   { 
00059     PCL_THROW_EXCEPTION (pcl::KernelWidthTooSmallException,
00060                         "kernel width " << kernel_width 
00061                         << "is too small for the given sigma " << sigma);
00062     return;
00063   }
00064 
00065   // Shift and resize if width less than kernel_width
00066   unsigned shift = (kernel_width - g_width)/2;
00067   for (unsigned i =0; i < g_width; i++)
00068     kernel[i] = kernel[i + shift];
00069   kernel.conservativeResize (g_width);
00070 
00071   // Normalize
00072   kernel/= kernel.sum ();
00073 }
00074 
00075 void 
00076 pcl::GaussianKernel::compute (float sigma, 
00077                               Eigen::VectorXf &kernel, 
00078                               Eigen::VectorXf &derivative,
00079                               unsigned kernel_width) const
00080 {
00081   assert (kernel_width %2 == 1);
00082   assert (sigma >= 0);
00083   kernel.resize (kernel_width);
00084   derivative.resize (kernel_width);
00085   const float factor = 0.01f;
00086   float max_gauss = 1.0f, max_deriv = float (sigma * exp (-0.5));
00087   int hw = kernel_width / 2;
00088 
00089   float sigma_sqr = 1.0f / (2.0f * sigma * sigma);
00090   for (int i = -hw, j = 0, k = kernel_width - 1; i < 0 ; i++, j++, k--)
00091   {
00092     kernel[k] = kernel[j] = expf (-static_cast<float>(i) * static_cast<float>(i) * sigma_sqr);
00093     derivative[j] = -static_cast<float>(i) * kernel[j];
00094     derivative[k] = -derivative[j];
00095   }
00096   kernel[hw] = 1;
00097   derivative[hw] = 0;
00098   // Compute kernel and derivative true width
00099   unsigned g_width = kernel_width;
00100   unsigned d_width = kernel_width;
00101   for (unsigned i = 0; fabs (derivative[i]/max_deriv) < factor; i++, d_width-= 2);
00102   for (unsigned i = 0; fabs (kernel[i]/max_gauss) < factor; i++, g_width-= 2);
00103   if (g_width == kernel_width || d_width == kernel_width)
00104   { 
00105     PCL_THROW_EXCEPTION (KernelWidthTooSmallException,
00106                         "kernel width " << kernel_width 
00107                         << "is too small for the given sigma " << sigma);
00108     return;
00109   }
00110 
00111   // Shift and resize if width less than kernel_width
00112   // Kernel
00113   unsigned shift = (kernel_width - g_width)/2;
00114   for (unsigned i =0; i < g_width; i++)
00115     kernel[i] = kernel[i + shift];
00116   // Normalize kernel
00117   kernel.conservativeResize (g_width);
00118   kernel/= kernel.sum ();
00119 
00120   // Derivative
00121   shift = (kernel_width - d_width)/2;
00122   for (unsigned i =0; i < d_width; i++)
00123     derivative[i] = derivative[i + shift];
00124   derivative.conservativeResize (d_width);
00125   // Normalize derivative
00126   hw = d_width / 2;
00127   float den = 0;
00128   for (int i = -hw ; i <= hw ; i++)
00129     den -=  static_cast<float>(i) * derivative[i+hw];
00130   derivative/= den;
00131 }
00132 
00133 void 
00134 pcl::GaussianKernel::convolveRows (const pcl::PointCloud<float>& input,
00135                                    const Eigen::VectorXf& kernel,
00136                                    pcl::PointCloud<float>& output) const
00137 {
00138   assert (kernel.size () % 2 == 1);
00139   size_t kernel_width = kernel.size () -1;
00140   size_t radius = kernel.size () / 2;
00141   const pcl::PointCloud<float>* input_;
00142   if (&input != &output)
00143   {
00144     if (output.height < input.height || output.width < input.width)
00145     {
00146       output.width = input.width;
00147       output.height = input.height;
00148       output.points.resize (input.height * input.width);
00149     }
00150     input_ = &input;
00151   }
00152   else
00153     input_ = new pcl::PointCloud<float>(input);
00154   
00155   size_t i;
00156   for (size_t j = 0; j < input_->height; j++)
00157   {
00158     for (i = 0 ; i < radius ; i++)
00159       output (i,j) = 0;
00160 
00161     for ( ; i < input_->width - radius ; i++)  
00162     {
00163       output (i,j) = 0;
00164       for (int k = static_cast<int>(kernel_width), l = static_cast<int>(i - radius); k >= 0 ; k--, l++)
00165         output (i,j) += (*input_) (l,j) * kernel[k];
00166     }
00167 
00168     for ( ; i < input_->width ; i++)
00169       output (i,j) = 0;
00170   }
00171 
00172   if (&input == &output)
00173   {
00174     delete input_;
00175   }
00176 }
00177 
00178 void 
00179 pcl::GaussianKernel::convolveCols (const pcl::PointCloud<float>& input,
00180                                    const Eigen::VectorXf& kernel,
00181                                    pcl::PointCloud<float>& output) const
00182 {
00183   assert (kernel.size () % 2 == 1);
00184   size_t kernel_width = kernel.size () -1;
00185   size_t radius = kernel.size () / 2;
00186   const pcl::PointCloud<float>* input_;
00187   if (&input != &output)
00188   {
00189     if (output.height < input.height || output.width < input.width)
00190     {
00191       output.width = input.width;
00192       output.height = input.height;
00193       output.resize (input.width * input.height);
00194     }
00195     input_ = &input;
00196   }
00197   else
00198     input_ = new pcl::PointCloud<float> (input);
00199 
00200   size_t j;
00201   for (size_t i = 0; i < input_->width; i++)
00202   {
00203     for (j = 0 ; j < radius ; j++)
00204       output (i,j) = 0;
00205 
00206     for ( ; j < input_->height - radius ; j++)  {
00207       output (i,j) = 0;
00208       for (int k = static_cast<int>(kernel_width), l = static_cast<int>(j - radius) ; k >= 0 ; k--, l++)
00209       {
00210         output (i,j) += (*input_) (i,l) * kernel[k];
00211       }
00212     }
00213 
00214     for ( ; j < input_->height ; j++)
00215       output (i,j) = 0;
00216   }
00217   if (&input == &output)
00218     delete input_;
00219 }


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