Point Cloud Library (PCL)  1.8.1
voxel_grid.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 #ifndef PCL_CUDA_FILTERS_VOXELGRID_H_
37 #define PCL_CUDA_FILTERS_VOXELGRID_H_
38 
39 #include <pcl_cuda/filters/filter.h>
40 #include <pcl_cuda/filters/passthrough.h>
41 #include <thrust/count.h>
42 #include <thrust/remove.h>
43 #include <vector_types.h>
44 
45 namespace pcl_cuda
46 {
47  ///////////////////////////////////////////////////////////////////////////////////////////
48  /** \brief @b VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
49  */
50  template <typename CloudT>
51  class VoxelGrid: public Filter<CloudT>
52  {
53  public:
55 
56  typedef typename PCLCUDABase<CloudT>::PointCloud PointCloud;
57  typedef typename PointCloud::Ptr PointCloudPtr;
59 
60  /** \brief Empty constructor. */
62  {
63  filter_name_ = "VoxelGrid";
64  };
65 
66  protected:
67  /** \brief Filter a Point Cloud.
68  * \param output the resultant point cloud message
69  */
70  void
71  applyFilter (PointCloud &output)
72  {
73  std::cerr << "applyFilter" << std::endl;
74  }
75  };
76 
77  ///////////////////////////////////////////////////////////////////////////////////////////
78  template <>
79  class VoxelGrid<PointCloudAOS<Device> >: public Filter<PointCloudAOS<Device> >
80  {
81  public:
82  /** \brief Empty constructor. */
84  {
85  filter_name_ = "VoxelGridAOS";
86  };
87 
88  protected:
89  /** \brief Filter a Point Cloud.
90  * \param output the resultant point cloud message
91  */
92  void
94  {
95  // Allocate enough space
96  output.points.resize (input_->points.size ());
97  // Copy data
98  Device<PointXYZRGB>::type::iterator nr_points = thrust::copy_if (input_->points.begin (), input_->points.end (), output.points.begin (), isFiniteAOS ());
99  output.points.resize (nr_points - output.points.begin ());
100 
101  //std::cerr << "[applyFilterAOS]: ";
102  //std::cerr << input_->points.size () << " " << output.points.size () << std::endl;
103  }
104  };
105 
106  //////////////////////////////////////////////////////////////////////////////////////////
107  template <>
108  class VoxelGrid<PointCloudSOA<Device> >: public Filter<PointCloudSOA<Device> >
109  {
110  public:
111  /** \brief Empty constructor. */
112  VoxelGrid () : zip_(false)
113  {
114  filter_name_ = "VoxelGridSOA";
115  };
116 
117  inline void
118  setZip (bool zip)
119  {
120  zip_ = zip;
121  }
122 
123 
124  protected:
125  /** \brief Filter a Point Cloud.
126  * \param output the resultant point cloud message
127  */
128  void
130  {
131  if (!zip_)
132  {
133  // Allocate enough space
134  output.resize (input_->size ());
135  // Copy data
136  Device<float>::type::iterator nr_points = thrust::copy_if (input_->points_x.begin (), input_->points_x.end (), output.points_x.begin (), isFiniteSOA ());
137  nr_points = thrust::copy_if (input_->points_y.begin (), input_->points_y.end (), output.points_y.begin (), isFiniteSOA ());
138  nr_points = thrust::copy_if (input_->points_z.begin (), input_->points_z.end (), output.points_z.begin (), isFiniteSOA ());
139  output.resize (nr_points - output.points_z.begin ());
140 
141  //std::cerr << "[applyFilterSOA]: ";
142  //std::cerr << input_->size () << " " << output.size () << std::endl;
143  }
144 
145  else
146  {
147  output = *input_;
148  PointCloud::zip_iterator result = thrust::remove_if (output.zip_begin (), output.zip_end (), isFiniteZIPSOA ());
149  PointCloud::iterator_tuple result_tuple = result.get_iterator_tuple ();
150  PointCloud::float_iterator xiter = thrust::get<0> (result_tuple),
151  yiter = thrust::get<1> (result_tuple),
152  ziter = thrust::get<2> (result_tuple);
153 
154  unsigned badpoints = distance (xiter, output.points_x.end ());
155  unsigned goodpoints = distance (output.points_x.begin (), xiter);
156 
157  output.resize (goodpoints);
158 
159  //std::cerr << "[applyFilterSOA-ZIP]: ";
160  //std::cerr << input_->size () << " " << output.size () << std::endl;
161  }
162  }
163 
164  private:
165  bool zip_;
166  };
167 }
168 
169 #endif //#ifndef PCL_FILTERS_VOXELGRID_H_
void applyFilter(PointCloud &output)
Filter a Point Cloud.
Definition: voxel_grid.h:129
PointCloud::Ptr PointCloudPtr
Definition: voxel_grid.h:57
PointCloud::ConstPtr PointCloudConstPtr
Definition: voxel_grid.h:58
Check if a specific point is valid or not.
Definition: passthrough.h:59
Check if a specific point is valid or not.
Definition: passthrough.h:48
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
VoxelGrid()
Empty constructor.
Definition: voxel_grid.h:61
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
Definition: voxel_grid.h:51
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
PCLCUDABase< PointCloudAOS< Device > >::PointCloud PointCloud
Definition: filter.h:67
Removes points with x, y, or z equal to NaN.
Definition: filter.h:59
Check if a specific point is valid or not.
Definition: passthrough.h:69
void applyFilter(PointCloud &output)
Filter a Point Cloud.
Definition: voxel_grid.h:93
std::string filter_name_
The filter name.
Definition: filter.h:153
PCLCUDABase< CloudT >::PointCloud PointCloud
Definition: voxel_grid.h:56
void applyFilter(PointCloud &output)
Filter a Point Cloud.
Definition: voxel_grid.h:71