bilateral.hpp
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 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_BILATERAL_IMPL_H_
00041 #define PCL_FILTERS_BILATERAL_IMPL_H_
00042 
00043 #include <pcl/filters/bilateral.h>
00044 
00046 template <typename PointT> double
00047 pcl::BilateralFilter<PointT>::computePointWeight (const int pid, 
00048                                                   const std::vector<int> &indices,
00049                                                   const std::vector<float> &distances)
00050 {
00051   double BF = 0, W = 0;
00052 
00053   // For each neighbor
00054   for (size_t n_id = 0; n_id < indices.size (); ++n_id)
00055   {
00056     int id = indices[n_id];
00057     // Compute the difference in intensity
00058     double intensity_dist = fabs (input_->points[pid].intensity - input_->points[id].intensity);
00059 
00060     // Compute the Gaussian intensity weights both in Euclidean and in intensity space
00061     double dist = std::sqrt (distances[n_id]);
00062     double weight = kernel (dist, sigma_s_) * kernel (intensity_dist, sigma_r_);
00063 
00064     // Calculate the bilateral filter response
00065     BF += weight * input_->points[id].intensity;
00066     W += weight;
00067   }
00068   return (BF / W);
00069 }
00070 
00072 template <typename PointT> void
00073 pcl::BilateralFilter<PointT>::applyFilter (PointCloud &output)
00074 {
00075   // Check if sigma_s has been given by the user
00076   if (sigma_s_ == 0)
00077   {
00078     PCL_ERROR ("[pcl::BilateralFilter::applyFilter] Need a sigma_s value given before continuing.\n");
00079     return;
00080   }
00081   // In case a search method has not been given, initialize it using some defaults
00082   if (!tree_)
00083   {
00084     // For organized datasets, use an OrganizedDataIndex
00085     if (input_->isOrganized ())
00086       tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
00087     // For unorganized data, use a FLANN kdtree
00088     else
00089       tree_.reset (new pcl::search::KdTree<PointT> (false));
00090   }
00091   tree_->setInputCloud (input_);
00092 
00093   std::vector<int> k_indices;
00094   std::vector<float> k_distances;
00095 
00096   // Copy the input data into the output
00097   output = *input_;
00098 
00099   // For all the indices given (equal to the entire cloud if none given)
00100   for (size_t i = 0; i < indices_->size (); ++i)
00101   {
00102     // Perform a radius search to find the nearest neighbors
00103     tree_->radiusSearch ((*indices_)[i], sigma_s_ * 2, k_indices, k_distances);
00104 
00105     // Overwrite the intensity value with the computed average
00106     output.points[(*indices_)[i]].intensity = static_cast<float> (computePointWeight ((*indices_)[i], k_indices, k_distances));
00107   }
00108 }
00109  
00110 #define PCL_INSTANTIATE_BilateralFilter(T) template class PCL_EXPORTS pcl::BilateralFilter<T>;
00111 
00112 #endif // PCL_FILTERS_BILATERAL_H_
00113 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:22:36