Point Cloud Library (PCL) 1.12.0
radius_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_RADIUS_OUTLIER_REMOVAL_H_
41#define PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_
42
43#include <pcl/filters/radius_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 if (search_radius_ == 0.0)
52 {
53 PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
54 indices.clear ();
55 removed_indices_->clear ();
56 return;
57 }
58
59 // Initialize the search class
60 if (!searcher_)
61 {
62 if (input_->isOrganized ())
63 searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
64 else
65 searcher_.reset (new pcl::search::KdTree<PointT> (false));
66 }
67 searcher_->setInputCloud (input_);
68
69 // The arrays to be used
70 Indices nn_indices (indices_->size ());
71 std::vector<float> nn_dists (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 // If the data is dense => use nearest-k search
77 if (input_->is_dense)
78 {
79 // Note: k includes the query point, so is always at least 1
80 int mean_k = min_pts_radius_ + 1;
81 double nn_dists_max = search_radius_ * search_radius_;
82
83 for (const auto& index : (*indices_))
84 {
85 // Perform the nearest-k search
86 int k = searcher_->nearestKSearch (index, mean_k, nn_indices, nn_dists);
87
88 // Check the number of neighbors
89 // Note: nn_dists is sorted, so check the last item
90 bool chk_neighbors = true;
91 if (k == mean_k)
92 {
93 if (negative_)
94 {
95 chk_neighbors = false;
96 if (nn_dists_max < nn_dists[k-1])
97 {
98 chk_neighbors = true;
99 }
100 }
101 else
102 {
103 chk_neighbors = true;
104 if (nn_dists_max < nn_dists[k-1])
105 {
106 chk_neighbors = false;
107 }
108 }
109 }
110 else
111 {
112 if (negative_)
113 chk_neighbors = true;
114 else
115 chk_neighbors = false;
116 }
117
118 // Points having too few neighbors are outliers and are passed to removed indices
119 // Unless negative was set, then it's the opposite condition
120 if (!chk_neighbors)
121 {
122 if (extract_removed_indices_)
123 (*removed_indices_)[rii++] = index;
124 continue;
125 }
126
127 // Otherwise it was a normal point for output (inlier)
128 indices[oii++] = index;
129 }
130 }
131 // NaN or Inf values could exist => use radius search
132 else
133 {
134 for (const auto& index : (*indices_))
135 {
136 // Perform the radius search
137 // Note: k includes the query point, so is always at least 1
138 int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists);
139
140 // Points having too few neighbors are outliers and are passed to removed indices
141 // Unless negative was set, then it's the opposite condition
142 if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
143 {
144 if (extract_removed_indices_)
145 (*removed_indices_)[rii++] = index;
146 continue;
147 }
148
149 // Otherwise it was a normal point for output (inlier)
150 indices[oii++] = index;
151 }
152 }
153
154 // Resize the output arrays
155 indices.resize (oii);
156 removed_indices_->resize (rii);
157}
158
159#define PCL_INSTANTIATE_RadiusOutlierRemoval(T) template class PCL_EXPORTS pcl::RadiusOutlierRemoval<T>;
160
161#endif // PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_
162
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 neigbhor search in organized point clouds.
Definition: organized.h:61
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133