Point Cloud Library (PCL)  1.7.1
filter.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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_FILTER_H_
41 #define PCL_FILTER_H_
42 
43 #include <pcl/pcl_base.h>
44 #include <pcl/conversions.h>
45 #include <pcl/filters/boost.h>
46 #include <cfloat>
47 #include <pcl/PointIndices.h>
48 
49 namespace pcl
50 {
51  /** \brief Removes points with x, y, or z equal to NaN
52  * \param[in] cloud_in the input point cloud
53  * \param[out] cloud_out the input point cloud
54  * \param[out] index the mapping (ordered): cloud_out.points[i] = cloud_in.points[index[i]]
55  * \note The density of the point cloud is lost.
56  * \note Can be called with cloud_in == cloud_out
57  * \ingroup filters
58  */
59  template<typename PointT> void
61  pcl::PointCloud<PointT> &cloud_out,
62  std::vector<int> &index);
63 
64  /** \brief Removes points that have their normals invalid (i.e., equal to NaN)
65  * \param[in] cloud_in the input point cloud
66  * \param[out] cloud_out the input point cloud
67  * \param[out] index the mapping (ordered): cloud_out.points[i] = cloud_in.points[index[i]]
68  * \note The density of the point cloud is lost.
69  * \note Can be called with cloud_in == cloud_out
70  * \ingroup filters
71  */
72  template<typename PointT> void
74  pcl::PointCloud<PointT> &cloud_out,
75  std::vector<int> &index);
76 
77  ////////////////////////////////////////////////////////////////////////////////////////////
78  /** \brief Filter represents the base filter class. All filters must inherit from this interface.
79  * \author Radu B. Rusu
80  * \ingroup filters
81  */
82  template<typename PointT>
83  class Filter : public PCLBase<PointT>
84  {
85  public:
88 
89  typedef boost::shared_ptr< Filter<PointT> > Ptr;
90  typedef boost::shared_ptr< const Filter<PointT> > ConstPtr;
91 
92 
94  typedef typename PointCloud::Ptr PointCloudPtr;
96 
97  /** \brief Empty constructor.
98  * \param[in] extract_removed_indices set to true if the filtered data indices should be saved in a
99  * separate list. Default: false.
100  */
101  Filter (bool extract_removed_indices = false) :
102  removed_indices_ (new std::vector<int>),
103  filter_name_ (),
104  extract_removed_indices_ (extract_removed_indices)
105  {
106  }
107 
108  /** \brief Empty destructor */
109  virtual ~Filter () {}
110 
111  /** \brief Get the point indices being removed */
112  inline IndicesConstPtr const
114  {
115  return (removed_indices_);
116  }
117 
118  /** \brief Get the point indices being removed
119  * \param[out] pi the resultant point indices that have been removed
120  */
121  inline void
123  {
125  }
126 
127  /** \brief Calls the filtering method and returns the filtered dataset in output.
128  * \param[out] output the resultant filtered point cloud dataset
129  */
130  inline void
131  filter (PointCloud &output)
132  {
133  if (!initCompute ())
134  return;
135 
136  // Resize the output dataset
137  //if (output.points.size () != indices_->size ())
138  // output.points.resize (indices_->size ());
139 
140  // Copy header at a minimum
141  output.header = input_->header;
142  output.sensor_origin_ = input_->sensor_origin_;
143  output.sensor_orientation_ = input_->sensor_orientation_;
144 
145  // Apply the actual filter
146  applyFilter (output);
147 
148  deinitCompute ();
149  }
150 
151  protected:
152 
155 
156  /** \brief Indices of the points that are removed */
158 
159  /** \brief The filter name. */
160  std::string filter_name_;
161 
162  /** \brief Set to true if we want to return the indices of the removed points. */
164 
165  /** \brief Abstract filter method.
166  *
167  * The implementation needs to set output.{points, width, height, is_dense}.
168  *
169  * \param[out] output the resultant filtered point cloud
170  */
171  virtual void
172  applyFilter (PointCloud &output) = 0;
173 
174  /** \brief Get a string representation of the name of this class. */
175  inline const std::string&
176  getClassName () const
177  {
178  return (filter_name_);
179  }
180  };
181 
182  ////////////////////////////////////////////////////////////////////////////////////////////
183  /** \brief Filter represents the base filter class. All filters must inherit from this interface.
184  * \author Radu B. Rusu
185  * \ingroup filters
186  */
187  template<>
188  class PCL_EXPORTS Filter<pcl::PCLPointCloud2> : public PCLBase<pcl::PCLPointCloud2>
189  {
190  public:
191  typedef boost::shared_ptr< Filter<pcl::PCLPointCloud2> > Ptr;
192  typedef boost::shared_ptr< const Filter<pcl::PCLPointCloud2> > ConstPtr;
193 
197 
198  /** \brief Empty constructor.
199  * \param[in] extract_removed_indices set to true if the filtered data indices should be saved in a
200  * separate list. Default: false.
201  */
202  Filter (bool extract_removed_indices = false) :
203  removed_indices_ (new std::vector<int>),
204  extract_removed_indices_ (extract_removed_indices),
205  filter_name_ ()
206  {
207  }
208 
209  /** \brief Empty destructor */
210  virtual ~Filter () {}
211 
212  /** \brief Get the point indices being removed */
213  inline IndicesConstPtr const
215  {
216  return (removed_indices_);
217  }
218 
219  /** \brief Get the point indices being removed
220  * \param[out] pi the resultant point indices that have been removed
221  */
222  inline void
224  {
225  pi.indices = *removed_indices_;
226  }
227 
228  /** \brief Calls the filtering method and returns the filtered dataset in output.
229  * \param[out] output the resultant filtered point cloud dataset
230  */
231  void
232  filter (PCLPointCloud2 &output);
233 
234  protected:
235 
236  /** \brief Indices of the points that are removed */
238 
239  /** \brief Set to true if we want to return the indices of the removed points. */
241 
242  /** \brief The filter name. */
243  std::string filter_name_;
244 
245  /** \brief Abstract filter method.
246  *
247  * The implementation needs to set output.{data, row_step, point_step, width, height, is_dense}.
248  *
249  * \param[out] output the resultant filtered point cloud
250  */
251  virtual void
252  applyFilter (PCLPointCloud2 &output) = 0;
253 
254  /** \brief Get a string representation of the name of this class. */
255  inline const std::string&
256  getClassName () const
257  {
258  return (filter_name_);
259  }
260  };
261 }
262 
263 #ifdef PCL_NO_PRECOMPILE
264 #include <pcl/filters/impl/filter.hpp>
265 #endif
266 
267 #endif //#ifndef PCL_FILTER_H_