Point Cloud Library (PCL)  1.9.1-dev
passthrough.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  */
35 
36 #pragma once
37 
38 #include <pcl_cuda/filters/filter.h>
39 #include <thrust/count.h>
40 #include <thrust/remove.h>
41 #include <vector_types.h>
42 
43 namespace pcl_cuda
44 {
45 
46  /** \brief Check if a specific point is valid or not. Applicable to AOS structures. */
47  struct isFiniteAOS
48  {
49  __inline__ __device__ bool
50  operator () (const PointXYZRGB &pt)
51  //operator () (const float3 &pt)
52  {
53  return (isfinite (pt.x) && isfinite (pt.y) && isfinite (pt.z));
54  }
55  };
56 
57  /** \brief Check if a specific point is valid or not. Applicable to SOA structures. */
58  struct isFiniteSOA
59  {
60  __inline__ __device__ bool
61  operator () (const float &pt)
62  {
63  return (isfinite (pt));
64  }
65  };
66 
67  /** \brief Check if a specific point is valid or not. Applicable to SOA structures. */
69  {
70  __inline__ __device__ bool
71  operator () (const PointCloudSOA<Device>::tuple_type& tuple)
72  {
73  using thrust::get;
74  return (!isfinite (get<0> (tuple)) ||
75  !isfinite (get<1> (tuple)) ||
76  !isfinite (get<2> (tuple)));
77  }
78  };
79 
80  ///////////////////////////////////////////////////////////////////////////////////////////
81  /** \brief @b PassThrough uses the base Filter class methods to pass through
82  * all data that satisfies the user given constraints.
83  */
84  template <typename CloudT>
85  class PassThrough: public Filter<CloudT>
86  {
87  public:
89 
90  using PointCloud = typename PCLCUDABase<CloudT>::PointCloud;
91  using PointCloudPtr = typename PointCloud::Ptr;
93 
94  /** \brief Empty constructor. */
96  {
97  filter_name_ = "PassThrough";
98  };
99 
100  protected:
101  /** \brief Filter a Point Cloud.
102  * \param output the resultant point cloud message
103  */
104  void
106  {
107  std::cerr << "applyFilter" << std::endl;
108  }
109  };
110 
111  ///////////////////////////////////////////////////////////////////////////////////////////
112  template <>
113  class PassThrough<PointCloudAOS<Device> >: public Filter<PointCloudAOS<Device> >
114  {
115  public:
116  /** \brief Empty constructor. */
118  {
119  filter_name_ = "PassThroughAOS";
120  };
121 
122  protected:
123  /** \brief Filter a Point Cloud.
124  * \param output the resultant point cloud message
125  */
126  void
128  {
129  // Allocate enough space
130  output.points.resize (input_->points.size ());
131  // Copy data
132  Device<PointXYZRGB>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.points.begin (), isFiniteAOS ());
133  //Device<float3>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.points.begin (), isFiniteAOS ());
134  output.points.resize (nr_points - output.points.begin ());
135 
136  //std::cerr << "[applyFilterAOS]: ";
137  //std::cerr << input_->points.size () << " " << output.points.size () << std::endl;
138  }
139  };
140 
141  //////////////////////////////////////////////////////////////////////////////////////////
142  template <>
143  class PassThrough<PointCloudSOA<Device> >: public Filter<PointCloudSOA<Device> >
144  {
145  public:
146  /** \brief Empty constructor. */
147  PassThrough () : zip_(false)
148  {
149  filter_name_ = "PassThroughSOA";
150  };
151 
152  inline void
153  setZip (bool zip)
154  {
155  zip_ = zip;
156  }
157 
158 
159  protected:
160  /** \brief Filter a Point Cloud.
161  * \param output the resultant point cloud message
162  */
163  void
165  {
166  if (!zip_)
167  {
168  // Allocate enough space
169  output.resize (input_->size ());
170  // Copy data
171  Device<float>::type::iterator nr_points = thrust::copy_if (input_->points_x.begin (), input_->points_x.end (), output.points_x.begin (), isFiniteSOA ());
172  nr_points = thrust::copy_if (input_->points_y.begin (), input_->points_y.end (), output.points_y.begin (), isFiniteSOA ());
173  nr_points = thrust::copy_if (input_->points_z.begin (), input_->points_z.end (), output.points_z.begin (), isFiniteSOA ());
174  output.resize (nr_points - output.points_z.begin ());
175 
176  //std::cerr << "[applyFilterSOA]: ";
177  //std::cerr << input_->size () << " " << output.size () << std::endl;
178  }
179 
180  else
181  {
182  output = *input_;
183  PointCloud::zip_iterator result = thrust::remove_if (output.zip_begin (), output.zip_end (), isFiniteZIPSOA ());
184  PointCloud::iterator_tuple result_tuple = result.get_iterator_tuple ();
185  PointCloud::float_iterator xiter = thrust::get<0> (result_tuple),
186  yiter = thrust::get<1> (result_tuple),
187  ziter = thrust::get<2> (result_tuple);
188 
189  unsigned badpoints = distance (xiter, output.points_x.end ());
190  unsigned goodpoints = distance (output.points_x.begin (), xiter);
191 
192  output.resize (goodpoints);
193 
194  //std::cerr << "[applyFilterSOA-ZIP]: ";
195  //std::cerr << input_->size () << " " << output.size () << std::endl;
196  }
197  }
198 
199  private:
200  bool zip_;
201  };
202 }
void applyFilter(PointCloud &output)
Filter a Point Cloud.
Definition: passthrough.h:164
Check if a specific point is valid or not.
Definition: passthrough.h:58
__inline__ __device__ bool operator()(const PointXYZRGB &pt)
Definition: passthrough.h:50
Check if a specific point is valid or not.
Definition: passthrough.h:47
typename PointCloud::Ptr PointCloudPtr
Definition: filter.h:67
PassThrough uses the base Filter class methods to pass through all data that satisfies the user given...
Definition: passthrough.h:85
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:411
Removes points with x, y, or z equal to NaN.
Definition: filter.h:58
Check if a specific point is valid or not.
Definition: passthrough.h:68
PassThrough()
Empty constructor.
Definition: passthrough.h:95
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:412
typename PCLCUDABase< CloudT >::PointCloud PointCloud
Definition: filter.h:66
void applyFilter(PointCloud &output)
Filter a Point Cloud.
Definition: passthrough.h:105
void applyFilter(PointCloud &output)
Filter a Point Cloud.
Definition: passthrough.h:127
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: filter.h:68