Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
voxel_grid.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 *
38 */
39
40#pragma once
41
42#include <pcl/filters/filter.h>
43#include <cfloat> // for FLT_MAX
44
45namespace pcl
46{
47 /** \brief Obtain the maximum and minimum points in 3D from a given point cloud.
48 * \param[in] cloud the pointer to a pcl::PCLPointCloud2 dataset
49 * \param[in] x_idx the index of the X channel
50 * \param[in] y_idx the index of the Y channel
51 * \param[in] z_idx the index of the Z channel
52 * \param[out] min_pt the minimum data point
53 * \param[out] max_pt the maximum data point
54 */
55 PCL_EXPORTS void
57 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
58
59 /** \brief Obtain the maximum and minimum points in 3D from a given point cloud.
60 * \note Performs internal data filtering as well.
61 * \param[in] cloud the pointer to a pcl::PCLPointCloud2 dataset
62 * \param[in] x_idx the index of the X channel
63 * \param[in] y_idx the index of the Y channel
64 * \param[in] z_idx the index of the Z channel
65 * \param[in] distance_field_name the name of the dimension to filter data along to
66 * \param[in] min_distance the minimum acceptable value in \a distance_field_name data
67 * \param[in] max_distance the maximum acceptable value in \a distance_field_name data
68 * \param[out] min_pt the minimum data point
69 * \param[out] max_pt the maximum data point
70 * \param[in] limit_negative \b false if data \b inside of the [min_distance; max_distance] interval should be
71 * considered, \b true otherwise.
72 */
73 PCL_EXPORTS void
75 const std::string &distance_field_name, float min_distance, float max_distance,
76 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
77
78 /** \brief Get the relative cell indices of the "upper half" 13 neighbors.
79 * \note Useful in combination with getNeighborCentroidIndices() from \ref VoxelGrid
80 * \ingroup filters
81 */
82 inline Eigen::MatrixXi
84 {
85 Eigen::MatrixXi relative_coordinates (3, 13);
86 int idx = 0;
87
88 // 0 - 8
89 for (int i = -1; i < 2; i++)
90 {
91 for (int j = -1; j < 2; j++)
92 {
93 relative_coordinates (0, idx) = i;
94 relative_coordinates (1, idx) = j;
95 relative_coordinates (2, idx) = -1;
96 idx++;
97 }
98 }
99 // 9 - 11
100 for (int i = -1; i < 2; i++)
101 {
102 relative_coordinates (0, idx) = i;
103 relative_coordinates (1, idx) = -1;
104 relative_coordinates (2, idx) = 0;
105 idx++;
106 }
107 // 12
108 relative_coordinates (0, idx) = -1;
109 relative_coordinates (1, idx) = 0;
110 relative_coordinates (2, idx) = 0;
111
112 return (relative_coordinates);
113 }
114
115 /** \brief Get the relative cell indices of all the 26 neighbors.
116 * \note Useful in combination with getNeighborCentroidIndices() from \ref VoxelGrid
117 * \ingroup filters
118 */
119 inline Eigen::MatrixXi
121 {
123 Eigen::MatrixXi relative_coordinates_all( 3, 26);
125 relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates;
127 }
128
129 /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions
130 * in a given pointcloud, without considering points outside of a distance threshold from the laser origin
131 * \param[in] cloud the point cloud data message
132 * \param[in] distance_field_name the field name that contains the distance values
133 * \param[in] min_distance the minimum distance a point will be considered from
134 * \param[in] max_distance the maximum distance a point will be considered to
135 * \param[out] min_pt the resultant minimum bounds
136 * \param[out] max_pt the resultant maximum bounds
137 * \param[in] limit_negative if set to true, then all points outside of the interval (min_distance;max_distace) are considered
138 * \ingroup filters
139 */
140 template <typename PointT> void
141 getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
142 const std::string &distance_field_name, float min_distance, float max_distance,
143 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
144
145 /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions
146 * in a given pointcloud, without considering points outside of a distance threshold from the laser origin
147 * \param[in] cloud the point cloud data message
148 * \param[in] indices the vector of indices to use
149 * \param[in] distance_field_name the field name that contains the distance values
150 * \param[in] min_distance the minimum distance a point will be considered from
151 * \param[in] max_distance the maximum distance a point will be considered to
152 * \param[out] min_pt the resultant minimum bounds
153 * \param[out] max_pt the resultant maximum bounds
154 * \param[in] limit_negative if set to true, then all points outside of the interval (min_distance;max_distace) are considered
155 * \ingroup filters
156 */
157 template <typename PointT> void
158 getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
159 const Indices &indices,
160 const std::string &distance_field_name, float min_distance, float max_distance,
161 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
162
163 /** \brief VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
164 *
165 * The VoxelGrid class creates a *3D voxel grid* (think about a voxel
166 * grid as a set of tiny 3D boxes in space) over the input point cloud data.
167 * Then, in each *voxel* (i.e., 3D box), all the points present will be
168 * approximated (i.e., *downsampled*) with their centroid. This approach is
169 * a bit slower than approximating them with the center of the voxel, but it
170 * represents the underlying surface more accurately.
171 *
172 * \author Radu B. Rusu, Bastian Steder
173 * \ingroup filters
174 */
175 template <typename PointT>
176 class VoxelGrid: public Filter<PointT>
177 {
178 protected:
181 using Filter<PointT>::input_;
183
187
188 public:
189
192
193 /** \brief Empty constructor. */
195 leaf_size_ (Eigen::Vector4f::Zero ()),
196 inverse_leaf_size_ (Eigen::Array4f::Zero ()),
199 min_b_ (Eigen::Vector4i::Zero ()),
200 max_b_ (Eigen::Vector4i::Zero ()),
201 div_b_ (Eigen::Vector4i::Zero ()),
202 divb_mul_ (Eigen::Vector4i::Zero ()),
208 {
209 filter_name_ = "VoxelGrid";
210 }
211
212 /** \brief Destructor. */
214 {
215 }
216
217 /** \brief Set the voxel grid leaf size.
218 * \param[in] leaf_size the voxel grid leaf size
219 */
220 inline void
221 setLeafSize (const Eigen::Vector4f &leaf_size)
222 {
224 // Avoid division errors
225 if (leaf_size_[3] == 0)
226 leaf_size_[3] = 1;
227 // Use multiplications instead of divisions
228 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
229 }
230
231 /** \brief Set the voxel grid leaf size.
232 * \param[in] lx the leaf size for X
233 * \param[in] ly the leaf size for Y
234 * \param[in] lz the leaf size for Z
235 */
236 inline void
237 setLeafSize (float lx, float ly, float lz)
238 {
239 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
240 // Avoid division errors
241 if (leaf_size_[3] == 0)
242 leaf_size_[3] = 1;
243 // Use multiplications instead of divisions
244 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
245 }
246
247 /** \brief Get the voxel grid leaf size. */
248 inline Eigen::Vector3f
249 getLeafSize () const { return (leaf_size_.head<3> ()); }
250
251 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ.
252 * \param[in] downsample the new value (true/false)
253 */
254 inline void
255 setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
256
257 /** \brief Get the state of the internal downsampling parameter (true if
258 * all fields need to be downsampled, false if just XYZ).
259 */
260 inline bool
262
263 /** \brief Set the minimum number of points required for a voxel to be used.
264 * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
265 */
266 inline void
268
269 /** \brief Return the minimum number of points required for a voxel to be used.
270 */
271 inline unsigned int
273
274 /** \brief Set to true if leaf layout information needs to be saved for later access.
275 * \param[in] save_leaf_layout the new value (true/false)
276 */
277 inline void
279
280 /** \brief Returns true if leaf layout information will to be saved for later access. */
281 inline bool
283
284 /** \brief Get the minimum coordinates of the bounding box (after
285 * filtering is performed).
286 */
287 inline Eigen::Vector3i
288 getMinBoxCoordinates () const { return (min_b_.head<3> ()); }
289
290 /** \brief Get the minimum coordinates of the bounding box (after
291 * filtering is performed).
292 */
293 inline Eigen::Vector3i
294 getMaxBoxCoordinates () const { return (max_b_.head<3> ()); }
295
296 /** \brief Get the number of divisions along all 3 axes (after filtering
297 * is performed).
298 */
299 inline Eigen::Vector3i
300 getNrDivisions () const { return (div_b_.head<3> ()); }
301
302 /** \brief Get the multipliers to be applied to the grid coordinates in
303 * order to find the centroid index (after filtering is performed).
304 */
305 inline Eigen::Vector3i
306 getDivisionMultiplier () const { return (divb_mul_.head<3> ()); }
307
308 /** \brief Returns the index in the resulting downsampled cloud of the specified point.
309 *
310 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering
311 * performed, and that the point is inside the grid, to avoid invalid access (or use
312 * getGridCoordinates+getCentroidIndexAt)
313 *
314 * \param[in] p the point to get the index at
315 */
316 inline int
317 getCentroidIndex (const PointT &p) const
318 {
319 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (std::floor (p.x * inverse_leaf_size_[0])),
320 static_cast<int> (std::floor (p.y * inverse_leaf_size_[1])),
321 static_cast<int> (std::floor (p.z * inverse_leaf_size_[2])), 0) - min_b_).dot (divb_mul_)));
322 }
323
324 /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates,
325 * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds).
326 * \param[in] reference_point the coordinates of the reference point (corresponding cell is allowed to be empty/out of bounds)
327 * \param[in] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell
328 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
329 */
330 inline std::vector<int>
332 {
333 Eigen::Vector4i ijk (static_cast<int> (std::floor (reference_point.x * inverse_leaf_size_[0])),
334 static_cast<int> (std::floor (reference_point.y * inverse_leaf_size_[1])),
335 static_cast<int> (std::floor (reference_point.z * inverse_leaf_size_[2])), 0);
336 Eigen::Array4i diff2min = min_b_ - ijk;
337 Eigen::Array4i diff2max = max_b_ - ijk;
338 std::vector<int> neighbors (relative_coordinates.cols());
339 for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
340 {
341 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
342 // checking if the specified cell is in the grid
343 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
344 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
345 else
346 neighbors[ni] = -1; // cell is out of bounds, consider it empty
347 }
348 return (neighbors);
349 }
350
351 /** \brief Returns the layout of the leafs for fast access to cells relative to current position.
352 * \note position at (i-min_x) + (j-min_y)*div_x + (k-min_z)*div_x*div_y holds the index of the element at coordinates (i,j,k) in the grid (-1 if empty)
353 */
354 inline std::vector<int>
355 getLeafLayout () const { return (leaf_layout_); }
356
357 /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
358 * \param[in] x the X point coordinate to get the (i, j, k) index at
359 * \param[in] y the Y point coordinate to get the (i, j, k) index at
360 * \param[in] z the Z point coordinate to get the (i, j, k) index at
361 */
362 inline Eigen::Vector3i
363 getGridCoordinates (float x, float y, float z) const
364 {
365 return (Eigen::Vector3i (static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
366 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
367 static_cast<int> (std::floor (z * inverse_leaf_size_[2]))));
368 }
369
370 /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates.
371 * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty)
372 */
373 inline int
374 getCentroidIndexAt (const Eigen::Vector3i &ijk) const
375 {
376 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
377 if (idx < 0 || idx >= static_cast<int> (leaf_layout_.size ())) // this checks also if leaf_layout_.size () == 0 i.e. everything was computed as needed
378 {
379 //if (verbose)
380 // PCL_ERROR ("[pcl::%s::getCentroidIndexAt] Specified coordinate is outside grid bounds, or leaf layout is not saved, make sure to call setSaveLeafLayout(true) and filter(output) first!\n", getClassName ().c_str ());
381 return (-1);
382 }
383 return (leaf_layout_[idx]);
384 }
385
386 /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits,
387 * points having values outside this interval will be discarded.
388 * \param[in] field_name the name of the field that contains values used for filtering
389 */
390 inline void
391 setFilterFieldName (const std::string &field_name)
392 {
394 }
395
396 /** \brief Get the name of the field used for filtering. */
397 inline std::string const
399 {
400 return (filter_field_name_);
401 }
402
403 /** \brief Set the field filter limits. All points having field values outside this interval will be discarded.
404 * \param[in] limit_min the minimum allowed field value
405 * \param[in] limit_max the maximum allowed field value
406 */
407 inline void
408 setFilterLimits (const double &limit_min, const double &limit_max)
409 {
412 }
413
414 /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX.
415 * \param[out] limit_min the minimum allowed field value
416 * \param[out] limit_max the maximum allowed field value
417 */
418 inline void
424
425 /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max).
426 * Default: false.
427 * \param[in] limit_negative return data inside the interval (false) or outside (true)
428 */
429 inline void
434
435 /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
436 * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
437 */
438 inline void
443
444 /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
445 * \return true if data \b outside the interval [min; max] is to be returned, false otherwise
446 */
447 inline bool
449 {
450 return (filter_limit_negative_);
451 }
452
453 protected:
454 /** \brief The size of a leaf. */
455 Eigen::Vector4f leaf_size_;
456
457 /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */
458 Eigen::Array4f inverse_leaf_size_;
459
460 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */
462
463 /** \brief Set to true if leaf layout information needs to be saved in \a leaf_layout_. */
465
466 /** \brief The leaf layout information for fast access to cells relative to current position **/
467 std::vector<int> leaf_layout_;
468
469 /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */
470 Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
471
472 /** \brief The desired user filter field name. */
474
475 /** \brief The minimum allowed filter value a point will be considered from. */
477
478 /** \brief The maximum allowed filter value a point will be considered from. */
480
481 /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
483
484 /** \brief Minimum number of points per voxel for the centroid to be computed */
486
487 using FieldList = typename pcl::traits::fieldList<PointT>::type;
488
489 /** \brief Downsample a Point Cloud using a voxelized grid approach
490 * \param[out] output the resultant point cloud message
491 */
492 void
493 applyFilter (PointCloud &output) override;
494 };
495
496 /** \brief VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
497 *
498 * The VoxelGrid class creates a *3D voxel grid* (think about a voxel
499 * grid as a set of tiny 3D boxes in space) over the input point cloud data.
500 * Then, in each *voxel* (i.e., 3D box), all the points present will be
501 * approximated (i.e., *downsampled*) with their centroid. This approach is
502 * a bit slower than approximating them with the center of the voxel, but it
503 * represents the underlying surface more accurately.
504 *
505 * \author Radu B. Rusu, Bastian Steder, Radoslaw Cybulski
506 * \ingroup filters
507 */
508 template <>
509 class PCL_EXPORTS VoxelGrid<pcl::PCLPointCloud2> : public Filter<pcl::PCLPointCloud2>
510 {
511 using Filter<pcl::PCLPointCloud2>::filter_name_;
512 using Filter<pcl::PCLPointCloud2>::getClassName;
513
515 using PCLPointCloud2Ptr = PCLPointCloud2::Ptr;
516 using PCLPointCloud2ConstPtr = PCLPointCloud2::ConstPtr;
517
518 public:
519 /** \brief Empty constructor. */
521 leaf_size_ (Eigen::Vector4f::Zero ()),
522 inverse_leaf_size_ (Eigen::Array4f::Zero ()),
523 downsample_all_data_ (true),
524 save_leaf_layout_ (false),
525 min_b_ (Eigen::Vector4i::Zero ()),
526 max_b_ (Eigen::Vector4i::Zero ()),
527 div_b_ (Eigen::Vector4i::Zero ()),
528 divb_mul_ (Eigen::Vector4i::Zero ()),
529 filter_field_name_ (""),
530 filter_limit_min_ (-FLT_MAX),
531 filter_limit_max_ (FLT_MAX),
532 filter_limit_negative_ (false),
533 min_points_per_voxel_ (0)
534 {
535 filter_name_ = "VoxelGrid";
536 }
537
538 /** \brief Destructor. */
540 {
541 }
542
543 /** \brief Set the voxel grid leaf size.
544 * \param[in] leaf_size the voxel grid leaf size
545 */
546 inline void
547 setLeafSize (const Eigen::Vector4f &leaf_size)
548 {
549 leaf_size_ = leaf_size;
550 // Avoid division errors
551 if (leaf_size_[3] == 0)
552 leaf_size_[3] = 1;
553 // Use multiplications instead of divisions
554 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
555 }
556
557 /** \brief Set the voxel grid leaf size.
558 * \param[in] lx the leaf size for X
559 * \param[in] ly the leaf size for Y
560 * \param[in] lz the leaf size for Z
561 */
562 inline void
563 setLeafSize (float lx, float ly, float lz)
564 {
565 leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
566 // Avoid division errors
567 if (leaf_size_[3] == 0)
568 leaf_size_[3] = 1;
569 // Use multiplications instead of divisions
570 inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
571 }
572
573 /** \brief Get the voxel grid leaf size. */
574 inline Eigen::Vector3f
575 getLeafSize () const { return (leaf_size_.head<3> ()); }
576
577 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ.
578 * \param[in] downsample the new value (true/false)
579 */
580 inline void
581 setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
582
583 /** \brief Get the state of the internal downsampling parameter (true if
584 * all fields need to be downsampled, false if just XYZ).
585 */
586 inline bool
587 getDownsampleAllData () const { return (downsample_all_data_); }
588
589 /** \brief Set the minimum number of points required for a voxel to be used.
590 * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
591 */
592 inline void
594
595 /** \brief Return the minimum number of points required for a voxel to be used.
596 */
597 inline unsigned int
598 getMinimumPointsNumberPerVoxel () const { return min_points_per_voxel_; }
599
600 /** \brief Set to true if leaf layout information needs to be saved for later access.
601 * \param[in] save_leaf_layout the new value (true/false)
602 */
603 inline void
605
606 /** \brief Returns true if leaf layout information will to be saved for later access. */
607 inline bool
608 getSaveLeafLayout () const { return (save_leaf_layout_); }
609
610 /** \brief Get the minimum coordinates of the bounding box (after
611 * filtering is performed).
612 */
613 inline Eigen::Vector3i
614 getMinBoxCoordinates () const { return (min_b_.head<3> ()); }
615
616 /** \brief Get the minimum coordinates of the bounding box (after
617 * filtering is performed).
618 */
619 inline Eigen::Vector3i
620 getMaxBoxCoordinates () const { return (max_b_.head<3> ()); }
621
622 /** \brief Get the number of divisions along all 3 axes (after filtering
623 * is performed).
624 */
625 inline Eigen::Vector3i
626 getNrDivisions () const { return (div_b_.head<3> ()); }
627
628 /** \brief Get the multipliers to be applied to the grid coordinates in
629 * order to find the centroid index (after filtering is performed).
630 */
631 inline Eigen::Vector3i
632 getDivisionMultiplier () const { return (divb_mul_.head<3> ()); }
633
634 /** \brief Returns the index in the resulting downsampled cloud of the specified point.
635 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed,
636 * and that the point is inside the grid, to avoid invalid access (or use getGridCoordinates+getCentroidIndexAt)
637 * \param[in] x the X point coordinate to get the index at
638 * \param[in] y the Y point coordinate to get the index at
639 * \param[in] z the Z point coordinate to get the index at
640 */
641 inline int
642 getCentroidIndex (float x, float y, float z) const
643 {
644 return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
645 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
646 static_cast<int> (std::floor (z * inverse_leaf_size_[2])),
647 0)
648 - min_b_).dot (divb_mul_)));
649 }
650
651 /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates,
652 * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds).
653 * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
654 * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
655 * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
656 * \param[out] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell
657 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
658 */
659 inline std::vector<int>
660 getNeighborCentroidIndices (float x, float y, float z, const Eigen::MatrixXi &relative_coordinates) const
661 {
662 Eigen::Vector4i ijk (static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
663 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
664 static_cast<int> (std::floor (z * inverse_leaf_size_[2])), 0);
665 Eigen::Array4i diff2min = min_b_ - ijk;
666 Eigen::Array4i diff2max = max_b_ - ijk;
667 std::vector<int> neighbors (relative_coordinates.cols());
668 for (Eigen::Index ni = 0; ni < relative_coordinates.cols (); ni++)
669 {
670 Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
671 // checking if the specified cell is in the grid
672 if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
673 neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
674 else
675 neighbors[ni] = -1; // cell is out of bounds, consider it empty
676 }
677 return (neighbors);
678 }
679
680 /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates,
681 * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds).
682 * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
683 * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
684 * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
685 * \param[out] relative_coordinates vector with the elements being the coordinates of the requested cells, relative to the reference point's cell
686 * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
687 */
688 inline std::vector<int>
689 getNeighborCentroidIndices (float x, float y, float z, const std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > &relative_coordinates) const
690 {
691 Eigen::Vector4i ijk (static_cast<int> (std::floor (x * inverse_leaf_size_[0])), static_cast<int> (std::floor (y * inverse_leaf_size_[1])), static_cast<int> (std::floor (z * inverse_leaf_size_[2])), 0);
692 std::vector<int> neighbors;
693 neighbors.reserve (relative_coordinates.size ());
694 for (const auto &relative_coordinate : relative_coordinates)
695 neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << relative_coordinate, 0).finished() - min_b_).dot (divb_mul_)]);
696 return (neighbors);
697 }
698
699 /** \brief Returns the layout of the leafs for fast access to cells relative to current position.
700 * \note position at (i-min_x) + (j-min_y)*div_x + (k-min_z)*div_x*div_y holds the index of the element at coordinates (i,j,k) in the grid (-1 if empty)
701 */
702 inline std::vector<int>
703 getLeafLayout () const { return (leaf_layout_); }
704
705 /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
706 * \param[in] x the X point coordinate to get the (i, j, k) index at
707 * \param[in] y the Y point coordinate to get the (i, j, k) index at
708 * \param[in] z the Z point coordinate to get the (i, j, k) index at
709 */
710 inline Eigen::Vector3i
711 getGridCoordinates (float x, float y, float z) const
712 {
713 return (Eigen::Vector3i (static_cast<int> (std::floor (x * inverse_leaf_size_[0])),
714 static_cast<int> (std::floor (y * inverse_leaf_size_[1])),
715 static_cast<int> (std::floor (z * inverse_leaf_size_[2]))));
716 }
717
718 /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates.
719 * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty)
720 */
721 inline int
722 getCentroidIndexAt (const Eigen::Vector3i &ijk) const
723 {
724 int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
725 if (idx < 0 || idx >= static_cast<int> (leaf_layout_.size ())) // this checks also if leaf_layout_.size () == 0 i.e. everything was computed as needed
726 {
727 //if (verbose)
728 // PCL_ERROR ("[pcl::%s::getCentroidIndexAt] Specified coordinate is outside grid bounds, or leaf layout is not saved, make sure to call setSaveLeafLayout(true) and filter(output) first!\n", getClassName ().c_str ());
729 return (-1);
730 }
731 return (leaf_layout_[idx]);
732 }
733
734 /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits,
735 * points having values outside this interval will be discarded.
736 * \param[in] field_name the name of the field that contains values used for filtering
737 */
738 inline void
739 setFilterFieldName (const std::string &field_name)
740 {
741 filter_field_name_ = field_name;
742 }
743
744 /** \brief Get the name of the field used for filtering. */
745 inline std::string const
747 {
748 return (filter_field_name_);
749 }
750
751 /** \brief Set the field filter limits. All points having field values outside this interval will be discarded.
752 * \param[in] limit_min the minimum allowed field value
753 * \param[in] limit_max the maximum allowed field value
754 */
755 inline void
756 setFilterLimits (const double &limit_min, const double &limit_max)
757 {
758 filter_limit_min_ = limit_min;
759 filter_limit_max_ = limit_max;
760 }
761
762 /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX.
763 * \param[out] limit_min the minimum allowed field value
764 * \param[out] limit_max the maximum allowed field value
765 */
766 inline void
767 getFilterLimits (double &limit_min, double &limit_max) const
768 {
769 limit_min = filter_limit_min_;
770 limit_max = filter_limit_max_;
771 }
772
773 /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max).
774 * Default: false.
775 * \param[in] limit_negative return data inside the interval (false) or outside (true)
776 */
777 inline void
779 {
780 filter_limit_negative_ = limit_negative;
781 }
782
783 /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
784 * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
785 */
786 inline void
788 {
789 limit_negative = filter_limit_negative_;
790 }
791
792 /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
793 * \return true if data \b outside the interval [min; max] is to be returned, false otherwise
794 */
795 inline bool
797 {
798 return (filter_limit_negative_);
799 }
800
801 protected:
802 /** \brief The size of a leaf. */
803 Eigen::Vector4f leaf_size_;
804
805 /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */
806 Eigen::Array4f inverse_leaf_size_;
807
808 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */
810
811 /** \brief Set to true if leaf layout information needs to be saved in \a
812 * leaf_layout.
813 */
815
816 /** \brief The leaf layout information for fast access to cells relative
817 * to current position
818 */
819 std::vector<int> leaf_layout_;
820
821 /** \brief The minimum and maximum bin coordinates, the number of
822 * divisions, and the division multiplier.
823 */
824 Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
825
826 /** \brief The desired user filter field name. */
828
829 /** \brief The minimum allowed filter value a point will be considered from. */
831
832 /** \brief The maximum allowed filter value a point will be considered from. */
834
835 /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
837
838 /** \brief Minimum number of points per voxel for the centroid to be computed */
840
841 /** \brief Downsample a Point Cloud using a voxelized grid approach
842 * \param[out] output the resultant point cloud
843 */
844 void
846 };
847}
848
849#ifdef PCL_NO_PRECOMPILE
850#include <pcl/filters/impl/voxel_grid.hpp>
851#endif
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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
int getCentroidIndexAt(const Eigen::Vector3i &ijk) const
Returns the index in the downsampled cloud corresponding to a given set of coordinates.
Definition voxel_grid.h:722
Eigen::Vector3i getDivisionMultiplier() const
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
Definition voxel_grid.h:632
bool getFilterLimitsNegative() const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
Definition voxel_grid.h:796
std::string filter_field_name_
The desired user filter field name.
Definition voxel_grid.h:827
std::vector< int > leaf_layout_
The leaf layout information for fast access to cells relative to current position.
Definition voxel_grid.h:819
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition voxel_grid.h:581
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
Definition voxel_grid.h:756
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
Definition voxel_grid.h:575
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition voxel_grid.h:809
std::vector< int > getNeighborCentroidIndices(float x, float y, float z, const std::vector< Eigen::Vector3i, Eigen::aligned_allocator< Eigen::Vector3i > > &relative_coordinates) const
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
Definition voxel_grid.h:689
Eigen::Vector3i getNrDivisions() const
Get the number of divisions along all 3 axes (after filtering is performed).
Definition voxel_grid.h:626
Eigen::Vector3i getMinBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
Definition voxel_grid.h:614
void applyFilter(PCLPointCloud2 &output) override
Downsample a Point Cloud using a voxelized grid approach.
unsigned int getMinimumPointsNumberPerVoxel() const
Return the minimum number of points required for a voxel to be used.
Definition voxel_grid.h:598
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:547
unsigned int min_points_per_voxel_
Minimum number of points per voxel for the centroid to be computed.
Definition voxel_grid.h:839
void getFilterLimits(double &limit_min, double &limit_max) const
Get the field filter limits (min/max) set by the user.
Definition voxel_grid.h:767
bool filter_limit_negative_
Set to true if we want to return the data outside (filter_limit_min_;filter_limit_max_).
Definition voxel_grid.h:836
std::vector< int > getNeighborCentroidIndices(float x, float y, float z, const Eigen::MatrixXi &relative_coordinates) const
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
Definition voxel_grid.h:660
Eigen::Vector3i getGridCoordinates(float x, float y, float z) const
Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
Definition voxel_grid.h:711
bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled,...
Definition voxel_grid.h:587
bool getSaveLeafLayout() const
Returns true if leaf layout information will to be saved for later access.
Definition voxel_grid.h:608
std::vector< int > getLeafLayout() const
Returns the layout of the leafs for fast access to cells relative to current position.
Definition voxel_grid.h:703
std::string const getFilterFieldName() const
Get the name of the field used for filtering.
Definition voxel_grid.h:746
double filter_limit_max_
The maximum allowed filter value a point will be considered from.
Definition voxel_grid.h:833
Eigen::Vector3i getMaxBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
Definition voxel_grid.h:620
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
Definition voxel_grid.h:739
void getFilterLimitsNegative(bool &limit_negative) const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
Definition voxel_grid.h:787
double filter_limit_min_
The minimum allowed filter value a point will be considered from.
Definition voxel_grid.h:830
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout.
Definition voxel_grid.h:814
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition voxel_grid.h:604
void setFilterLimitsNegative(const bool limit_negative)
Set to true if we want to return the data outside the interval specified by setFilterLimits (min,...
Definition voxel_grid.h:778
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
Definition voxel_grid.h:563
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition voxel_grid.h:806
void setMinimumPointsNumberPerVoxel(unsigned int min_points_per_voxel)
Set the minimum number of points required for a voxel to be used.
Definition voxel_grid.h:593
int getCentroidIndex(float x, float y, float z) const
Returns the index in the resulting downsampled cloud of the specified point.
Definition voxel_grid.h:642
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition voxel_grid.h:803
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
Definition voxel_grid.h:177
Eigen::Vector3i getGridCoordinates(float x, float y, float z) const
Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
Definition voxel_grid.h:363
void setFilterFieldName(const std::string &field_name)
Provide the name of the field to be used for filtering data.
Definition voxel_grid.h:391
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition voxel_grid.h:255
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition voxel_grid.h:461
Eigen::Vector3i getDivisionMultiplier() const
Get the multipliers to be applied to the grid coordinates in order to find the centroid index (after ...
Definition voxel_grid.h:306
bool getFilterLimitsNegative() const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
Definition voxel_grid.h:448
void setFilterLimitsNegative(const bool limit_negative)
Set to true if we want to return the data outside the interval specified by setFilterLimits (min,...
Definition voxel_grid.h:430
Eigen::Vector3i getMaxBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
Definition voxel_grid.h:294
Eigen::Vector4i max_b_
Definition voxel_grid.h:470
unsigned int min_points_per_voxel_
Minimum number of points per voxel for the centroid to be computed.
Definition voxel_grid.h:485
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
unsigned int getMinimumPointsNumberPerVoxel() const
Return the minimum number of points required for a voxel to be used.
Definition voxel_grid.h:272
bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled,...
Definition voxel_grid.h:261
std::vector< int > leaf_layout_
The leaf layout information for fast access to cells relative to current position.
Definition voxel_grid.h:467
void getFilterLimits(double &limit_min, double &limit_max) const
Get the field filter limits (min/max) set by the user.
Definition voxel_grid.h:419
Eigen::Vector4i divb_mul_
Definition voxel_grid.h:470
typename pcl::traits::fieldList< PointT >::type FieldList
Definition voxel_grid.h:487
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
Definition voxel_grid.h:317
Eigen::Vector3i getNrDivisions() const
Get the number of divisions along all 3 axes (after filtering is performed).
Definition voxel_grid.h:300
bool save_leaf_layout_
Set to true if leaf layout information needs to be saved in leaf_layout_.
Definition voxel_grid.h:464
VoxelGrid()
Empty constructor.
Definition voxel_grid.h:194
bool filter_limit_negative_
Set to true if we want to return the data outside (filter_limit_min_;filter_limit_max_).
Definition voxel_grid.h:482
~VoxelGrid()
Destructor.
Definition voxel_grid.h:213
typename PointCloud::ConstPtr PointCloudConstPtr
Definition voxel_grid.h:186
int getCentroidIndexAt(const Eigen::Vector3i &ijk) const
Returns the index in the downsampled cloud corresponding to a given set of coordinates.
Definition voxel_grid.h:374
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:221
Eigen::Vector4i min_b_
The minimum and maximum bin coordinates, the number of divisions, and the division multiplier.
Definition voxel_grid.h:470
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
Definition voxel_grid.h:278
double filter_limit_max_
The maximum allowed filter value a point will be considered from.
Definition voxel_grid.h:479
void setMinimumPointsNumberPerVoxel(unsigned int min_points_per_voxel)
Set the minimum number of points required for a voxel to be used.
Definition voxel_grid.h:267
typename Filter< PointT >::PointCloud PointCloud
Definition voxel_grid.h:184
std::string filter_field_name_
The desired user filter field name.
Definition voxel_grid.h:473
std::vector< int > getLeafLayout() const
Returns the layout of the leafs for fast access to cells relative to current position.
Definition voxel_grid.h:355
Eigen::Vector4f leaf_size_
The size of a leaf.
Definition voxel_grid.h:455
double filter_limit_min_
The minimum allowed filter value a point will be considered from.
Definition voxel_grid.h:476
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
Definition voxel_grid.h:249
std::vector< int > getNeighborCentroidIndices(const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates) const
Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinate...
Definition voxel_grid.h:331
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
Definition voxel_grid.h:237
std::string const getFilterFieldName() const
Get the name of the field used for filtering.
Definition voxel_grid.h:398
Eigen::Vector3i getMinBoxCoordinates() const
Get the minimum coordinates of the bounding box (after filtering is performed).
Definition voxel_grid.h:288
Eigen::Array4f inverse_leaf_size_
Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons.
Definition voxel_grid.h:458
bool getSaveLeafLayout() const
Returns true if leaf layout information will to be saved for later access.
Definition voxel_grid.h:282
Eigen::Vector4i div_b_
Definition voxel_grid.h:470
void getFilterLimitsNegative(bool &limit_negative) const
Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
Definition voxel_grid.h:439
void setFilterLimits(const double &limit_min, const double &limit_max)
Set the field filter limits.
Definition voxel_grid.h:408
typename PointCloud::Ptr PointCloudPtr
Definition voxel_grid.h:185
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
Eigen::MatrixXi getAllNeighborCellIndices()
Get the relative cell indices of all the 26 neighbors.
Definition voxel_grid.h:120
Eigen::MatrixXi getHalfNeighborCellIndices()
Get the relative cell indices of the "upper half" 13 neighbors.
Definition voxel_grid.h:83
Definition bfgs.h:10
PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
shared_ptr< const ::pcl::PCLPointCloud2 > ConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.