00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #include <pcl/filters/impl/radius_outlier_removal.hpp> 00042 #include <pcl/conversions.h> 00043 00045 void 00046 pcl::RadiusOutlierRemoval<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output) 00047 { 00048 output.is_dense = true; 00049 // If fields x/y/z are not present, we cannot filter 00050 if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1) 00051 { 00052 PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ()); 00053 output.width = output.height = 0; 00054 output.data.clear (); 00055 return; 00056 } 00057 00058 if (search_radius_ == 0.0) 00059 { 00060 PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ()); 00061 output.width = output.height = 0; 00062 output.data.clear (); 00063 return; 00064 } 00065 // Send the input dataset to the spatial locator 00066 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); 00067 pcl::fromPCLPointCloud2 (*input_, *cloud); 00068 00069 // Initialize the spatial locator 00070 if (!tree_) 00071 { 00072 if (cloud->isOrganized ()) 00073 tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ()); 00074 else 00075 tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false)); 00076 } 00077 tree_->setInputCloud (cloud); 00078 00079 // Allocate enough space to hold the results 00080 std::vector<int> nn_indices (indices_->size ()); 00081 std::vector<float> nn_dists (indices_->size ()); 00082 00083 // Copy the common fields 00084 output.is_bigendian = input_->is_bigendian; 00085 output.point_step = input_->point_step; 00086 output.height = 1; 00087 00088 output.data.resize (input_->width * input_->point_step); // reserve enough space 00089 removed_indices_->resize (input_->data.size ()); 00090 00091 int nr_p = 0; 00092 int nr_removed_p = 0; 00093 // Go over all the points and check which doesn't have enough neighbors 00094 for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp) 00095 { 00096 int k = tree_->radiusSearch ((*indices_)[cp], search_radius_, nn_indices, nn_dists); 00097 // Check if the number of neighbors is larger than the user imposed limit 00098 if (k < min_pts_radius_) 00099 { 00100 if (extract_removed_indices_) 00101 { 00102 (*removed_indices_)[nr_removed_p] = cp; 00103 nr_removed_p++; 00104 } 00105 continue; 00106 } 00107 00108 memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step], 00109 output.point_step); 00110 nr_p++; 00111 } 00112 00113 output.width = nr_p; 00114 output.height = 1; 00115 output.data.resize (output.width * output.point_step); 00116 output.row_step = output.point_step * output.width; 00117 00118 removed_indices_->resize (nr_removed_p); 00119 } 00120 00121 #ifndef PCL_NO_PRECOMPILE 00122 #include <pcl/impl/instantiate.hpp> 00123 #include <pcl/point_types.h> 00124 00125 // Instantiations of specific point types 00126 PCL_INSTANTIATE(RadiusOutlierRemoval, PCL_XYZ_POINT_TYPES) 00127 00128 #endif // PCL_NO_PRECOMPILE 00129