Point Cloud Library (PCL)
1.7.0
|
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_GAUSSIAN_KERNEL_IMPL 00041 #define PCL_GAUSSIAN_KERNEL_IMPL 00042 00043 #include <pcl/exceptions.h> 00044 00045 template <typename PointT> void 00046 pcl::GaussianKernel::convolveRows(const pcl::PointCloud<PointT> &input, 00047 boost::function <float (const PointT& p)> field_accessor, 00048 const Eigen::VectorXf& kernel, 00049 pcl::PointCloud<float> &output) const 00050 { 00051 assert(kernel.size () % 2 == 1); 00052 int kernel_width = kernel.size () -1; 00053 int radius = kernel.size () / 2.0; 00054 if(output.height < input.height || output.width < input.width) 00055 { 00056 output.width = input.width; 00057 output.height = input.height; 00058 output.points.resize (input.height * input.width); 00059 } 00060 00061 int i; 00062 for(int j = 0; j < input.height; j++) 00063 { 00064 for (i = 0 ; i < radius ; i++) 00065 output (i,j) = 0; 00066 00067 for ( ; i < input.width - radius ; i++) { 00068 output (i,j) = 0; 00069 for (int k = kernel_width, l = i - radius; k >= 0 ; k--, l++) 00070 output (i,j) += field_accessor (input (l,j)) * kernel[k]; 00071 } 00072 00073 for ( ; i < input.width ; i++) 00074 output (i,j) = 0; 00075 } 00076 } 00077 00078 template <typename PointT> void 00079 pcl::GaussianKernel::convolveCols(const pcl::PointCloud<PointT> &input, 00080 boost::function <float (const PointT& p)> field_accessor, 00081 const Eigen::VectorXf& kernel, 00082 pcl::PointCloud<float> &output) const 00083 { 00084 assert(kernel.size () % 2 == 1); 00085 int kernel_width = kernel.size () -1; 00086 int radius = kernel.size () / 2.0; 00087 if(output.height < input.height || output.width < input.width) 00088 { 00089 output.width = input.width; 00090 output.height = input.height; 00091 output.points.resize (input.height * input.width); 00092 } 00093 00094 int j; 00095 for(int i = 0; i < input.width; i++) 00096 { 00097 for (j = 0 ; j < radius ; j++) 00098 output (i,j) = 0; 00099 00100 for ( ; j < input.height - radius ; j++) { 00101 output (i,j) = 0; 00102 for (int k = kernel_width, l = j - radius ; k >= 0 ; k--, l++) 00103 { 00104 output (i,j) += field_accessor (input (i,l)) * kernel[k]; 00105 } 00106 } 00107 00108 for ( ; j < input.height ; j++) 00109 output (i,j) = 0; 00110 } 00111 } 00112 00113 #endif