Point Cloud Library (PCL)  1.3.1
radius_outlier_removal.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
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: radius_outlier_removal.hpp 3028 2011-11-01 04:12:17Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_
00039 #define PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_
00040 
00041 #include "pcl/filters/radius_outlier_removal.h"
00042 
00044 template <typename PointT> void
00045 pcl::RadiusOutlierRemoval<PointT>::applyFilter (PointCloud &output)
00046 {
00047   if (search_radius_ == 0.0)
00048   {
00049     PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
00050     output.width = output.height = 0;
00051     output.points.clear ();
00052     return;
00053   }
00054   // Initialize the spatial locator
00055   if (!tree_)
00056   {
00057     if (input_->isOrganized ())
00058       tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
00059     else
00060       tree_.reset (new pcl::search::KdTree<PointT> (false));
00061   }
00062 
00063   // Send the input dataset to the spatial locator
00064   tree_->setInputCloud (input_);
00065 
00066   // Allocate enough space to hold the results
00067   std::vector<int> nn_indices (indices_->size ());
00068   std::vector<float> nn_dists (indices_->size ());
00069 
00070 
00071   output.points.resize (input_->points.size ());      // reserve enough space
00072   removed_indices_->resize (input_->points.size ());
00073   
00074   int nr_p = 0;
00075   int nr_removed_p = 0;
00076   
00077   // Go over all the points and check which doesn't have enough neighbors
00078   for (size_t cp = 0; cp < indices_->size (); ++cp)
00079   {
00080     int k = tree_->radiusSearch ((*indices_)[cp], search_radius_, nn_indices, nn_dists);
00081     // Check if the number of neighbors is larger than the user imposed limit
00082     if (k < min_pts_radius_)
00083     {
00084       if (extract_removed_indices_)
00085       {
00086         (*removed_indices_)[nr_removed_p] = cp;
00087         nr_removed_p++;
00088       }
00089       continue;
00090     }
00091 
00092     output.points[nr_p++] = input_->points[(*indices_)[cp]];
00093   }
00094   removed_indices_->resize (nr_removed_p);
00095   output.points.resize (nr_p);
00096   output.width  = nr_p;
00097   output.height = 1;
00098   output.is_dense = true; // radiusSearch filters invalid points
00099 }
00100 
00101 #define PCL_INSTANTIATE_RadiusOutlierRemoval(T) template class PCL_EXPORTS pcl::RadiusOutlierRemoval<T>;
00102 
00103 #endif    // PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_
00104 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines