Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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
42namespace 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 {
102 using Filter<PointT>::input_;
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> (),
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 {
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...
ApproximateVoxelGrid & operator=(const ApproximateVoxelGrid &src)
Copy operator.
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(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
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition filter.h:174
std::string filter_name_
The filter name.
Definition filter.h:158
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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