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