Point Cloud Library (PCL) 1.15.0
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 { }
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_{0};
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) { }
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_{0};
90 };
91
92 /** \brief ApproximateVoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
93 * Thus, this is similar to the \ref VoxelGrid filter.
94 * This class works best if points that are stored in memory next to each other (in the input point cloud), are also somewhat close in 3D euclidean space (this is for example usually the case for organized point clouds). If the points have no storage order (e.g. in synthetic, randomly generated point clouds), this class will give very poor results, and \ref VoxelGrid should be used instead.
95 * \sa VoxelGrid
96 * \author James Bowman, Radu B. Rusu
97 * \ingroup filters
98 */
99 template <typename PointT>
100 class ApproximateVoxelGrid: public Filter<PointT>
101 {
104 using Filter<PointT>::input_;
106
107 using PointCloud = typename Filter<PointT>::PointCloud;
108 using PointCloudPtr = typename PointCloud::Ptr;
109 using PointCloudConstPtr = typename PointCloud::ConstPtr;
110
111 private:
112 struct he
113 {
114 he () = default;
115 int ix{0}, iy{0}, iz{0};
116 int count{0};
117 Eigen::VectorXf centroid;
118 };
119
120 public:
121
122 using Ptr = shared_ptr<ApproximateVoxelGrid<PointT> >;
123 using ConstPtr = shared_ptr<const ApproximateVoxelGrid<PointT> >;
124
125
126 /** \brief Empty constructor. */
128 pcl::Filter<PointT> (),
129 leaf_size_ (Eigen::Vector3f::Ones ()),
130 inverse_leaf_size_ (Eigen::Array3f::Ones ()),
131
132 history_ (new he[histsize_])
133 {
134 filter_name_ = "ApproximateVoxelGrid";
135 }
136
137 /** \brief Copy constructor.
138 * \param[in] src the approximate voxel grid to copy into this.
139 */
141 pcl::Filter<PointT> (),
145 histsize_ (src.histsize_),
146 history_ ()
147 {
148 history_ = new he[histsize_];
149 for (std::size_t i = 0; i < histsize_; i++)
150 history_[i] = src.history_[i];
151 }
152
153
154 /** \brief Destructor.
155 */
157 {
158 delete [] history_;
159 }
160
161
162 /** \brief Copy operator.
163 * \param[in] src the approximate voxel grid to copy into this.
164 */
165 inline ApproximateVoxelGrid&
167 {
171 histsize_ = src.histsize_;
172 history_ = new he[histsize_];
173 for (std::size_t i = 0; i < histsize_; i++)
174 history_[i] = src.history_[i];
175 return (*this);
176 }
177
178 /** \brief Set the voxel grid leaf size.
179 * \param[in] leaf_size the voxel grid leaf size
180 */
181 inline void
182 setLeafSize (const Eigen::Vector3f &leaf_size)
183 {
184 leaf_size_ = leaf_size;
185 inverse_leaf_size_ = Eigen::Array3f::Ones () / leaf_size_.array ();
186 }
187
188 /** \brief Set the voxel grid leaf size.
189 * \param[in] lx the leaf size for X
190 * \param[in] ly the leaf size for Y
191 * \param[in] lz the leaf size for Z
192 */
193 inline void
194 setLeafSize (float lx, float ly, float lz)
195 {
196 setLeafSize (Eigen::Vector3f (lx, ly, lz));
197 }
198
199 /** \brief Get the voxel grid leaf size. */
200 inline Eigen::Vector3f
201 getLeafSize () const { return (leaf_size_); }
202
203 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ.
204 * \param downsample the new value (true/false)
205 */
206 inline void
207 setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
208
209 /** \brief Get the state of the internal downsampling parameter (true if
210 * all fields need to be downsampled, false if just XYZ).
211 */
212 inline bool
214
215 protected:
216 /** \brief The size of a leaf. */
217 Eigen::Vector3f leaf_size_;
218
219 /** \brief Compute 1/leaf_size_ to avoid division later */
220 Eigen::Array3f inverse_leaf_size_;
221
222 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */
224
225 /** \brief history buffer size, power of 2 */
226 std::size_t histsize_{512};
227
228 /** \brief history buffer */
229 struct he* history_;
230
231 using FieldList = typename pcl::traits::fieldList<PointT>::type;
232
233 /** \brief Downsample a Point Cloud using a voxelized grid approach
234 * \param output the resultant point cloud message
235 */
236 void
237 applyFilter (PointCloud &output) override;
238
239 /** \brief Write a single point from the hash to the output cloud
240 */
241 void
242 flush (PointCloud &output, std::size_t op, he *hhe, int rgba_index, int centroid_size);
243 };
244}
245
246#ifdef PCL_NO_PRECOMPILE
247#include <pcl/filters/impl/approximate_voxel_grid.hpp>
248#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