Point Cloud Library (PCL)  1.10.1-dev
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 #pragma once
41 
42 #include <pcl/pcl_base.h>
43 #include <pcl/common/io.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 output 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 output 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 
91  using PointCloudPtr = typename PointCloud::Ptr;
93 
94  /** \brief Empty constructor.
95  * \param[in] extract_removed_indices set to true if the filtered data indices should be saved in a
96  * separate list. Default: false.
97  */
98  Filter (bool extract_removed_indices = false) :
99  removed_indices_ (new std::vector<int>),
100  extract_removed_indices_ (extract_removed_indices)
101  {
102  }
103 
104  /** \brief Get the point indices being removed */
105  inline IndicesConstPtr const
107  {
108  return (removed_indices_);
109  }
110 
111  /** \brief Get the point indices being removed
112  * \param[out] pi the resultant point indices that have been removed
113  */
114  inline void
116  {
118  }
119 
120  /** \brief Calls the filtering method and returns the filtered dataset in output.
121  * \param[out] output the resultant filtered point cloud dataset
122  */
123  inline void
124  filter (PointCloud &output)
125  {
126  if (!initCompute ())
127  return;
128 
129  if (input_.get () == &output) // cloud_in = cloud_out
130  {
131  PointCloud output_temp;
132  applyFilter (output_temp);
133  output_temp.header = input_->header;
134  output_temp.sensor_origin_ = input_->sensor_origin_;
135  output_temp.sensor_orientation_ = input_->sensor_orientation_;
136  pcl::copyPointCloud (output_temp, output);
137  }
138  else
139  {
140  output.header = input_->header;
141  output.sensor_origin_ = input_->sensor_origin_;
142  output.sensor_orientation_ = input_->sensor_orientation_;
143  applyFilter (output);
144  }
145 
146  deinitCompute ();
147  }
148 
149  protected:
150 
153 
156 
157  /** \brief Indices of the points that are removed */
159 
160  /** \brief The filter name. */
161  std::string filter_name_;
162 
163  /** \brief Set to true if we want to return the indices of the removed points. */
165 
166  /** \brief Abstract filter method.
167  *
168  * The implementation needs to set output.{points, width, height, is_dense}.
169  *
170  * \param[out] output the resultant filtered point cloud
171  */
172  virtual void
173  applyFilter (PointCloud &output) = 0;
174 
175  /** \brief Get a string representation of the name of this class. */
176  inline const std::string&
177  getClassName () const
178  {
179  return (filter_name_);
180  }
181  };
182 
183  ////////////////////////////////////////////////////////////////////////////////////////////
184  /** \brief Filter represents the base filter class. All filters must inherit from this interface.
185  * \author Radu B. Rusu
186  * \ingroup filters
187  */
188  template<>
189  class PCL_EXPORTS Filter<pcl::PCLPointCloud2> : public PCLBase<pcl::PCLPointCloud2>
190  {
191  public:
194 
198 
199  /** \brief Empty constructor.
200  * \param[in] extract_removed_indices set to true if the filtered data indices should be saved in a
201  * separate list. Default: false.
202  */
203  Filter (bool extract_removed_indices = false) :
204  removed_indices_ (new std::vector<int>),
205  extract_removed_indices_ (extract_removed_indices)
206  {
207  }
208 
209  /** \brief Get the point indices being removed */
210  inline IndicesConstPtr const
212  {
213  return (removed_indices_);
214  }
215 
216  /** \brief Get the point indices being removed
217  * \param[out] pi the resultant point indices that have been removed
218  */
219  inline void
221  {
223  }
224 
225  /** \brief Calls the filtering method and returns the filtered dataset in output.
226  * \param[out] output the resultant filtered point cloud dataset
227  */
228  void
229  filter (PCLPointCloud2 &output);
230 
231  protected:
232 
233  /** \brief Indices of the points that are removed */
235 
236  /** \brief Set to true if we want to return the indices of the removed points. */
238 
239  /** \brief The filter name. */
240  std::string filter_name_;
241 
242  /** \brief Abstract filter method.
243  *
244  * The implementation needs to set output.{data, row_step, point_step, width, height, is_dense}.
245  *
246  * \param[out] output the resultant filtered point cloud
247  */
248  virtual void
249  applyFilter (PCLPointCloud2 &output) = 0;
250 
251  /** \brief Get a string representation of the name of this class. */
252  inline const std::string&
253  getClassName () const
254  {
255  return (filter_name_);
256  }
257  };
258 }
259 
260 #ifdef PCL_NO_PRECOMPILE
261 #include <pcl/filters/impl/filter.hpp>
262 #endif
void removeNaNNormalsFromPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, std::vector< int > &index)
Removes points that have their normals invalid (i.e., equal to NaN)
Definition: filter.hpp:99
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
void getRemovedIndices(PointIndices &pi)
Get the point indices being removed.
Definition: filter.h:220
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
std::string filter_name_
The filter name.
Definition: filter.h:240
void removeNaNFromPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, std::vector< int > &index)
Removes points with x, y, or z equal to NaN.
Definition: filter.hpp:46
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:62
shared_ptr< Filter< pcl::PCLPointCloud2 > > Ptr
Definition: filter.h:192
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:124
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:63
Filter(bool extract_removed_indices=false)
Empty constructor.
Definition: filter.h:98
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138
IndicesConstPtr const getRemovedIndices() const
Get the point indices being removed.
Definition: filter.h:211
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:407
Filter(bool extract_removed_indices=false)
Empty constructor.
Definition: filter.h:203
Filter represents the base filter class.
Definition: filter.h:83
void getRemovedIndices(PointIndices &pi)
Get the point indices being removed.
Definition: filter.h:115
shared_ptr< Filter< pcl::PointXYZRGBL > > Ptr
Definition: filter.h:86
PCL base class.
Definition: pcl_base.h:70
IndicesPtr removed_indices_
Indices of the points that are removed.
Definition: filter.h:158
IndicesConstPtr const getRemovedIndices() const
Get the point indices being removed.
Definition: filter.h:106
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:74
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:409
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:171
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:393
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: filter.h:177
PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr
Definition: pcl_base.h:187
bool extract_removed_indices_
Set to true if we want to return the indices of the removed points.
Definition: filter.h:237
use but beware of subtle difference in behavior(see documentation)") PCL_EXPORTS bool concatenatePointCloud ( const pcl PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
bool extract_removed_indices_
Set to true if we want to return the indices of the removed points.
Definition: filter.h:164
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:415
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: filter.h:253
virtual void applyFilter(PointCloud &output)=0
Abstract filter method.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:148
std::string filter_name_
The filter name.
Definition: filter.h:161
shared_ptr< const Filter< pcl::PCLPointCloud2 > > ConstPtr
Definition: filter.h:193
shared_ptr< const Filter< pcl::PointXYZRGBL > > ConstPtr
Definition: filter.h:87
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: memory.h:81
#define PCL_EXPORTS
Definition: pcl_macros.h:276
PCLPointCloud2::Ptr PCLPointCloud2Ptr
Definition: pcl_base.h:186
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:75
IndicesPtr removed_indices_
Indices of the points that are removed.
Definition: filter.h:234