Point Cloud Library (PCL)  1.7.1
radius_outlier_removal.h
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_RADIUS_OUTLIER_REMOVAL_H_
41 #define PCL_FILTERS_RADIUS_OUTLIER_REMOVAL_H_
42 
43 #include <pcl/filters/filter_indices.h>
44 #include <pcl/search/pcl_search.h>
45 
46 namespace pcl
47 {
48  /** \brief @b RadiusOutlierRemoval filters points in a cloud based on the number of neighbors they have.
49  * \details Iterates through the entire input once, and for each point, retrieves the number of neighbors within a certain radius.
50  * The point will be considered an outlier if it has too few neighbors, as determined by setMinNeighborsInRadius().
51  * The radius can be changed using setRadiusSearch().
52  * <br>
53  * The neighbors found for each query point will be found amongst ALL points of setInputCloud(), not just those indexed by setIndices().
54  * The setIndices() method only indexes the points that will be iterated through as search query points.
55  * <br><br>
56  * Usage example:
57  * \code
58  * pcl::RadiusOutlierRemoval<PointType> rorfilter (true); // Initializing with true will allow us to extract the removed indices
59  * rorfilter.setInputCloud (cloud_in);
60  * rorfilter.setRadiusSearch (0.1);
61  * rorfilter.setMinNeighborsInRadius (5);
62  * rorfilter.setNegative (true);
63  * rorfilter.filter (*cloud_out);
64  * // The resulting cloud_out contains all points of cloud_in that have 4 or less neighbors within the 0.1 search radius
65  * indices_rem = rorfilter.getRemovedIndices ();
66  * // The indices_rem array indexes all points of cloud_in that have 5 or more neighbors within the 0.1 search radius
67  * \endcode
68  * \author Radu Bogdan Rusu
69  * \ingroup filters
70  */
71  template<typename PointT>
72  class RadiusOutlierRemoval : public FilterIndices<PointT>
73  {
74  protected:
76  typedef typename PointCloud::Ptr PointCloudPtr;
79 
80  public:
81 
82  typedef boost::shared_ptr< RadiusOutlierRemoval<PointT> > Ptr;
83  typedef boost::shared_ptr< const RadiusOutlierRemoval<PointT> > ConstPtr;
84 
85 
86  /** \brief Constructor.
87  * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false).
88  */
89  RadiusOutlierRemoval (bool extract_removed_indices = false) :
90  FilterIndices<PointT>::FilterIndices (extract_removed_indices),
91  searcher_ (),
92  search_radius_ (0.0),
93  min_pts_radius_ (1)
94  {
95  filter_name_ = "RadiusOutlierRemoval";
96  }
97 
98  /** \brief Set the radius of the sphere that will determine which points are neighbors.
99  * \details The number of points within this distance from the query point will need to be equal or greater
100  * than setMinNeighborsInRadius() in order to be classified as an inlier point (i.e. will not be filtered).
101  * \param[in] radius The radius of the sphere for nearest neighbor searching.
102  */
103  inline void
104  setRadiusSearch (double radius)
105  {
106  search_radius_ = radius;
107  }
108 
109  /** \brief Get the radius of the sphere that will determine which points are neighbors.
110  * \details The number of points within this distance from the query point will need to be equal or greater
111  * than setMinNeighborsInRadius() in order to be classified as an inlier point (i.e. will not be filtered).
112  * \return The radius of the sphere for nearest neighbor searching.
113  */
114  inline double
116  {
117  return (search_radius_);
118  }
119 
120  /** \brief Set the number of neighbors that need to be present in order to be classified as an inlier.
121  * \details The number of points within setRadiusSearch() from the query point will need to be equal or greater
122  * than this number in order to be classified as an inlier point (i.e. will not be filtered).
123  * \param min_pts The minimum number of neighbors (default = 1).
124  */
125  inline void
127  {
128  min_pts_radius_ = min_pts;
129  }
130 
131  /** \brief Get the number of neighbors that need to be present in order to be classified as an inlier.
132  * \details The number of points within setRadiusSearch() from the query point will need to be equal or greater
133  * than this number in order to be classified as an inlier point (i.e. will not be filtered).
134  * \param min_pts The minimum number of neighbors (default = 1).
135  */
136  inline int
138  {
139  return (min_pts_radius_);
140  }
141 
142  protected:
152 
153  /** \brief Filtered results are stored in a separate point cloud.
154  * \param[out] output The resultant point cloud.
155  */
156  void
157  applyFilter (PointCloud &output);
158 
159  /** \brief Filtered results are indexed by an indices array.
160  * \param[out] indices The resultant indices.
161  */
162  void
163  applyFilter (std::vector<int> &indices)
164  {
165  applyFilterIndices (indices);
166  }
167 
168  /** \brief Filtered results are indexed by an indices array.
169  * \param[out] indices The resultant indices.
170  */
171  void
172  applyFilterIndices (std::vector<int> &indices);
173 
174  private:
175  /** \brief A pointer to the spatial search object. */
176  SearcherPtr searcher_;
177 
178  /** \brief The nearest neighbors search radius for each point. */
179  double search_radius_;
180 
181  /** \brief The minimum number of neighbors that a point needs to have in the given search radius to be considered an inlier. */
182  int min_pts_radius_;
183  };
184 
185  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
186  /** \brief @b RadiusOutlierRemoval is a simple filter that removes outliers if the number of neighbors in a certain
187  * search radius is smaller than a given K.
188  * \note setFilterFieldName (), setFilterLimits (), and setFilterLimitNegative () are ignored.
189  * \author Radu Bogdan Rusu
190  * \ingroup filters
191  */
192  template<>
193  class PCL_EXPORTS RadiusOutlierRemoval<pcl::PCLPointCloud2> : public Filter<pcl::PCLPointCloud2>
194  {
197 
200 
202  typedef pcl::search::Search<pcl::PointXYZ>::Ptr KdTreePtr;
203 
207 
208  public:
209  /** \brief Empty constructor. */
210  RadiusOutlierRemoval (bool extract_removed_indices = false) :
211  Filter<pcl::PCLPointCloud2>::Filter (extract_removed_indices),
212  search_radius_ (0.0), min_pts_radius_ (1), tree_ ()
213  {
214  filter_name_ = "RadiusOutlierRemoval";
215  }
216 
217  /** \brief Set the sphere radius that is to be used for determining the k-nearest neighbors for filtering.
218  * \param radius the sphere radius that is to contain all k-nearest neighbors
219  */
220  inline void
221  setRadiusSearch (double radius)
222  {
223  search_radius_ = radius;
224  }
225 
226  /** \brief Get the sphere radius used for determining the k-nearest neighbors. */
227  inline double
229  {
230  return (search_radius_);
231  }
232 
233  /** \brief Set the minimum number of neighbors that a point needs to have in the given search radius in order to
234  * be considered an inlier (i.e., valid).
235  * \param min_pts the minimum number of neighbors
236  */
237  inline void
239  {
240  min_pts_radius_ = min_pts;
241  }
242 
243  /** \brief Get the minimum number of neighbors that a point needs to have in the given search radius to be
244  * considered an inlier and avoid being filtered.
245  */
246  inline double
248  {
249  return (min_pts_radius_);
250  }
251 
252  protected:
253  /** \brief The nearest neighbors search radius for each point. */
255 
256  /** \brief The minimum number of neighbors that a point needs to have in the given search radius to be considered
257  * an inlier.
258  */
260 
261  /** \brief A pointer to the spatial search object. */
262  KdTreePtr tree_;
263 
264  void
265  applyFilter (PCLPointCloud2 &output);
266  };
267 }
268 
269 #ifdef PCL_NO_PRECOMPILE
270 #include <pcl/filters/impl/radius_outlier_removal.hpp>
271 #endif
272 
273 #endif // PCL_FILTERS_RADIUS_OUTLIER_REMOVAL_H_
274