Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
voxel_grid.hpp
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$
35 *
36 */
37
38#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_H_
39#define PCL_FILTERS_IMPL_VOXEL_GRID_H_
40
41#include <pcl/common/centroid.h>
42#include <pcl/common/common.h>
43#include <pcl/common/io.h>
44#include <pcl/filters/voxel_grid.h>
45#include <boost/sort/spreadsort/integer_sort.hpp>
46
47///////////////////////////////////////////////////////////////////////////////////////////
48template <typename PointT> void
50 const std::string &distance_field_name, float min_distance, float max_distance,
51 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
52{
53 Eigen::Array4f min_p, max_p;
54 min_p.setConstant (FLT_MAX);
55 max_p.setConstant (-FLT_MAX);
56
57 // Get the fields list and the distance field index
58 std::vector<pcl::PCLPointField> fields;
59 int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
60
61 float distance_value;
62 // If dense, no need to check for NaNs
63 if (cloud->is_dense)
64 {
65 for (const auto& point: *cloud)
66 {
67 // Get the distance value
68 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
69 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
70
71 if (limit_negative)
72 {
73 // Use a threshold for cutting out points which inside the interval
74 if ((distance_value < max_distance) && (distance_value > min_distance))
75 continue;
76 }
77 else
78 {
79 // Use a threshold for cutting out points which are too close/far away
80 if ((distance_value > max_distance) || (distance_value < min_distance))
81 continue;
82 }
83 // Create the point structure and get the min/max
84 pcl::Array4fMapConst pt = point.getArray4fMap ();
85 min_p = min_p.min (pt);
86 max_p = max_p.max (pt);
87 }
88 }
89 else
90 {
91 for (const auto& point: *cloud)
92 {
93 // Get the distance value
94 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&point);
95 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
96
97 if (limit_negative)
98 {
99 // Use a threshold for cutting out points which inside the interval
100 if ((distance_value < max_distance) && (distance_value > min_distance))
101 continue;
102 }
103 else
104 {
105 // Use a threshold for cutting out points which are too close/far away
106 if ((distance_value > max_distance) || (distance_value < min_distance))
107 continue;
108 }
109
110 // Check if the point is invalid
111 if (!isXYZFinite (point))
112 continue;
113 // Create the point structure and get the min/max
114 pcl::Array4fMapConst pt = point.getArray4fMap ();
115 min_p = min_p.min (pt);
116 max_p = max_p.max (pt);
117 }
118 }
119 min_pt = min_p;
120 max_pt = max_p;
121}
122
123///////////////////////////////////////////////////////////////////////////////////////////
124template <typename PointT> void
126 const Indices &indices,
127 const std::string &distance_field_name, float min_distance, float max_distance,
128 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative)
129{
130 Eigen::Array4f min_p, max_p;
131 min_p.setConstant (FLT_MAX);
132 max_p.setConstant (-FLT_MAX);
133
134 // Get the fields list and the distance field index
135 std::vector<pcl::PCLPointField> fields;
136 int distance_idx = pcl::getFieldIndex<PointT> (distance_field_name, fields);
137
138 float distance_value;
139 // If dense, no need to check for NaNs
140 if (cloud->is_dense)
141 {
142 for (const auto &index : indices)
143 {
144 // Get the distance value
145 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud)[index]);
146 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
147
148 if (limit_negative)
149 {
150 // Use a threshold for cutting out points which inside the interval
151 if ((distance_value < max_distance) && (distance_value > min_distance))
152 continue;
153 }
154 else
155 {
156 // Use a threshold for cutting out points which are too close/far away
157 if ((distance_value > max_distance) || (distance_value < min_distance))
158 continue;
159 }
160 // Create the point structure and get the min/max
161 pcl::Array4fMapConst pt = (*cloud)[index].getArray4fMap ();
162 min_p = min_p.min (pt);
163 max_p = max_p.max (pt);
164 }
165 }
166 else
167 {
168 for (const auto &index : indices)
169 {
170 // Get the distance value
171 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*cloud)[index]);
172 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
173
174 if (limit_negative)
175 {
176 // Use a threshold for cutting out points which inside the interval
177 if ((distance_value < max_distance) && (distance_value > min_distance))
178 continue;
179 }
180 else
181 {
182 // Use a threshold for cutting out points which are too close/far away
183 if ((distance_value > max_distance) || (distance_value < min_distance))
184 continue;
185 }
186
187 // Check if the point is invalid
188 if (!std::isfinite ((*cloud)[index].x) ||
189 !std::isfinite ((*cloud)[index].y) ||
190 !std::isfinite ((*cloud)[index].z))
191 continue;
192 // Create the point structure and get the min/max
193 pcl::Array4fMapConst pt = (*cloud)[index].getArray4fMap ();
194 min_p = min_p.min (pt);
195 max_p = max_p.max (pt);
196 }
197 }
198 min_pt = min_p;
199 max_pt = max_p;
200}
201
203{
204 unsigned int idx;
205 unsigned int cloud_point_index;
206
208 cloud_point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
209 bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); }
210};
211
212//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
213template <typename PointT> void
215{
216 // Has the input dataset been set already?
217 if (!input_)
218 {
219 PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
220 output.width = output.height = 0;
221 output.clear ();
222 return;
223 }
224
225 // Copy the header (and thus the frame_id) + allocate enough space for points
226 output.height = 1; // downsampling breaks the organized structure
227 output.is_dense = true; // we filter out invalid points
228
229 Eigen::Vector4f min_p, max_p;
230 // Get the minimum and maximum dimensions
231 if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
232 getMinMax3D<PointT> (input_, *indices_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
233 else
234 getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
235
236 // Check that the leaf size is not too small, given the size of the data
237 std::int64_t dx = static_cast<std::int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
238 std::int64_t dy = static_cast<std::int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
239 std::int64_t dz = static_cast<std::int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
240
241 if ((dx*dy*dz) > static_cast<std::int64_t>(std::numeric_limits<std::int32_t>::max()))
242 {
243 PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName().c_str());
244 output = *input_;
245 return;
246 }
247
248 // Compute the minimum and maximum bounding box values
249 min_b_[0] = static_cast<int> (std::floor (min_p[0] * inverse_leaf_size_[0]));
250 max_b_[0] = static_cast<int> (std::floor (max_p[0] * inverse_leaf_size_[0]));
251 min_b_[1] = static_cast<int> (std::floor (min_p[1] * inverse_leaf_size_[1]));
252 max_b_[1] = static_cast<int> (std::floor (max_p[1] * inverse_leaf_size_[1]));
253 min_b_[2] = static_cast<int> (std::floor (min_p[2] * inverse_leaf_size_[2]));
254 max_b_[2] = static_cast<int> (std::floor (max_p[2] * inverse_leaf_size_[2]));
255
256 // Compute the number of divisions needed along all axis
257 div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
258 div_b_[3] = 0;
259
260 // Set up the division multiplier
261 divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
262
263 // Storage for mapping leaf and pointcloud indexes
264 std::vector<cloud_point_index_idx> index_vector;
265 index_vector.reserve (indices_->size ());
266
267 // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
268 if (!filter_field_name_.empty ())
269 {
270 // Get the distance field index
271 std::vector<pcl::PCLPointField> fields;
272 int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
273 if (distance_idx == -1)
274 PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
275
276 // First pass: go over all points and insert them into the index_vector vector
277 // with calculated idx. Points with the same idx value will contribute to the
278 // same point of resulting CloudPoint
279 for (const auto& index : (*indices_))
280 {
281 if (!input_->is_dense)
282 // Check if the point is invalid
283 if (!isXYZFinite ((*input_)[index]))
284 continue;
285
286 // Get the distance value
287 const std::uint8_t* pt_data = reinterpret_cast<const std::uint8_t*> (&(*input_)[index]);
288 float distance_value = 0;
289 memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
290
291 if (filter_limit_negative_)
292 {
293 // Use a threshold for cutting out points which inside the interval
294 if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
295 continue;
296 }
297 else
298 {
299 // Use a threshold for cutting out points which are too close/far away
300 if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
301 continue;
302 }
303
304 int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
305 int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
306 int ijk2 = static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
307
308 // Compute the centroid leaf index
309 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
310 index_vector.emplace_back(static_cast<unsigned int> (idx), index);
311 }
312 }
313 // No distance filtering, process all data
314 else
315 {
316 // First pass: go over all points and insert them into the index_vector vector
317 // with calculated idx. Points with the same idx value will contribute to the
318 // same point of resulting CloudPoint
319 for (const auto& index : (*indices_))
320 {
321 if (!input_->is_dense)
322 // Check if the point is invalid
323 if (!isXYZFinite ((*input_)[index]))
324 continue;
325
326 int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
327 int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
328 int ijk2 = static_cast<int> (std::floor ((*input_)[index].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
329
330 // Compute the centroid leaf index
331 int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
332 index_vector.emplace_back(static_cast<unsigned int> (idx), index);
333 }
334 }
335
336 // Second pass: sort the index_vector vector using value representing target cell as index
337 // in effect all points belonging to the same output cell will be next to each other
338 auto rightshift_func = [](const cloud_point_index_idx &x, const unsigned offset) { return x.idx >> offset; };
339 boost::sort::spreadsort::integer_sort(index_vector.begin(), index_vector.end(), rightshift_func);
340
341 // Third pass: count output cells
342 // we need to skip all the same, adjacent idx values
343 unsigned int total = 0;
344 unsigned int index = 0;
345 // first_and_last_indices_vector[i] represents the index in index_vector of the first point in
346 // index_vector belonging to the voxel which corresponds to the i-th output point,
347 // and of the first point not belonging to.
348 std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
349 // Worst case size
350 first_and_last_indices_vector.reserve (index_vector.size ());
351 while (index < index_vector.size ())
352 {
353 unsigned int i = index + 1;
354 while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
355 ++i;
356 if (i - index >= min_points_per_voxel_)
357 {
358 ++total;
359 first_and_last_indices_vector.emplace_back(index, i);
360 }
361 index = i;
362 }
363
364 // Fourth pass: compute centroids, insert them into their final position
365 output.resize (total);
366 if (save_leaf_layout_)
367 {
368 try
369 {
370 // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it needs to be re-initialized to -1
371 std::uint32_t new_layout_size = div_b_[0]*div_b_[1]*div_b_[2];
372 //This is the number of elements that need to be re-initialized to -1
373 std::uint32_t reinit_size = std::min (static_cast<unsigned int> (new_layout_size), static_cast<unsigned int> (leaf_layout_.size()));
374 for (std::uint32_t i = 0; i < reinit_size; i++)
375 {
376 leaf_layout_[i] = -1;
377 }
378 leaf_layout_.resize (new_layout_size, -1);
379 }
380 catch (std::bad_alloc&)
381 {
382 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
383 "voxel_grid.hpp", "applyFilter");
384 }
385 catch (std::length_error&)
386 {
387 throw PCLException("VoxelGrid bin size is too low; impossible to allocate memory for layout",
388 "voxel_grid.hpp", "applyFilter");
389 }
390 }
391
392 index = 0;
393 for (const auto &cp : first_and_last_indices_vector)
394 {
395 // calculate centroid - sum values from all input points, that have the same idx value in index_vector array
396 unsigned int first_index = cp.first;
397 unsigned int last_index = cp.second;
398
399 // index is centroid final position in resulting PointCloud
400 if (save_leaf_layout_)
401 leaf_layout_[index_vector[first_index].idx] = index;
402
403 //Limit downsampling to coords
404 if (!downsample_all_data_)
405 {
406 Eigen::Vector4f centroid (Eigen::Vector4f::Zero ());
407
408 for (unsigned int li = first_index; li < last_index; ++li)
409 centroid += (*input_)[index_vector[li].cloud_point_index].getVector4fMap ();
410
411 centroid /= static_cast<float> (last_index - first_index);
412 output[index].getVector4fMap () = centroid;
413 }
414 else
415 {
416 CentroidPoint<PointT> centroid;
417
418 // fill in the accumulator with leaf points
419 for (unsigned int li = first_index; li < last_index; ++li)
420 centroid.add ((*input_)[index_vector[li].cloud_point_index]);
421
422 centroid.get (output[index]);
423 }
424
425 ++index;
426 }
427 output.width = output.size ();
428}
429
430#define PCL_INSTANTIATE_VoxelGrid(T) template class PCL_EXPORTS pcl::VoxelGrid<T>;
431#define PCL_INSTANTIATE_getMinMax3D(T) template PCL_EXPORTS void pcl::getMinMax3D<T> (const pcl::PointCloud<T>::ConstPtr &, const std::string &, float, float, Eigen::Vector4f &, Eigen::Vector4f &, bool);
432
433#endif // PCL_FILTERS_IMPL_VOXEL_GRID_H_
434
Define methods for centroid estimation and covariance matrix calculus.
A generic class that computes the centroid of points fed to it.
Definition centroid.h:1023
void get(PointOutT &point) const
Retrieve the current centroid.
Definition centroid.hpp:867
void add(const PointT &point)
Add a new point to the centroid computation.
Definition centroid.hpp:858
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition exceptions.h:64
shared_ptr< const PointCloud< PointT > > ConstPtr
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
typename Filter< PointT >::PointCloud PointCloud
Definition voxel_grid.h:184
Define standard C methods and C++ classes that are common to all methods.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
Definition common.hpp:295
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Definition io.hpp:54
const Eigen::Map< const Eigen::Array4f, Eigen::Aligned > Array4fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
constexpr bool isXYZFinite(const PointT &) noexcept
unsigned int cloud_point_index
cloud_point_index_idx()=default
bool operator<(const cloud_point_index_idx &p) const
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)