Point Cloud Library (PCL) 1.14.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 * \author James Bowman, Radu B. Rusu
96 * \ingroup filters
97 */
98 template <typename PointT>
99 class ApproximateVoxelGrid: public Filter<PointT>
100 {
103 using Filter<PointT>::input_;
105
106 using PointCloud = typename Filter<PointT>::PointCloud;
107 using PointCloudPtr = typename PointCloud::Ptr;
108 using PointCloudConstPtr = typename PointCloud::ConstPtr;
109
110 private:
111 struct he
112 {
113 he () = default;
114 int ix{0}, iy{0}, iz{0};
115 int count{0};
116 Eigen::VectorXf centroid;
117 };
118
119 public:
120
121 using Ptr = shared_ptr<ApproximateVoxelGrid<PointT> >;
122 using ConstPtr = shared_ptr<const ApproximateVoxelGrid<PointT> >;
123
124
125 /** \brief Empty constructor. */
127 pcl::Filter<PointT> (),
128 leaf_size_ (Eigen::Vector3f::Ones ()),
129 inverse_leaf_size_ (Eigen::Array3f::Ones ()),
130
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> (),
144 histsize_ (src.histsize_),
145 history_ ()
146 {
147 history_ = new he[histsize_];
148 for (std::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&
166 {
170 histsize_ = src.histsize_;
171 history_ = new he[histsize_];
172 for (std::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
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 std::size_t histsize_{512};
226
227 /** \brief history buffer */
228 struct he* history_;
229
230 using FieldList = typename pcl::traits::fieldList<PointT>::type;
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, std::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
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