Point Cloud Library (PCL)
1.3.1
|
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