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