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