Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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/organized.h> // for OrganizedNeighbor
45#include <pcl/search/kdtree.h> // for KdTree
46
47////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
48template <typename PointT> void
50{
51 // Initialize the search class
52 if (!searcher_)
53 {
54 if (input_->isOrganized ())
55 searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
56 else
57 searcher_.reset (new pcl::search::KdTree<PointT> (false));
58 }
59 if (!searcher_->setInputCloud (input_))
60 {
61 PCL_ERROR ("[pcl::%s::applyFilter] Error when initializing search method!\n", getClassName ().c_str ());
62 indices.clear ();
63 removed_indices_->clear ();
64 return;
65 }
66
67 // The arrays to be used
68 const int searcher_k = mean_k_ + 1; // Find one more, since results include the query point.
69 Indices nn_indices (searcher_k);
70 std::vector<float> nn_dists (searcher_k);
71 std::vector<float> distances (indices_->size ());
72 indices.resize (indices_->size ());
73 removed_indices_->resize (indices_->size ());
74 int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
75
76 // First pass: Compute the mean distances for all points with respect to their k nearest neighbors
77 int valid_distances = 0;
78 for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
79 {
80 if (!std::isfinite ((*input_)[(*indices_)[iii]].x) ||
81 !std::isfinite ((*input_)[(*indices_)[iii]].y) ||
82 !std::isfinite ((*input_)[(*indices_)[iii]].z))
83 {
84 distances[iii] = 0.0;
85 continue;
86 }
87
88 // Perform the nearest k search
89 if (searcher_->nearestKSearch ((*indices_)[iii], searcher_k, nn_indices, nn_dists) == 0)
90 {
91 distances[iii] = 0.0;
92 PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
93 continue;
94 }
95
96 // Calculate the mean distance to its neighbors
97 double dist_sum = 0.0;
98 for (int k = 1; k < searcher_k; ++k) // k = 0 is the query point
99 dist_sum += sqrt (nn_dists[k]);
100 distances[iii] = static_cast<float> (dist_sum / mean_k_);
101 valid_distances++;
102 }
103
104 // Estimate the mean and the standard deviation of the distance vector
105 double sum = 0, sq_sum = 0;
106 for (const float &distance : distances)
107 {
108 sum += distance;
109 sq_sum += distance * distance;
110 }
111 double mean = sum / static_cast<double>(valid_distances);
112 double variance = (sq_sum - sum * sum / static_cast<double>(valid_distances)) / (static_cast<double>(valid_distances) - 1);
113 double stddev = sqrt (variance);
114 //getMeanStd (distances, mean, stddev);
115
116 double distance_threshold = mean + std_mul_ * stddev;
117
118 // Second pass: Classify the points on the computed distance threshold
119 for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
120 {
121 // Points having a too high average distance are outliers and are passed to removed indices
122 // Unless negative was set, then it's the opposite condition
123 if ((!negative_ && distances[iii] > distance_threshold) || (negative_ && distances[iii] <= distance_threshold))
124 {
125 if (extract_removed_indices_)
126 (*removed_indices_)[rii++] = (*indices_)[iii];
127 continue;
128 }
129
130 // Otherwise it was a normal point for output (inlier)
131 indices[oii++] = (*indices_)[iii];
132 }
133
134 // Resize the output arrays
135 indices.resize (oii);
136 removed_indices_->resize (rii);
137}
138
139#define PCL_INSTANTIATE_StatisticalOutlierRemoval(T) template class PCL_EXPORTS pcl::StatisticalOutlierRemoval<T>;
140
141#endif // PCL_FILTERS_IMPL_STATISTICAL_OUTLIER_REMOVAL_H_
142
void applyFilterIndices(Indices &indices)
Filtered results are indexed by an indices array.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
Definition organized.h:65
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133