Point Cloud Library (PCL)  1.7.2
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
85  getHalfNeighborCellIndices ()
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
122  getAllNeighborCellIndices ()
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:
181  using Filter<PointT>::filter_name_;
182  using Filter<PointT>::getClassName;
183  using Filter<PointT>::input_;
184  using Filter<PointT>::indices_;
185 
186  typedef typename Filter<PointT>::PointCloud PointCloud;
187  typedef typename PointCloud::Ptr PointCloudPtr;
188  typedef typename PointCloud::ConstPtr PointCloudConstPtr;
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  min_points_per_voxel_ (0)
210  {
211  filter_name_ = "VoxelGrid";
212  }
213 
214  /** \brief Destructor. */
215  virtual ~VoxelGrid ()
216  {
217  }
218 
219  /** \brief Set the voxel grid leaf size.
220  * \param[in] leaf_size the voxel grid leaf size
221  */
222  inline void
223  setLeafSize (const Eigen::Vector4f &leaf_size)
224  {
225  leaf_size_ = leaf_size;
226  // Avoid division errors
227  if (leaf_size_[3] == 0)
228  leaf_size_[3] = 1;
229  // Use multiplications instead of divisions
230  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
231  }
232 
233  /** \brief Set the voxel grid leaf size.
234  * \param[in] lx the leaf size for X
235  * \param[in] ly the leaf size for Y
236  * \param[in] lz the leaf size for Z
237  */
238  inline void
239  setLeafSize (float lx, float ly, float lz)
240  {
241  leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
242  // Avoid division errors
243  if (leaf_size_[3] == 0)
244  leaf_size_[3] = 1;
245  // Use multiplications instead of divisions
246  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
247  }
248 
249  /** \brief Get the voxel grid leaf size. */
250  inline Eigen::Vector3f
251  getLeafSize () { return (leaf_size_.head<3> ()); }
252 
253  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ.
254  * \param[in] downsample the new value (true/false)
255  */
256  inline void
257  setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
258 
259  /** \brief Get the state of the internal downsampling parameter (true if
260  * all fields need to be downsampled, false if just XYZ).
261  */
262  inline bool
263  getDownsampleAllData () { return (downsample_all_data_); }
264 
265  /** \brief Set the minimum number of points required for a voxel to be used.
266  * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
267  */
268  inline void
269  setMinimumPointsNumberPerVoxel (unsigned int min_points_per_voxel) { min_points_per_voxel_ = min_points_per_voxel; }
270 
271  /** \brief Return the minimum number of points required for a voxel to be used.
272  */
273  inline unsigned int
274  getMinimumPointsNumberPerVoxel () { return min_points_per_voxel_; }
275 
276  /** \brief Set to true if leaf layout information needs to be saved for later access.
277  * \param[in] save_leaf_layout the new value (true/false)
278  */
279  inline void
280  setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; }
281 
282  /** \brief Returns true if leaf layout information will to be saved for later access. */
283  inline bool
284  getSaveLeafLayout () { return (save_leaf_layout_); }
285 
286  /** \brief Get the minimum coordinates of the bounding box (after
287  * filtering is performed).
288  */
289  inline Eigen::Vector3i
290  getMinBoxCoordinates () { return (min_b_.head<3> ()); }
291 
292  /** \brief Get the minimum coordinates of the bounding box (after
293  * filtering is performed).
294  */
295  inline Eigen::Vector3i
296  getMaxBoxCoordinates () { return (max_b_.head<3> ()); }
297 
298  /** \brief Get the number of divisions along all 3 axes (after filtering
299  * is performed).
300  */
301  inline Eigen::Vector3i
302  getNrDivisions () { return (div_b_.head<3> ()); }
303 
304  /** \brief Get the multipliers to be applied to the grid coordinates in
305  * order to find the centroid index (after filtering is performed).
306  */
307  inline Eigen::Vector3i
308  getDivisionMultiplier () { return (divb_mul_.head<3> ()); }
309 
310  /** \brief Returns the index in the resulting downsampled cloud of the specified point.
311  *
312  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering
313  * performed, and that the point is inside the grid, to avoid invalid access (or use
314  * getGridCoordinates+getCentroidIndexAt)
315  *
316  * \param[in] p the point to get the index at
317  */
318  inline int
319  getCentroidIndex (const PointT &p)
320  {
321  return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (p.x * inverse_leaf_size_[0])),
322  static_cast<int> (floor (p.y * inverse_leaf_size_[1])),
323  static_cast<int> (floor (p.z * inverse_leaf_size_[2])), 0) - min_b_).dot (divb_mul_)));
324  }
325 
326  /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates,
327  * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds).
328  * \param[in] reference_point the coordinates of the reference point (corresponding cell is allowed to be empty/out of bounds)
329  * \param[in] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell
330  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
331  */
332  inline std::vector<int>
333  getNeighborCentroidIndices (const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates)
334  {
335  Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x * inverse_leaf_size_[0])),
336  static_cast<int> (floor (reference_point.y * inverse_leaf_size_[1])),
337  static_cast<int> (floor (reference_point.z * inverse_leaf_size_[2])), 0);
338  Eigen::Array4i diff2min = min_b_ - ijk;
339  Eigen::Array4i diff2max = max_b_ - ijk;
340  std::vector<int> neighbors (relative_coordinates.cols());
341  for (int ni = 0; ni < relative_coordinates.cols (); ni++)
342  {
343  Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
344  // checking if the specified cell is in the grid
345  if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
346  neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
347  else
348  neighbors[ni] = -1; // cell is out of bounds, consider it empty
349  }
350  return (neighbors);
351  }
352 
353  /** \brief Returns the layout of the leafs for fast access to cells relative to current position.
354  * \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)
355  */
356  inline std::vector<int>
357  getLeafLayout () { return (leaf_layout_); }
358 
359  /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
360  * \param[in] x the X point coordinate to get the (i, j, k) index at
361  * \param[in] y the Y point coordinate to get the (i, j, k) index at
362  * \param[in] z the Z point coordinate to get the (i, j, k) index at
363  */
364  inline Eigen::Vector3i
365  getGridCoordinates (float x, float y, float z)
366  {
367  return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
368  static_cast<int> (floor (y * inverse_leaf_size_[1])),
369  static_cast<int> (floor (z * inverse_leaf_size_[2]))));
370  }
371 
372  /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates.
373  * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty)
374  */
375  inline int
376  getCentroidIndexAt (const Eigen::Vector3i &ijk)
377  {
378  int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
379  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
380  {
381  //if (verbose)
382  // 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 ());
383  return (-1);
384  }
385  return (leaf_layout_[idx]);
386  }
387 
388  /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits,
389  * points having values outside this interval will be discarded.
390  * \param[in] field_name the name of the field that contains values used for filtering
391  */
392  inline void
393  setFilterFieldName (const std::string &field_name)
394  {
395  filter_field_name_ = field_name;
396  }
397 
398  /** \brief Get the name of the field used for filtering. */
399  inline std::string const
400  getFilterFieldName ()
401  {
402  return (filter_field_name_);
403  }
404 
405  /** \brief Set the field filter limits. All points having field values outside this interval will be discarded.
406  * \param[in] limit_min the minimum allowed field value
407  * \param[in] limit_max the maximum allowed field value
408  */
409  inline void
410  setFilterLimits (const double &limit_min, const double &limit_max)
411  {
412  filter_limit_min_ = limit_min;
413  filter_limit_max_ = limit_max;
414  }
415 
416  /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX.
417  * \param[out] limit_min the minimum allowed field value
418  * \param[out] limit_max the maximum allowed field value
419  */
420  inline void
421  getFilterLimits (double &limit_min, double &limit_max)
422  {
423  limit_min = filter_limit_min_;
424  limit_max = filter_limit_max_;
425  }
426 
427  /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max).
428  * Default: false.
429  * \param[in] limit_negative return data inside the interval (false) or outside (true)
430  */
431  inline void
432  setFilterLimitsNegative (const bool limit_negative)
433  {
434  filter_limit_negative_ = limit_negative;
435  }
436 
437  /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
438  * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
439  */
440  inline void
441  getFilterLimitsNegative (bool &limit_negative)
442  {
443  limit_negative = filter_limit_negative_;
444  }
445 
446  /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
447  * \return true if data \b outside the interval [min; max] is to be returned, false otherwise
448  */
449  inline bool
450  getFilterLimitsNegative ()
451  {
452  return (filter_limit_negative_);
453  }
454 
455  protected:
456  /** \brief The size of a leaf. */
457  Eigen::Vector4f leaf_size_;
458 
459  /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */
460  Eigen::Array4f inverse_leaf_size_;
461 
462  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */
463  bool downsample_all_data_;
464 
465  /** \brief Set to true if leaf layout information needs to be saved in \a leaf_layout_. */
466  bool save_leaf_layout_;
467 
468  /** \brief The leaf layout information for fast access to cells relative to current position **/
469  std::vector<int> leaf_layout_;
470 
471  /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */
472  Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
473 
474  /** \brief The desired user filter field name. */
475  std::string filter_field_name_;
476 
477  /** \brief The minimum allowed filter value a point will be considered from. */
478  double filter_limit_min_;
479 
480  /** \brief The maximum allowed filter value a point will be considered from. */
481  double filter_limit_max_;
482 
483  /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
484  bool filter_limit_negative_;
485 
486  /** \brief Minimum number of points per voxel for the centroid to be computed */
487  unsigned int min_points_per_voxel_;
488 
489  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
490 
491  /** \brief Downsample a Point Cloud using a voxelized grid approach
492  * \param[out] output the resultant point cloud message
493  */
494  void
495  applyFilter (PointCloud &output);
496  };
497 
498  /** \brief VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
499  *
500  * The VoxelGrid class creates a *3D voxel grid* (think about a voxel
501  * grid as a set of tiny 3D boxes in space) over the input point cloud data.
502  * Then, in each *voxel* (i.e., 3D box), all the points present will be
503  * approximated (i.e., *downsampled*) with their centroid. This approach is
504  * a bit slower than approximating them with the center of the voxel, but it
505  * represents the underlying surface more accurately.
506  *
507  * \author Radu B. Rusu, Bastian Steder, Radoslaw Cybulski
508  * \ingroup filters
509  */
510  template <>
511  class PCL_EXPORTS VoxelGrid<pcl::PCLPointCloud2> : public Filter<pcl::PCLPointCloud2>
512  {
513  using Filter<pcl::PCLPointCloud2>::filter_name_;
514  using Filter<pcl::PCLPointCloud2>::getClassName;
515 
516  typedef pcl::PCLPointCloud2 PCLPointCloud2;
517  typedef PCLPointCloud2::Ptr PCLPointCloud2Ptr;
518  typedef PCLPointCloud2::ConstPtr PCLPointCloud2ConstPtr;
519 
520  public:
521  /** \brief Empty constructor. */
522  VoxelGrid () :
523  leaf_size_ (Eigen::Vector4f::Zero ()),
524  inverse_leaf_size_ (Eigen::Array4f::Zero ()),
525  downsample_all_data_ (true),
526  save_leaf_layout_ (false),
527  leaf_layout_ (),
528  min_b_ (Eigen::Vector4i::Zero ()),
529  max_b_ (Eigen::Vector4i::Zero ()),
530  div_b_ (Eigen::Vector4i::Zero ()),
531  divb_mul_ (Eigen::Vector4i::Zero ()),
532  filter_field_name_ (""),
533  filter_limit_min_ (-FLT_MAX),
534  filter_limit_max_ (FLT_MAX),
535  filter_limit_negative_ (false),
536  min_points_per_voxel_ (0)
537  {
538  filter_name_ = "VoxelGrid";
539  }
540 
541  /** \brief Destructor. */
542  virtual ~VoxelGrid ()
543  {
544  }
545 
546  /** \brief Set the voxel grid leaf size.
547  * \param[in] leaf_size the voxel grid leaf size
548  */
549  inline void
550  setLeafSize (const Eigen::Vector4f &leaf_size)
551  {
552  leaf_size_ = leaf_size;
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 Set the voxel grid leaf size.
561  * \param[in] lx the leaf size for X
562  * \param[in] ly the leaf size for Y
563  * \param[in] lz the leaf size for Z
564  */
565  inline void
566  setLeafSize (float lx, float ly, float lz)
567  {
568  leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
569  // Avoid division errors
570  if (leaf_size_[3] == 0)
571  leaf_size_[3] = 1;
572  // Use multiplications instead of divisions
573  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
574  }
575 
576  /** \brief Get the voxel grid leaf size. */
577  inline Eigen::Vector3f
578  getLeafSize () { return (leaf_size_.head<3> ()); }
579 
580  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ.
581  * \param[in] downsample the new value (true/false)
582  */
583  inline void
584  setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
585 
586  /** \brief Get the state of the internal downsampling parameter (true if
587  * all fields need to be downsampled, false if just XYZ).
588  */
589  inline bool
590  getDownsampleAllData () { return (downsample_all_data_); }
591 
592  /** \brief Set the minimum number of points required for a voxel to be used.
593  * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
594  */
595  inline void
596  setMinimumPointsNumberPerVoxel (unsigned int min_points_per_voxel) { min_points_per_voxel_ = min_points_per_voxel; }
597 
598  /** \brief Return the minimum number of points required for a voxel to be used.
599  */
600  inline unsigned int
601  getMinimumPointsNumberPerVoxel () { return min_points_per_voxel_; }
602 
603  /** \brief Set to true if leaf layout information needs to be saved for later access.
604  * \param[in] save_leaf_layout the new value (true/false)
605  */
606  inline void
607  setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; }
608 
609  /** \brief Returns true if leaf layout information will to be saved for later access. */
610  inline bool
611  getSaveLeafLayout () { return (save_leaf_layout_); }
612 
613  /** \brief Get the minimum coordinates of the bounding box (after
614  * filtering is performed).
615  */
616  inline Eigen::Vector3i
617  getMinBoxCoordinates () { return (min_b_.head<3> ()); }
618 
619  /** \brief Get the minimum coordinates of the bounding box (after
620  * filtering is performed).
621  */
622  inline Eigen::Vector3i
623  getMaxBoxCoordinates () { return (max_b_.head<3> ()); }
624 
625  /** \brief Get the number of divisions along all 3 axes (after filtering
626  * is performed).
627  */
628  inline Eigen::Vector3i
629  getNrDivisions () { return (div_b_.head<3> ()); }
630 
631  /** \brief Get the multipliers to be applied to the grid coordinates in
632  * order to find the centroid index (after filtering is performed).
633  */
634  inline Eigen::Vector3i
635  getDivisionMultiplier () { return (divb_mul_.head<3> ()); }
636 
637  /** \brief Returns the index in the resulting downsampled cloud of the specified point.
638  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed,
639  * and that the point is inside the grid, to avoid invalid access (or use getGridCoordinates+getCentroidIndexAt)
640  * \param[in] x the X point coordinate to get the index at
641  * \param[in] y the Y point coordinate to get the index at
642  * \param[in] z the Z point coordinate to get the index at
643  */
644  inline int
645  getCentroidIndex (float x, float y, float z)
646  {
647  return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
648  static_cast<int> (floor (y * inverse_leaf_size_[1])),
649  static_cast<int> (floor (z * inverse_leaf_size_[2])),
650  0)
651  - min_b_).dot (divb_mul_)));
652  }
653 
654  /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates,
655  * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds).
656  * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
657  * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
658  * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
659  * \param[out] relative_coordinates matrix with the columns being the coordinates of the requested cells, relative to the reference point's cell
660  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
661  */
662  inline std::vector<int>
663  getNeighborCentroidIndices (float x, float y, float z, const Eigen::MatrixXi &relative_coordinates)
664  {
665  Eigen::Vector4i ijk (static_cast<int> (floor (x * inverse_leaf_size_[0])),
666  static_cast<int> (floor (y * inverse_leaf_size_[1])),
667  static_cast<int> (floor (z * inverse_leaf_size_[2])), 0);
668  Eigen::Array4i diff2min = min_b_ - ijk;
669  Eigen::Array4i diff2max = max_b_ - ijk;
670  std::vector<int> neighbors (relative_coordinates.cols());
671  for (int ni = 0; ni < relative_coordinates.cols (); ni++)
672  {
673  Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
674  // checking if the specified cell is in the grid
675  if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
676  neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
677  else
678  neighbors[ni] = -1; // cell is out of bounds, consider it empty
679  }
680  return (neighbors);
681  }
682 
683  /** \brief Returns the indices in the resulting downsampled cloud of the points at the specified grid coordinates,
684  * relative to the grid coordinates of the specified point (or -1 if the cell was empty/out of bounds).
685  * \param[in] x the X coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
686  * \param[in] y the Y coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
687  * \param[in] z the Z coordinate of the reference point (corresponding cell is allowed to be empty/out of bounds)
688  * \param[out] relative_coordinates vector with the elements being the coordinates of the requested cells, relative to the reference point's cell
689  * \note for efficiency, user must make sure that the saving of the leaf layout is enabled and filtering performed
690  */
691  inline std::vector<int>
692  getNeighborCentroidIndices (float x, float y, float z, const std::vector<Eigen::Vector3i> &relative_coordinates)
693  {
694  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);
695  std::vector<int> neighbors;
696  neighbors.reserve (relative_coordinates.size ());
697  for (std::vector<Eigen::Vector3i>::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++)
698  neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]);
699  return (neighbors);
700  }
701 
702  /** \brief Returns the layout of the leafs for fast access to cells relative to current position.
703  * \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)
704  */
705  inline std::vector<int>
706  getLeafLayout () { return (leaf_layout_); }
707 
708  /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
709  * \param[in] x the X point coordinate to get the (i, j, k) index at
710  * \param[in] y the Y point coordinate to get the (i, j, k) index at
711  * \param[in] z the Z point coordinate to get the (i, j, k) index at
712  */
713  inline Eigen::Vector3i
714  getGridCoordinates (float x, float y, float z)
715  {
716  return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
717  static_cast<int> (floor (y * inverse_leaf_size_[1])),
718  static_cast<int> (floor (z * inverse_leaf_size_[2]))));
719  }
720 
721  /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates.
722  * \param[in] ijk the coordinates (i,j,k) in the grid (-1 if empty)
723  */
724  inline int
725  getCentroidIndexAt (const Eigen::Vector3i &ijk)
726  {
727  int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
728  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
729  {
730  //if (verbose)
731  // 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 ());
732  return (-1);
733  }
734  return (leaf_layout_[idx]);
735  }
736 
737  /** \brief Provide the name of the field to be used for filtering data. In conjunction with \a setFilterLimits,
738  * points having values outside this interval will be discarded.
739  * \param[in] field_name the name of the field that contains values used for filtering
740  */
741  inline void
742  setFilterFieldName (const std::string &field_name)
743  {
744  filter_field_name_ = field_name;
745  }
746 
747  /** \brief Get the name of the field used for filtering. */
748  inline std::string const
749  getFilterFieldName ()
750  {
751  return (filter_field_name_);
752  }
753 
754  /** \brief Set the field filter limits. All points having field values outside this interval will be discarded.
755  * \param[in] limit_min the minimum allowed field value
756  * \param[in] limit_max the maximum allowed field value
757  */
758  inline void
759  setFilterLimits (const double &limit_min, const double &limit_max)
760  {
761  filter_limit_min_ = limit_min;
762  filter_limit_max_ = limit_max;
763  }
764 
765  /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX.
766  * \param[out] limit_min the minimum allowed field value
767  * \param[out] limit_max the maximum allowed field value
768  */
769  inline void
770  getFilterLimits (double &limit_min, double &limit_max)
771  {
772  limit_min = filter_limit_min_;
773  limit_max = filter_limit_max_;
774  }
775 
776  /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max).
777  * Default: false.
778  * \param[in] limit_negative return data inside the interval (false) or outside (true)
779  */
780  inline void
781  setFilterLimitsNegative (const bool limit_negative)
782  {
783  filter_limit_negative_ = limit_negative;
784  }
785 
786  /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
787  * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise
788  */
789  inline void
790  getFilterLimitsNegative (bool &limit_negative)
791  {
792  limit_negative = filter_limit_negative_;
793  }
794 
795  /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false).
796  * \return true if data \b outside the interval [min; max] is to be returned, false otherwise
797  */
798  inline bool
799  getFilterLimitsNegative ()
800  {
801  return (filter_limit_negative_);
802  }
803 
804  protected:
805  /** \brief The size of a leaf. */
806  Eigen::Vector4f leaf_size_;
807 
808  /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */
809  Eigen::Array4f inverse_leaf_size_;
810 
811  /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */
812  bool downsample_all_data_;
813 
814  /** \brief Set to true if leaf layout information needs to be saved in \a
815  * leaf_layout.
816  */
817  bool save_leaf_layout_;
818 
819  /** \brief The leaf layout information for fast access to cells relative
820  * to current position
821  */
822  std::vector<int> leaf_layout_;
823 
824  /** \brief The minimum and maximum bin coordinates, the number of
825  * divisions, and the division multiplier.
826  */
827  Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
828 
829  /** \brief The desired user filter field name. */
830  std::string filter_field_name_;
831 
832  /** \brief The minimum allowed filter value a point will be considered from. */
833  double filter_limit_min_;
834 
835  /** \brief The maximum allowed filter value a point will be considered from. */
836  double filter_limit_max_;
837 
838  /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */
839  bool filter_limit_negative_;
840 
841  /** \brief Minimum number of points per voxel for the centroid to be computed */
842  unsigned int min_points_per_voxel_;
843 
844  /** \brief Downsample a Point Cloud using a voxelized grid approach
845  * \param[out] output the resultant point cloud
846  */
847  void
848  applyFilter (PCLPointCloud2 &output);
849  };
850 }
851 
852 #ifdef PCL_NO_PRECOMPILE
853 #include <pcl/filters/impl/voxel_grid.hpp>
854 #endif
855 
856 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_