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