Point Cloud Library (PCL)  1.7.0
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/common/io.h>
45 
46 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT> void
49 {
50  std::vector<int> indices;
51  if (keep_organized_)
52  {
53  bool temp = extract_removed_indices_;
54  extract_removed_indices_ = true;
55  applyFilterIndices (indices);
56  extract_removed_indices_ = temp;
57 
58  output = *input_;
59  for (int rii = 0; rii < static_cast<int> (removed_indices_->size ()); ++rii) // rii = removed indices iterator
60  output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_;
61  if (!pcl_isfinite (user_filter_value_))
62  output.is_dense = false;
63  }
64  else
65  {
66  applyFilterIndices (indices);
67  copyPointCloud (*input_, indices, output);
68  }
69 }
70 
71 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointT> void
74 {
75  // Initialize the search class
76  if (!searcher_)
77  {
78  if (input_->isOrganized ())
79  searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
80  else
81  searcher_.reset (new pcl::search::KdTree<PointT> (false));
82  }
83  searcher_->setInputCloud (input_);
84 
85  // The arrays to be used
86  std::vector<int> nn_indices (mean_k_);
87  std::vector<float> nn_dists (mean_k_);
88  std::vector<float> distances (indices_->size ());
89  indices.resize (indices_->size ());
90  removed_indices_->resize (indices_->size ());
91  int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
92 
93  // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
94  int valid_distances = 0;
95  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
96  {
97  if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) ||
98  !pcl_isfinite (input_->points[(*indices_)[iii]].y) ||
99  !pcl_isfinite (input_->points[(*indices_)[iii]].z))
100  {
101  distances[iii] = 0.0;
102  continue;
103  }
104 
105  // Perform the nearest k search
106  if (searcher_->nearestKSearch ((*indices_)[iii], mean_k_ + 1, nn_indices, nn_dists) == 0)
107  {
108  distances[iii] = 0.0;
109  PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
110  continue;
111  }
112 
113  // Calculate the mean distance to its neighbors
114  double dist_sum = 0.0;
115  for (int k = 1; k < mean_k_ + 1; ++k) // k = 0 is the query point
116  dist_sum += sqrt (nn_dists[k]);
117  distances[iii] = static_cast<float> (dist_sum / mean_k_);
118  valid_distances++;
119  }
120 
121  // Estimate the mean and the standard deviation of the distance vector
122  double sum = 0, sq_sum = 0;
123  for (size_t i = 0; i < distances.size (); ++i)
124  {
125  sum += distances[i];
126  sq_sum += distances[i] * distances[i];
127  }
128  double mean = sum / static_cast<double>(valid_distances);
129  double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
130  double stddev = sqrt (variance);
131  //getMeanStd (distances, mean, stddev);
132 
133  double distance_threshold = mean + std_mul_ * stddev;
134 
135  // Second pass: Classify the points on the computed distance threshold
136  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
137  {
138  // Points having a too high average distance are outliers and are passed to removed indices
139  // Unless negative was set, then it's the opposite condition
140  if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
141  {
142  if (extract_removed_indices_)
143  (*removed_indices_)[rii++] = (*indices_)[iii];
144  continue;
145  }
146 
147  // Otherwise it was a normal point for output (inlier)
148  indices[oii++] = (*indices_)[iii];
149  }
150 
151  // Resize the output arrays
152  indices.resize (oii);
153  removed_indices_->resize (rii);
154 }
155 
156 #define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
157 
158 #endif // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
159