Point Cloud Library (PCL) 1.15.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 // Note: k includes the query point, so is always at least 1
76 const int mean_k = min_pts_radius_ + 1;
77 const double nn_dists_max = search_radius_ * search_radius_;
78
79 // The arrays to be used
80 Indices nn_indices (mean_k);
81 std::vector<float> nn_dists(mean_k);
82 // Set to keep all points and in the filtering set those we don't want to keep, assuming
83 // we want to keep the majority of the points.
84 // 0 = remove, 1 = keep
85 std::vector<std::uint8_t> to_keep(indices_->size(), 1);
86 indices.resize (indices_->size ());
87 removed_indices_->resize (indices_->size ());
88 int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator
89
90 // If the data is dense => use nearest-k search
91 if (input_->is_dense)
92 {
93 #pragma omp parallel for \
94 schedule(dynamic,64) \
95 firstprivate(nn_indices, nn_dists) \
96 shared(to_keep) \
97 num_threads(num_threads_)
98 for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(indices_->size()); i++)
99 {
100 const auto& index = (*indices_)[i];
101 // Perform the nearest-k search
102 const int k = searcher_->nearestKSearch (index, mean_k, nn_indices, nn_dists);
103
104 // Check the number of neighbors
105 // Note: nn_dists is sorted, so check the last item
106 if (k == mean_k)
107 {
108 // if negative_ is false and a neighbor is further away than max distance, remove the point
109 // or
110 // if negative is true and a neighbor is closer than max distance, remove the point
111 if ((!negative_ && nn_dists_max < nn_dists[k-1]) || (negative_ && nn_dists_max >= nn_dists[k - 1]))
112 {
113 to_keep[i] = 0;
114 continue;
115 }
116 }
117 else
118 {
119 if (!negative_)
120 {
121 // if too few neighbors, remove the point
122 to_keep[i] = 0;
123 continue;
124 }
125 }
126 }
127 }
128 // NaN or Inf values could exist => use radius search
129 else
130 {
131 #pragma omp parallel for \
132 schedule(dynamic, 64) \
133 firstprivate(nn_indices, nn_dists) \
134 shared(to_keep) \
135 num_threads(num_threads_)
136 for (ptrdiff_t i = 0; i < static_cast<ptrdiff_t>(indices_->size()); i++)
137 {
138 const auto& index = (*indices_)[i];
139 if (!pcl::isXYZFinite((*input_)[index]))
140 {
141 to_keep[i] = 0;
142 continue;
143 }
144
145 // Perform the radius search
146 // Note: k includes the query point, so is always at least 1
147 // last parameter (max_nn) is the maximum number of neighbors returned. If enough neighbors are found so that point can not be an outlier, we stop searching.
148 const int k = searcher_->radiusSearch (index, search_radius_, nn_indices, nn_dists, min_pts_radius_ + 1);
149
150 // Points having too few neighbors are removed
151 // or if negative_ is true, then if it has too many neighbors
152 if ((!negative_ && k <= min_pts_radius_) || (negative_ && k > min_pts_radius_))
153 {
154 to_keep[i] = 0;
155 continue;
156 }
157 }
158 }
159
160 for (index_t i=0; i < static_cast<index_t>(to_keep.size()); i++)
161 {
162 if (to_keep[i] == 0)
163 {
164 if (extract_removed_indices_)
165 (*removed_indices_)[rii++] = (*indices_)[i];
166 continue;
167 }
168
169 // Otherwise it was a normal point for output (inlier)
170 indices[oii++] = (*indices_)[i];
171 }
172
173 // Resize the output arrays
174 indices.resize (oii);
175 removed_indices_->resize (rii);
176}
177
178#define PCL_INSTANTIATE_RadiusOutlierRemoval(T) template class PCL_EXPORTS pcl::RadiusOutlierRemoval<T>;
179
180#endif // PCL_FILTERS_IMPL_RADIUS_OUTLIER_REMOVAL_H_
181
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:66
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
constexpr bool isXYZFinite(const PointT &) noexcept