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