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: statistical_outlier_removal.hpp 3028 2011-11-01 04:12:17Z rusu $ 00035 * 00036 */ 00037 00038 #ifndef PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_ 00039 #define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_ 00040 00041 #include "pcl/filters/statistical_outlier_removal.h" 00042 00043 00045 template <typename PointT> void 00046 pcl::StatisticalOutlierRemoval<PointT>::applyFilter (PointCloud &output) 00047 { 00048 if (std_mul_ == 0.0) 00049 { 00050 PCL_ERROR ("[pcl::%s::applyFilter] Standard deviation multiplier not set!\n", getClassName ().c_str ()); 00051 output.width = output.height = 0; 00052 output.points.clear (); 00053 return; 00054 } 00055 00056 if (input_->points.empty ()) 00057 { 00058 output.width = output.height = 0; 00059 output.points.clear (); 00060 return; 00061 } 00062 00063 // Initialize the spatial locator 00064 if (!tree_) 00065 { 00066 if (input_->isOrganized ()) 00067 tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); 00068 else 00069 tree_.reset (new pcl::search::KdTree<PointT> (false)); 00070 } 00071 00072 // Send the input dataset to the spatial locator 00073 tree_->setInputCloud (input_); 00074 00075 // Allocate enough space to hold the results 00076 std::vector<int> nn_indices (mean_k_); 00077 std::vector<float> nn_dists (mean_k_); 00078 00079 std::vector<float> distances (indices_->size ()); 00080 // Go over all the points and calculate the mean or smallest distance 00081 for (size_t cp = 0; cp < indices_->size (); ++cp) 00082 { 00083 if (!pcl_isfinite (input_->points[(*indices_)[cp]].x) || 00084 !pcl_isfinite (input_->points[(*indices_)[cp]].y) || 00085 !pcl_isfinite (input_->points[(*indices_)[cp]].z)) 00086 { 00087 distances[cp] = 0; 00088 continue; 00089 } 00090 00091 if (tree_->nearestKSearch ((*indices_)[cp], mean_k_, nn_indices, nn_dists) == 0) 00092 { 00093 distances[cp] = 0; 00094 PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_); 00095 continue; 00096 } 00097 00098 // Minimum distance (if mean_k_ == 2) or mean distance 00099 double dist_sum = 0; 00100 for (int j = 1; j < mean_k_; ++j) 00101 dist_sum += sqrt (nn_dists[j]); 00102 distances[cp] = dist_sum / (mean_k_-1); 00103 } 00104 00105 // Estimate the mean and the standard deviation of the distance vector 00106 double mean, stddev; 00107 getMeanStd (distances, mean, stddev); 00108 double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier 00109 00110 output.points.resize (input_->points.size ()); // reserve enough space 00111 removed_indices_->resize (input_->points.size ()); 00112 00113 // Build a new cloud by neglecting outliers 00114 int nr_p = 0; 00115 int nr_removed_p = 0; 00116 00117 for (size_t cp = 0; cp < indices_->size (); ++cp) 00118 { 00119 if (negative_) 00120 { 00121 if (distances[cp] <= distance_threshold) 00122 { 00123 if (extract_removed_indices_) 00124 { 00125 (*removed_indices_)[nr_removed_p] = cp; 00126 nr_removed_p++; 00127 } 00128 continue; 00129 } 00130 } 00131 else 00132 { 00133 if (distances[cp] > distance_threshold) 00134 { 00135 if (extract_removed_indices_) 00136 { 00137 (*removed_indices_)[nr_removed_p] = cp; 00138 nr_removed_p++; 00139 } 00140 continue; 00141 } 00142 } 00143 00144 output.points[nr_p++] = input_->points[(*indices_)[cp]]; 00145 } 00146 00147 output.points.resize (nr_p); 00148 output.width = nr_p; 00149 output.height = 1; 00150 output.is_dense = true; // nearestKSearch filters invalid points 00151 00152 removed_indices_->resize (nr_removed_p); 00153 } 00154 00155 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>; 00156 00157 #endif // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_ 00158