Point Cloud Library (PCL)  1.15.1-dev
statistical_outlier_removal.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
41 #define PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
42 
43 #include <pcl/filters/statistical_outlier_removal.h>
44 #include <pcl/search/auto.h> // for autoSelectMethod
45 
46 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT> void
49 {
50  // Initialize the search class
51  if (!searcher_)
52  {
53  searcher_.reset (pcl::search::autoSelectMethod<PointT>(input_, false, pcl::search::Purpose::many_knn_search));
54  }
55  else if (!searcher_->setInputCloud (input_))
56  {
57  PCL_ERROR ("[pcl::%s::applyFilter] Error when initializing search method!\n", getClassName ().c_str ());
58  indices.clear ();
59  removed_indices_->clear ();
60  return;
61  }
62 
63  // The arrays to be used
64  const int searcher_k = mean_k_ + 1; // Find one more, since results include the query point.
65  Indices nn_indices (searcher_k);
66  std::vector<float> nn_dists (searcher_k);
67  std::vector<float> distances (indices_->size ());
68  indices.resize (indices_->size ());
69  removed_indices_->resize (indices_->size ());
70  int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
71 
72  // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
73  int valid_distances = 0;
74  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
75  {
76  if (!std::isfinite ((*input_)[(*indices_)[iii]].x) ||
77  !std::isfinite ((*input_)[(*indices_)[iii]].y) ||
78  !std::isfinite ((*input_)[(*indices_)[iii]].z))
79  {
80  distances[iii] = 0.0;
81  continue;
82  }
83 
84  // Perform the nearest k search
85  if (searcher_->nearestKSearch ((*indices_)[iii], searcher_k, nn_indices, nn_dists) == 0)
86  {
87  distances[iii] = 0.0;
88  PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
89  continue;
90  }
91 
92  // Calculate the mean distance to its neighbors.
93  double dist_sum = 0.0;
94  for (std::size_t k = 1; k < nn_dists.size(); ++k) // k = 0 is the query point
95  dist_sum += sqrt(nn_dists[k]);
96  distances[iii] = static_cast<float>(dist_sum / (nn_dists.size() - 1));
97  valid_distances++;
98  }
99 
100  // Estimate the mean and the standard deviation of the distance vector
101  double sum = 0, sq_sum = 0;
102  for (const float &distance : distances)
103  {
104  sum += distance;
105  sq_sum += distance * distance;
106  }
107  double mean = sum / static_cast<double>(valid_distances);
108  double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
109  double stddev = sqrt (variance);
110  //getMeanStd (distances, mean, stddev);
111 
112  double distance_threshold = mean + std_mul_ * stddev;
113 
114  // Second pass: Classify the points on the computed distance threshold
115  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
116  {
117  // Points having a too high average distance are outliers and are passed to removed indices
118  // Unless negative was set, then it's the opposite condition
119  if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
120  {
121  if (extract_removed_indices_)
122  (*removed_indices_)[rii++] = (*indices_)[iii];
123  continue;
124  }
125 
126  // Otherwise it was a normal point for output (inlier)
127  indices[oii++] = (*indices_)[iii];
128  }
129 
130  // Resize the output arrays
131  indices.resize (oii);
132  removed_indices_->resize (rii);
133 }
134 
135 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
136 
137 #endif // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
138 
void applyFilterIndices(Indices &indices)
Filtered results are indexed by an indices array.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
@ many_knn_search
The search method will mainly be used for nearestKSearch where k is larger than 1.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133