Point Cloud Library (PCL)  1.9.1-dev
approximate_voxel_grid.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, 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 the copyright holder(s) 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  * $Id: voxel_grid.h 1374 2011-06-19 02:29:56Z bouffa $
35  *
36  */
37 
38 #pragma once
39 
40 #include <pcl/filters/boost.h>
41 #include <pcl/filters/filter.h>
42 
43 namespace pcl
44 {
45  /** \brief Helper functor structure for copying data between an Eigen::VectorXf and a PointT. */
46  template <typename PointT>
48  {
49  typedef typename traits::POD<PointT>::type Pod;
50 
51  xNdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointT &p2)
52  : p1_ (p1),
53  p2_ (reinterpret_cast<Pod&>(p2)),
54  f_idx_ (0) { }
55 
56  template<typename Key> inline void operator() ()
57  {
58  //boost::fusion::at_key<Key> (p2_) = p1_[f_idx_++];
60  uint8_t* data_ptr = reinterpret_cast<uint8_t*>(&p2_) + pcl::traits::offset<PointT, Key>::value;
61  *reinterpret_cast<T*>(data_ptr) = static_cast<T> (p1_[f_idx_++]);
62  }
63 
64  private:
65  const Eigen::VectorXf &p1_;
66  Pod &p2_;
67  int f_idx_;
68  };
69 
70  /** \brief Helper functor structure for copying data between an Eigen::VectorXf and a PointT. */
71  template <typename PointT>
73  {
74  typedef typename traits::POD<PointT>::type Pod;
75 
76  xNdCopyPointEigenFunctor (const PointT &p1, Eigen::VectorXf &p2)
77  : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
78 
79  template<typename Key> inline void operator() ()
80  {
81  //p2_[f_idx_++] = boost::fusion::at_key<Key> (p1_);
83  const uint8_t* data_ptr = reinterpret_cast<const uint8_t*>(&p1_) + pcl::traits::offset<PointT, Key>::value;
84  p2_[f_idx_++] = static_cast<float> (*reinterpret_cast<const T*>(data_ptr));
85  }
86 
87  private:
88  const Pod &p1_;
89  Eigen::VectorXf &p2_;
90  int f_idx_;
91  };
92 
93  /** \brief ApproximateVoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
94  *
95  * \author James Bowman, Radu B. Rusu
96  * \ingroup filters
97  */
98  template <typename PointT>
99  class ApproximateVoxelGrid: public Filter<PointT>
100  {
105 
106  typedef typename Filter<PointT>::PointCloud PointCloud;
107  typedef typename PointCloud::Ptr PointCloudPtr;
108  typedef typename PointCloud::ConstPtr PointCloudConstPtr;
109 
110  private:
111  struct he
112  {
113  he () : ix (), iy (), iz (), count (0), centroid () {}
114  int ix, iy, iz;
115  int count;
116  Eigen::VectorXf centroid;
117  };
118 
119  public:
120 
121  typedef boost::shared_ptr< ApproximateVoxelGrid<PointT> > Ptr;
122  typedef boost::shared_ptr< const ApproximateVoxelGrid<PointT> > ConstPtr;
123 
124 
125  /** \brief Empty constructor. */
127  pcl::Filter<PointT> (),
128  leaf_size_ (Eigen::Vector3f::Ones ()),
129  inverse_leaf_size_ (Eigen::Array3f::Ones ()),
130  downsample_all_data_ (true), histsize_ (512),
131  history_ (new he[histsize_])
132  {
133  filter_name_ = "ApproximateVoxelGrid";
134  }
135 
136  /** \brief Copy constructor.
137  * \param[in] src the approximate voxel grid to copy into this.
138  */
140  pcl::Filter<PointT> (),
141  leaf_size_ (src.leaf_size_),
142  inverse_leaf_size_ (src.inverse_leaf_size_),
143  downsample_all_data_ (src.downsample_all_data_),
144  histsize_ (src.histsize_),
145  history_ ()
146  {
147  history_ = new he[histsize_];
148  for (size_t i = 0; i < histsize_; i++)
149  history_[i] = src.history_[i];
150  }
151 
152 
153  /** \brief Destructor.
154  */
156  {
157  delete [] history_;
158  }
159 
160 
161  /** \brief Copy operator.
162  * \param[in] src the approximate voxel grid to copy into this.
163  */
164  inline ApproximateVoxelGrid&
165  operator = (const ApproximateVoxelGrid &src)
166  {
167  leaf_size_ = src.leaf_size_;
168  inverse_leaf_size_ = src.inverse_leaf_size_;
169  downsample_all_data_ = src.downsample_all_data_;
170  histsize_ = src.histsize_;
171  history_ = new he[histsize_];
172  for (size_t i = 0; i < histsize_; i++)
173  history_[i] = src.history_[i];
174  return (*this);
175  }
176 
177  /** \brief Set the voxel grid leaf size.
178  * \param[in] leaf_size the voxel grid leaf size
179  */
180  inline void
181  setLeafSize (const Eigen::Vector3f &leaf_size)
182  {
183  leaf_size_ = leaf_size;
184  inverse_leaf_size_ = Eigen::Array3f::Ones () / leaf_size_.array ();
185  }
186 
187  /** \brief Set the voxel grid leaf size.
188  * \param[in] lx the leaf size for X
189  * \param[in] ly the leaf size for Y
190  * \param[in] lz the leaf size for Z
191  */
192  inline void
193  setLeafSize (float lx, float ly, float lz)
194  {
195  setLeafSize (Eigen::Vector3f (lx, ly, lz));
196  }
197 
198  /** \brief Get the voxel grid leaf size. */
199  inline Eigen::Vector3f
200  getLeafSize () const { return (leaf_size_); }
201 
202  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ.
203  * \param downsample the new value (true/false)
204  */
205  inline void
206  setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
207 
208  /** \brief Get the state of the internal downsampling parameter (true if
209  * all fields need to be downsampled, false if just XYZ).
210  */
211  inline bool
212  getDownsampleAllData () const { return (downsample_all_data_); }
213 
214  protected:
215  /** \brief The size of a leaf. */
216  Eigen::Vector3f leaf_size_;
217 
218  /** \brief Compute 1/leaf_size_ to avoid division later */
219  Eigen::Array3f inverse_leaf_size_;
220 
221  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */
223 
224  /** \brief history buffer size, power of 2 */
225  size_t histsize_;
226 
227  /** \brief history buffer */
228  struct he* history_;
229 
231 
232  /** \brief Downsample a Point Cloud using a voxelized grid approach
233  * \param output the resultant point cloud message
234  */
235  void
236  applyFilter (PointCloud &output) override;
237 
238  /** \brief Write a single point from the hash to the output cloud
239  */
240  void
241  flush (PointCloud &output, size_t op, he *hhe, int rgba_index, int centroid_size);
242  };
243 }
244 
245 #ifdef PCL_NO_PRECOMPILE
246 #include <pcl/filters/impl/approximate_voxel_grid.hpp>
247 #endif
PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:73
bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled...
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
xNdCopyEigenPointFunctor(const Eigen::VectorXf &p1, PointT &p2)
boost::shared_ptr< const ApproximateVoxelGrid< PointT > > ConstPtr
ApproximateVoxelGrid()
Empty constructor.
boost::shared_ptr< ApproximateVoxelGrid< PointT > > Ptr
ApproximateVoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the...
Definition: bfgs.h:9
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
size_t histsize_
history buffer size, power of 2
PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:72
Filter represents the base filter class.
Definition: filter.h:83
Eigen::Array3f inverse_leaf_size_
Compute 1/leaf_size_ to avoid division later.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:428
traits::POD< PointT >::type Pod
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
xNdCopyPointEigenFunctor(const PointT &p1, Eigen::VectorXf &p2)
Eigen::Vector3f leaf_size_
The size of a leaf.
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
struct he * history_
history buffer
traits::POD< PointT >::type Pod
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.
pcl::traits::fieldList< PointT >::type FieldList
A point structure representing Euclidean xyz coordinates, and the RGB color.
void setLeafSize(const Eigen::Vector3f &leaf_size)
Set the voxel grid leaf size.
ApproximateVoxelGrid(const ApproximateVoxelGrid &src)
Copy constructor.