Point Cloud Library (PCL)  1.7.0
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 #ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
41 #define PCL_FILTERS_VOXEL_GRID_MAP_H_
42 
43 #include <pcl/filters/boost.h>
44 #include <pcl/filters/filter.h>
45 #include <map>
46 
47 namespace pcl
48 {
49  /** \brief Obtain the maximum and minimum points in 3D from a given point cloud.
50  * \param[in] cloud the pointer to a pcl::PCLPointCloud2 dataset
51  * \param[in] x_idx the index of the X channel
52  * \param[in] y_idx the index of the Y channel
53  * \param[in] z_idx the index of the Z channel
54  * \param[out] min_pt the minimum data point
55  * \param[out] max_pt the maximum data point
56  */
57  PCL_EXPORTS void
58  getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
59  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
60 
61  /** \brief Obtain the maximum and minimum points in 3D from a given point cloud.
62  * \note Performs internal data filtering as well.
63  * \param[in] cloud the pointer to a pcl::PCLPointCloud2 dataset
64  * \param[in] x_idx the index of the X channel
65  * \param[in] y_idx the index of the Y channel
66  * \param[in] z_idx the index of the Z channel
67  * \param[in] distance_field_name the name of the dimension to filter data along to
68  * \param[in] min_distance the minimum acceptable value in \a distance_field_name data
69  * \param[in] max_distance the maximum acceptable value in \a distance_field_name data
70  * \param[out] min_pt the minimum data point
71  * \param[out] max_pt the maximum data point
72  * \param[in] limit_negative \b false if data \b inside of the [min_distance; max_distance] interval should be
73  * considered, \b true otherwise.
74  */
75  PCL_EXPORTS void
76  getMinMax3D (const pcl::PCLPointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
77  const std::string &distance_field_name, float min_distance, float max_distance,
78  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
79 
80  /** \brief Get the relative cell indices of the "upper half" 13 neighbors.
81  * \note Useful in combination with getNeighborCentroidIndices() from \ref VoxelGrid
82  * \ingroup filters
83  */
84  inline Eigen::MatrixXi
86  {
87  Eigen::MatrixXi relative_coordinates (3, 13);
88  int idx = 0;
89 
90  // 0 - 8
91  for (int i = -1; i < 2; i++)
92  {
93  for (int j = -1; j < 2; j++)
94  {
95  relative_coordinates (0, idx) = i;
96  relative_coordinates (1, idx) = j;
97  relative_coordinates (2, idx) = -1;
98  idx++;
99  }
100  }
101  // 9 - 11
102  for (int i = -1; i < 2; i++)
103  {
104  relative_coordinates (0, idx) = i;
105  relative_coordinates (1, idx) = -1;
106  relative_coordinates (2, idx) = 0;
107  idx++;
108  }
109  // 12
110  relative_coordinates (0, idx) = -1;
111  relative_coordinates (1, idx) = 0;
112  relative_coordinates (2, idx) = 0;
113 
114  return (relative_coordinates);
115  }
116 
117  /** \brief Get the relative cell indices of all the 26 neighbors.
118  * \note Useful in combination with getNeighborCentroidIndices() from \ref VoxelGrid
119  * \ingroup filters
120  */
121  inline Eigen::MatrixXi
123  {
124  Eigen::MatrixXi relative_coordinates = getHalfNeighborCellIndices ();
125  Eigen::MatrixXi relative_coordinates_all( 3, 26);
126  relative_coordinates_all.block<3, 13> (0, 0) = relative_coordinates;
127  relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates;
128  return (relative_coordinates_all);
129  }
130 
131  /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions
132  * in a given pointcloud, without considering points outside of a distance threshold from the laser origin
133  * \param[in] cloud the point cloud data message
134  * \param[in] distance_field_name the field name that contains the distance values
135  * \param[in] min_distance the minimum distance a point will be considered from
136  * \param[in] max_distance the maximum distance a point will be considered to
137  * \param[out] min_pt the resultant minimum bounds
138  * \param[out] max_pt the resultant maximum bounds
139  * \param[in] limit_negative if set to true, then all points outside of the interval (min_distance;max_distace) are considered
140  * \ingroup filters
141  */
142  template <typename PointT> void
143  getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
144  const std::string &distance_field_name, float min_distance, float max_distance,
145  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
146 
147  /** \brief Get the minimum and maximum values on each of the 3 (x-y-z) dimensions
148  * in a given pointcloud, without considering points outside of a distance threshold from the laser origin
149  * \param[in] cloud the point cloud data message
150  * \param[in] indices the vector of indices to use
151  * \param[in] distance_field_name the field name that contains the distance values
152  * \param[in] min_distance the minimum distance a point will be considered from
153  * \param[in] max_distance the maximum distance a point will be considered to
154  * \param[out] min_pt the resultant minimum bounds
155  * \param[out] max_pt the resultant maximum bounds
156  * \param[in] limit_negative if set to true, then all points outside of the interval (min_distance;max_distace) are considered
157  * \ingroup filters
158  */
159  template <typename PointT> void
160  getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
161  const std::vector<int> &indices,
162  const std::string &distance_field_name, float min_distance, float max_distance,
163  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
164 
165  /** \brief VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
166  *
167  * The VoxelGrid class creates a *3D voxel grid* (think about a voxel
168  * grid as a set of tiny 3D boxes in space) over the input point cloud data.
169  * Then, in each *voxel* (i.e., 3D box), all the points present will be
170  * approximated (i.e., *downsampled*) with their centroid. This approach is
171  * a bit slower than approximating them with the center of the voxel, but it
172  * represents the underlying surface more accurately.
173  *
174  * \author Radu B. Rusu, Bastian Steder
175  * \ingroup filters
176  */
177  template <typename PointT>
178  class VoxelGrid: public Filter<PointT>
179  {
180  protected:
185 
187  typedef typename PointCloud::Ptr PointCloudPtr;
189  typedef boost::shared_ptr< VoxelGrid<PointT> > Ptr;
190  typedef boost::shared_ptr< const VoxelGrid<PointT> > ConstPtr;
191 
192 
193  public:
194  /** \brief Empty constructor. */
195  VoxelGrid () :
196  leaf_size_ (Eigen::Vector4f::Zero ()),
197  inverse_leaf_size_ (Eigen::Array4f::Zero ()),
198  downsample_all_data_ (true),
199  save_leaf_layout_ (false),
200  leaf_layout_ (),
201  min_b_ (Eigen::Vector4i::Zero ()),
202  max_b_ (Eigen::Vector4i::Zero ()),
203  div_b_ (Eigen::Vector4i::Zero ()),
204  divb_mul_ (Eigen::Vector4i::Zero ()),
205  filter_field_name_ (""),
206  filter_limit_min_ (-FLT_MAX),
207  filter_limit_max_ (FLT_MAX),
208  filter_limit_negative_ (false)
209  {
210  filter_name_ = "VoxelGrid";
211  }
212 
213  /** \brief Destructor. */
214  virtual ~VoxelGrid ()
215  {
216  }
217 
218  /** \brief Set the voxel grid leaf size.
219  * \param[in] leaf_size the voxel grid leaf size
220  */
221  inline void
222  setLeafSize (const Eigen::Vector4f &leaf_size)
223  {
224  leaf_size_ = leaf_size;
225  // Avoid division errors
226  if (leaf_size_[3] == 0)
227  leaf_size_[3] = 1;
228  // Use multiplications instead of divisions
229  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
230  }
231 
232  /** \brief Set the voxel grid leaf size.
233  * \param[in] lx the leaf size for X
234  * \param[in] ly the leaf size for Y
235  * \param[in] lz the leaf size for Z
236  */
237  inline void
238  setLeafSize (float lx, float ly, float lz)
239  {
240  leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
241  // Avoid division errors
242  if (leaf_size_[3] == 0)
243  leaf_size_[3] = 1;
244  // Use multiplications instead of divisions
245  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
246  }
247 
248  /** \brief Get the voxel grid leaf size. */
249  inline Eigen::Vector3f
250  getLeafSize () { return (leaf_size_.head<3> ()); }
251 
252  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ.
253  * \param[in] downsample the new value (true/false)
254  */
255  inline void
256  setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
257 
258  /** \brief Get the state of the internal downsampling parameter (true if
259  * all fields need to be downsampled, false if just XYZ).
260  */
261  inline bool
263 
264  /** \brief Set to true if leaf layout information needs to be saved for later access.
265  * \param[in] save_leaf_layout the new value (true/false)
266  */
267  inline void
268  setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; }
269 
270  /** \brief Returns true if leaf layout information will to be saved for later access. */
271  inline bool
273 
274  /** \brief Get the minimum coordinates of the bounding box (after
275  * filtering is performed).
276  */
277  inline Eigen::Vector3i
278  getMinBoxCoordinates () { return (min_b_.head<3> ()); }
279 
280  /** \brief Get the minimum coordinates of the bounding box (after
281  * filtering is performed).
282  */
283  inline Eigen::Vector3i
284  getMaxBoxCoordinates () { return (max_b_.head<3> ()); }
285 
286  /** \brief Get the number of divisions along all 3 axes (after filtering
287  * is performed).
288  */
289  inline Eigen::Vector3i
290  getNrDivisions () { return (div_b_.head<3> ()); }
291 
292  /** \brief Get the multipliers to be applied to the grid coordinates in
293  * order to find the centroid index (after filtering is performed).
294  */
295  inline Eigen::Vector3i
296  getDivisionMultiplier () { return (divb_mul_.head<3> ()); }
297 
298  /** \brief Returns the index in the resulting downsampled cloud of the specified point.
299  *
300  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering
301  * performed, and that the point is inside the grid, to avoid invalid access (or use
302  * getGridCoordinates+getCentroidIndexAt)
303  *
304  * \param[in] p the point to get the index at
305  */
306  inline int
308  {
309  return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (p.x * inverse_leaf_size_[0])),
310  static_cast<int> (floor (p.y * inverse_leaf_size_[1])),
311  static_cast<int> (floor (p.z * inverse_leaf_size_[2])), 0) - min_b_).dot (divb_mul_)));
312  }
313 
314  /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates,
315  * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds).
316  * \param[in] reference_point the coordinates of the reference point (corresponding cell is allowed to be empty/out of bounds)
317  * \param[in] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell
318  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
319  */
320  inline std::vector<int>
321  getNeighborCentroidIndices (const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates)
322  {
323  Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x * inverse_leaf_size_[0])),
324  static_cast<int> (floor (reference_point.y * inverse_leaf_size_[1])),
325  static_cast<int> (floor (reference_point.z * inverse_leaf_size_[2])), 0);
326  Eigen::Array4i diff2min = min_b_ - ijk;
327  Eigen::Array4i diff2max = max_b_ - ijk;
328  std::vector<int> neighbors (relative_coordinates.cols());
329  for (int ni = 0; ni < relative_coordinates.cols (); ni++)
330  {
331  Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
332  // checking if the specified cell is in the grid
333  if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
334  neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
335  else
336  neighbors[ni] = -1; // cell is out of bounds, consider it empty
337  }
338  return (neighbors);
339  }
340 
341  /** \brief Returns the layout of the leafs for fast access to cells relative to current position.
342  * \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)
343  */
344  inline std::vector<int>
345  getLeafLayout () { return (leaf_layout_); }
346 
347  /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
348  * \param[in] x the X point coordinate to get the (i, j, k) index at
349  * \param[in] y the Y point coordinate to get the (i, j, k) index at
350  * \param[in] z the Z point coordinate to get the (i, j, k) index at
351  */
352  inline Eigen::Vector3i
353  getGridCoordinates (float x, float y, float z)
354  {
355  return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
356  static_cast<int> (floor (y * inverse_leaf_size_[1])),
357  static_cast<int> (floor (z * inverse_leaf_size_[2]))));
358  }
359 
360  /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates.
361  * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty)
362  */
363  inline int
364  getCentroidIndexAt (const Eigen::Vector3i &ijk)
365  {
366  int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
367  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
368  {
369  //if (verbose)
370  // 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 ());
371  return (-1);
372  }
373  return (leaf_layout_[idx]);
374  }
375 
376  /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits,
377  * points having values outside this interval will be discarded.
378  * \param[in] field_name the name of the field that contains values used for filtering
379  */
380  inline void
381  setFilterFieldName (const std::string &field_name)
382  {
383  filter_field_name_ = field_name;
384  }
385 
386  /** \brief Get the name of the field used for filtering. */
387  inline std::string const
389  {
390  return (filter_field_name_);
391  }
392 
393  /** \brief Set the field filter limits. All points having field values outside this interval will be discarded.
394  * \param[in] limit_min the minimum allowed field value
395  * \param[in] limit_max the maximum allowed field value
396  */
397  inline void
398  setFilterLimits (const double &limit_min, const double &limit_max)
399  {
400  filter_limit_min_ = limit_min;
401  filter_limit_max_ = limit_max;
402  }
403 
404  /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX.
405  * \param[out] limit_min the minimum allowed field value
406  * \param[out] limit_max the maximum allowed field value
407  */
408  inline void
409  getFilterLimits (double &limit_min, double &limit_max)
410  {
411  limit_min = filter_limit_min_;
412  limit_max = filter_limit_max_;
413  }
414 
415  /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max).
416  * Default: false.
417  * \param[in] limit_negative return data inside the interval (false) or outside (true)
418  */
419  inline void
420  setFilterLimitsNegative (const bool limit_negative)
421  {
422  filter_limit_negative_ = limit_negative;
423  }
424 
425  /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
426  * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
427  */
428  inline void
429  getFilterLimitsNegative (bool &limit_negative)
430  {
431  limit_negative = filter_limit_negative_;
432  }
433 
434  /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
435  * \return true if data \b outside the interval [min; max] is to be returned, false otherwise
436  */
437  inline bool
439  {
440  return (filter_limit_negative_);
441  }
442 
443  protected:
444  /** \brief The size of a leaf. */
445  Eigen::Vector4f leaf_size_;
446 
447  /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */
448  Eigen::Array4f inverse_leaf_size_;
449 
450  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */
452 
453  /** \brief Set to true if leaf layout information needs to be saved in \a leaf_layout_. */
455 
456  /** \brief The leaf layout information for fast access to cells relative to current position **/
457  std::vector<int> leaf_layout_;
458 
459  /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */
460  Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
461 
462  /** \brief The desired user filter field name. */
463  std::string filter_field_name_;
464 
465  /** \brief The minimum allowed filter value a point will be considered from. */
467 
468  /** \brief The maximum allowed filter value a point will be considered from. */
470 
471  /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
473 
475 
476  /** \brief Downsample a Point Cloud using a voxelized grid approach
477  * \param[out] output the resultant point cloud message
478  */
479  void
480  applyFilter (PointCloud &output);
481  };
482 
483  /** \brief VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
484  *
485  * The VoxelGrid class creates a *3D voxel grid* (think about a voxel
486  * grid as a set of tiny 3D boxes in space) over the input point cloud data.
487  * Then, in each *voxel* (i.e., 3D box), all the points present will be
488  * approximated (i.e., *downsampled*) with their centroid. This approach is
489  * a bit slower than approximating them with the center of the voxel, but it
490  * represents the underlying surface more accurately.
491  *
492  * \author Radu B. Rusu, Bastian Steder, Radoslaw Cybulski
493  * \ingroup filters
494  */
495  template <>
496  class PCL_EXPORTS VoxelGrid<pcl::PCLPointCloud2> : public Filter<pcl::PCLPointCloud2>
497  {
500 
504 
505  public:
506  /** \brief Empty constructor. */
507  VoxelGrid () :
508  leaf_size_ (Eigen::Vector4f::Zero ()),
509  inverse_leaf_size_ (Eigen::Array4f::Zero ()),
510  downsample_all_data_ (true),
511  save_leaf_layout_ (false),
512  leaf_layout_ (),
513  min_b_ (Eigen::Vector4i::Zero ()),
514  max_b_ (Eigen::Vector4i::Zero ()),
515  div_b_ (Eigen::Vector4i::Zero ()),
516  divb_mul_ (Eigen::Vector4i::Zero ()),
517  filter_field_name_ (""),
518  filter_limit_min_ (-FLT_MAX),
519  filter_limit_max_ (FLT_MAX),
520  filter_limit_negative_ (false)
521  {
522  filter_name_ = "VoxelGrid";
523  }
524 
525  /** \brief Destructor. */
526  virtual ~VoxelGrid ()
527  {
528  }
529 
530  /** \brief Set the voxel grid leaf size.
531  * \param[in] leaf_size the voxel grid leaf size
532  */
533  inline void
534  setLeafSize (const Eigen::Vector4f &leaf_size)
535  {
536  leaf_size_ = leaf_size;
537  // Avoid division errors
538  if (leaf_size_[3] == 0)
539  leaf_size_[3] = 1;
540  // Use multiplications instead of divisions
541  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
542  }
543 
544  /** \brief Set the voxel grid leaf size.
545  * \param[in] lx the leaf size for X
546  * \param[in] ly the leaf size for Y
547  * \param[in] lz the leaf size for Z
548  */
549  inline void
550  setLeafSize (float lx, float ly, float lz)
551  {
552  leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
553  // Avoid division errors
554  if (leaf_size_[3] == 0)
555  leaf_size_[3] = 1;
556  // Use multiplications instead of divisions
557  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
558  }
559 
560  /** \brief Get the voxel grid leaf size. */
561  inline Eigen::Vector3f
562  getLeafSize () { return (leaf_size_.head<3> ()); }
563 
564  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ.
565  * \param[in] downsample the new value (true/false)
566  */
567  inline void
568  setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
569 
570  /** \brief Get the state of the internal downsampling parameter (true if
571  * all fields need to be downsampled, false if just XYZ).
572  */
573  inline bool
574  getDownsampleAllData () { return (downsample_all_data_); }
575 
576  /** \brief Set to true if leaf layout information needs to be saved for later access.
577  * \param[in] save_leaf_layout the new value (true/false)
578  */
579  inline void
580  setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; }
581 
582  /** \brief Returns true if leaf layout information will to be saved for later access. */
583  inline bool
584  getSaveLeafLayout () { return (save_leaf_layout_); }
585 
586  /** \brief Get the minimum coordinates of the bounding box (after
587  * filtering is performed).
588  */
589  inline Eigen::Vector3i
590  getMinBoxCoordinates () { return (min_b_.head<3> ()); }
591 
592  /** \brief Get the minimum coordinates of the bounding box (after
593  * filtering is performed).
594  */
595  inline Eigen::Vector3i
596  getMaxBoxCoordinates () { return (max_b_.head<3> ()); }
597 
598  /** \brief Get the number of divisions along all 3 axes (after filtering
599  * is performed).
600  */
601  inline Eigen::Vector3i
602  getNrDivisions () { return (div_b_.head<3> ()); }
603 
604  /** \brief Get the multipliers to be applied to the grid coordinates in
605  * order to find the centroid index (after filtering is performed).
606  */
607  inline Eigen::Vector3i
608  getDivisionMultiplier () { return (divb_mul_.head<3> ()); }
609 
610  /** \brief Returns the index in the resulting downsampled cloud of the specified point.
611  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed,
612  * and that the point is inside the grid, to avoid invalid access (or use getGridCoordinates+getCentroidIndexAt)
613  * \param[in] x the X point coordinate to get the index at
614  * \param[in] y the Y point coordinate to get the index at
615  * \param[in] z the Z point coordinate to get the index at
616  */
617  inline int
618  getCentroidIndex (float x, float y, float z)
619  {
620  return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
621  static_cast<int> (floor (y * inverse_leaf_size_[1])),
622  static_cast<int> (floor (z * inverse_leaf_size_[2])),
623  0)
624  - min_b_).dot (divb_mul_)));
625  }
626 
627  /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates,
628  * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds).
629  * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
630  * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
631  * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
632  * \param[out] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell
633  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
634  */
635  inline std::vector<int>
636  getNeighborCentroidIndices (float x, float y, float z, const Eigen::MatrixXi &relative_coordinates)
637  {
638  Eigen::Vector4i ijk (static_cast<int> (floor (x * inverse_leaf_size_[0])),
639  static_cast<int> (floor (y * inverse_leaf_size_[1])),
640  static_cast<int> (floor (z * inverse_leaf_size_[2])), 0);
641  Eigen::Array4i diff2min = min_b_ - ijk;
642  Eigen::Array4i diff2max = max_b_ - ijk;
643  std::vector<int> neighbors (relative_coordinates.cols());
644  for (int ni = 0; ni < relative_coordinates.cols (); ni++)
645  {
646  Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
647  // checking if the specified cell is in the grid
648  if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
649  neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
650  else
651  neighbors[ni] = -1; // cell is out of bounds, consider it empty
652  }
653  return (neighbors);
654  }
655 
656  /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates,
657  * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds).
658  * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
659  * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
660  * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
661  * \param[out] relative_coordinates vector with the elements being the coordinates of the requested cells, relative to the reference point's cell
662  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
663  */
664  inline std::vector<int>
665  getNeighborCentroidIndices (float x, float y, float z, const std::vector<Eigen::Vector3i> &relative_coordinates)
666  {
667  Eigen::Vector4i ijk (static_cast<int> (floorf (x * inverse_leaf_size_[0])), static_cast<int> (floorf (y * inverse_leaf_size_[1])), static_cast<int> (floorf (z * inverse_leaf_size_[2])), 0);
668  std::vector<int> neighbors;
669  neighbors.reserve (relative_coordinates.size ());
670  for (std::vector<Eigen::Vector3i>::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++)
671  neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]);
672  return (neighbors);
673  }
674 
675  /** \brief Returns the layout of the leafs for fast access to cells relative to current position.
676  * \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)
677  */
678  inline std::vector<int>
679  getLeafLayout () { return (leaf_layout_); }
680 
681  /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
682  * \param[in] x the X point coordinate to get the (i, j, k) index at
683  * \param[in] y the Y point coordinate to get the (i, j, k) index at
684  * \param[in] z the Z point coordinate to get the (i, j, k) index at
685  */
686  inline Eigen::Vector3i
687  getGridCoordinates (float x, float y, float z)
688  {
689  return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
690  static_cast<int> (floor (y * inverse_leaf_size_[1])),
691  static_cast<int> (floor (z * inverse_leaf_size_[2]))));
692  }
693 
694  /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates.
695  * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty)
696  */
697  inline int
698  getCentroidIndexAt (const Eigen::Vector3i &ijk)
699  {
700  int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
701  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
702  {
703  //if (verbose)
704  // 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 ());
705  return (-1);
706  }
707  return (leaf_layout_[idx]);
708  }
709 
710  /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits,
711  * points having values outside this interval will be discarded.
712  * \param[in] field_name the name of the field that contains values used for filtering
713  */
714  inline void
715  setFilterFieldName (const std::string &field_name)
716  {
717  filter_field_name_ = field_name;
718  }
719 
720  /** \brief Get the name of the field used for filtering. */
721  inline std::string const
723  {
724  return (filter_field_name_);
725  }
726 
727  /** \brief Set the field filter limits. All points having field values outside this interval will be discarded.
728  * \param[in] limit_min the minimum allowed field value
729  * \param[in] limit_max the maximum allowed field value
730  */
731  inline void
732  setFilterLimits (const double &limit_min, const double &limit_max)
733  {
734  filter_limit_min_ = limit_min;
735  filter_limit_max_ = limit_max;
736  }
737 
738  /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX.
739  * \param[out] limit_min the minimum allowed field value
740  * \param[out] limit_max the maximum allowed field value
741  */
742  inline void
743  getFilterLimits (double &limit_min, double &limit_max)
744  {
745  limit_min = filter_limit_min_;
746  limit_max = filter_limit_max_;
747  }
748 
749  /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max).
750  * Default: false.
751  * \param[in] limit_negative return data inside the interval (false) or outside (true)
752  */
753  inline void
754  setFilterLimitsNegative (const bool limit_negative)
755  {
756  filter_limit_negative_ = limit_negative;
757  }
758 
759  /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
760  * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
761  */
762  inline void
763  getFilterLimitsNegative (bool &limit_negative)
764  {
765  limit_negative = filter_limit_negative_;
766  }
767 
768  /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
769  * \return true if data \b outside the interval [min; max] is to be returned, false otherwise
770  */
771  inline bool
773  {
774  return (filter_limit_negative_);
775  }
776 
777  protected:
778  /** \brief The size of a leaf. */
779  Eigen::Vector4f leaf_size_;
780 
781  /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */
782  Eigen::Array4f inverse_leaf_size_;
783 
784  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */
786 
787  /** \brief Set to true if leaf layout information needs to be saved in \a
788  * leaf_layout.
789  */
791 
792  /** \brief The leaf layout information for fast access to cells relative
793  * to current position
794  */
795  std::vector<int> leaf_layout_;
796 
797  /** \brief The minimum and maximum bin coordinates, the number of
798  * divisions, and the division multiplier.
799  */
800  Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
801 
802  /** \brief The desired user filter field name. */
803  std::string filter_field_name_;
804 
805  /** \brief The minimum allowed filter value a point will be considered from. */
807 
808  /** \brief The maximum allowed filter value a point will be considered from. */
810 
811  /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
813 
814  /** \brief Downsample a Point Cloud using a voxelized grid approach
815  * \param[out] output the resultant point cloud
816  */
817  void
818  applyFilter (PCLPointCloud2 &output);
819  };
820 }
821 
822 #ifdef PCL_NO_PRECOMPILE
823 #include <pcl/filters/impl/voxel_grid.hpp>
824 #endif
825 
826 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_