Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
voxel_grid.h
Go to the documentation of this file.
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 Willow Garage, Inc. 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: voxel_grid.h 5654 2012-05-01 05:32:03Z rusu $
37  *
38  */
39 
40 #ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
41 #define PCL_FILTERS_VOXEL_GRID_MAP_H_
42 
43 #include <pcl/filters/filter.h>
44 #include <map>
45 #include <boost/unordered_map.hpp>
46 #include <boost/fusion/sequence/intrinsic/at_key.hpp>
47 
48 namespace pcl
49 {
58  PCL_EXPORTS void
59  getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
60  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);
61 
76  PCL_EXPORTS void
77  getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
78  const std::string &distance_field_name, float min_distance, float max_distance,
79  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
80 
85  inline Eigen::MatrixXi
86  getHalfNeighborCellIndices ()
87  {
88  Eigen::MatrixXi relative_coordinates (3, 13);
89  int idx = 0;
90 
91  // 0 - 8
92  for (int i = -1; i < 2; i++)
93  {
94  for (int j = -1; j < 2; j++)
95  {
96  relative_coordinates (0, idx) = i;
97  relative_coordinates (1, idx) = j;
98  relative_coordinates (2, idx) = -1;
99  idx++;
100  }
101  }
102  // 9 - 11
103  for (int i = -1; i < 2; i++)
104  {
105  relative_coordinates (0, idx) = i;
106  relative_coordinates (1, idx) = -1;
107  relative_coordinates (2, idx) = 0;
108  idx++;
109  }
110  // 12
111  relative_coordinates (0, idx) = -1;
112  relative_coordinates (1, idx) = 0;
113  relative_coordinates (2, idx) = 0;
114 
115  return (relative_coordinates);
116  }
117 
122  inline Eigen::MatrixXi
123  getAllNeighborCellIndices ()
124  {
125  Eigen::MatrixXi relative_coordinates = getHalfNeighborCellIndices ();
126  Eigen::MatrixXi relative_coordinates_all( 3, 26);
127  relative_coordinates_all.block<3, 13> (0, 0) = relative_coordinates;
128  relative_coordinates_all.block<3, 13> (0, 13) = -relative_coordinates;
129  return (relative_coordinates_all);
130  }
131 
143  template <typename PointT> void
144  getMinMax3D (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
145  const std::string &distance_field_name, float min_distance, float max_distance,
146  Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, bool limit_negative = false);
147 
160  template <typename PointT>
161  class VoxelGrid: public Filter<PointT>
162  {
163  protected:
164  using Filter<PointT>::filter_name_;
165  using Filter<PointT>::getClassName;
166  using Filter<PointT>::input_;
167  using Filter<PointT>::indices_;
168 
169  typedef typename Filter<PointT>::PointCloud PointCloud;
170  typedef typename PointCloud::Ptr PointCloudPtr;
171  typedef typename PointCloud::ConstPtr PointCloudConstPtr;
172 
173  public:
175  VoxelGrid () :
176  leaf_size_ (Eigen::Vector4f::Zero ()),
177  inverse_leaf_size_ (Eigen::Array4f::Zero ()),
178  downsample_all_data_ (true),
179  save_leaf_layout_ (false),
180  leaf_layout_ (),
181  min_b_ (Eigen::Vector4i::Zero ()),
182  max_b_ (Eigen::Vector4i::Zero ()),
183  div_b_ (Eigen::Vector4i::Zero ()),
184  divb_mul_ (Eigen::Vector4i::Zero ()),
185  filter_field_name_ (""),
186  filter_limit_min_ (-FLT_MAX),
187  filter_limit_max_ (FLT_MAX),
188  filter_limit_negative_ (false)
189  {
190  filter_name_ = "VoxelGrid";
191  }
192 
194  virtual ~VoxelGrid ()
195  {
196  }
197 
201  inline void
202  setLeafSize (const Eigen::Vector4f &leaf_size)
203  {
204  leaf_size_ = leaf_size;
205  // Avoid division errors
206  if (leaf_size_[3] == 0)
207  leaf_size_[3] = 1;
208  // Use multiplications instead of divisions
209  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
210  }
211 
217  inline void
218  setLeafSize (float lx, float ly, float lz)
219  {
220  leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
221  // Avoid division errors
222  if (leaf_size_[3] == 0)
223  leaf_size_[3] = 1;
224  // Use multiplications instead of divisions
225  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
226  }
227 
229  inline Eigen::Vector3f
230  getLeafSize () { return (leaf_size_.head<3> ()); }
231 
235  inline void
236  setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
237 
241  inline bool
242  getDownsampleAllData () { return (downsample_all_data_); }
243 
247  inline void
248  setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; }
249 
251  inline bool
252  getSaveLeafLayout () { return (save_leaf_layout_); }
253 
257  inline Eigen::Vector3i
258  getMinBoxCoordinates () { return (min_b_.head<3> ()); }
259 
263  inline Eigen::Vector3i
264  getMaxBoxCoordinates () { return (max_b_.head<3> ()); }
265 
269  inline Eigen::Vector3i
270  getNrDivisions () { return (div_b_.head<3> ()); }
271 
275  inline Eigen::Vector3i
276  getDivisionMultiplier () { return (divb_mul_.head<3> ()); }
277 
286  inline int
287  getCentroidIndex (const PointT &p)
288  {
289  return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (p.x * inverse_leaf_size_[0])),
290  static_cast<int> (floor (p.y * inverse_leaf_size_[1])),
291  static_cast<int> (floor (p.z * inverse_leaf_size_[2])), 0) - min_b_).dot (divb_mul_)));
292  }
293 
300  inline std::vector<int>
301  getNeighborCentroidIndices (const PointT &reference_point, const Eigen::MatrixXi &relative_coordinates)
302  {
303  Eigen::Vector4i ijk (static_cast<int> (floor (reference_point.x * inverse_leaf_size_[0])),
304  static_cast<int> (floor (reference_point.y * inverse_leaf_size_[1])),
305  static_cast<int> (floor (reference_point.z * inverse_leaf_size_[2])), 0);
306  Eigen::Array4i diff2min = min_b_ - ijk;
307  Eigen::Array4i diff2max = max_b_ - ijk;
308  std::vector<int> neighbors (relative_coordinates.cols());
309  for (int ni = 0; ni < relative_coordinates.cols (); ni++)
310  {
311  Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
312  // checking if the specified cell is in the grid
313  if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
314  neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
315  else
316  neighbors[ni] = -1; // cell is out of bounds, consider it empty
317  }
318  return (neighbors);
319  }
320 
324  inline std::vector<int>
325  getLeafLayout () { return (leaf_layout_); }
326 
332  inline Eigen::Vector3i
333  getGridCoordinates (float x, float y, float z)
334  {
335  return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
336  static_cast<int> (floor (y * inverse_leaf_size_[1])),
337  static_cast<int> (floor (z * inverse_leaf_size_[2]))));
338  }
339 
343  inline int
344  getCentroidIndexAt (const Eigen::Vector3i &ijk)
345  {
346  int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
347  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
348  {
349  //if (verbose)
350  // 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 ());
351  return (-1);
352  }
353  return (leaf_layout_[idx]);
354  }
355 
360  inline void
361  setFilterFieldName (const std::string &field_name)
362  {
363  filter_field_name_ = field_name;
364  }
365 
367  inline std::string const
368  getFilterFieldName ()
369  {
370  return (filter_field_name_);
371  }
372 
377  inline void
378  setFilterLimits (const double &limit_min, const double &limit_max)
379  {
380  filter_limit_min_ = limit_min;
381  filter_limit_max_ = limit_max;
382  }
383 
388  inline void
389  getFilterLimits (double &limit_min, double &limit_max)
390  {
391  limit_min = filter_limit_min_;
392  limit_max = filter_limit_max_;
393  }
394 
399  inline void
400  setFilterLimitsNegative (const bool limit_negative)
401  {
402  filter_limit_negative_ = limit_negative;
403  }
404 
408  inline void
409  getFilterLimitsNegative (bool &limit_negative)
410  {
411  limit_negative = filter_limit_negative_;
412  }
413 
417  inline bool
418  getFilterLimitsNegative ()
419  {
420  return (filter_limit_negative_);
421  }
422 
423  protected:
425  Eigen::Vector4f leaf_size_;
426 
428  Eigen::Array4f inverse_leaf_size_;
429 
431  bool downsample_all_data_;
432 
434  bool save_leaf_layout_;
435 
437  std::vector<int> leaf_layout_;
438 
440  Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
441 
443  std::string filter_field_name_;
444 
446  double filter_limit_min_;
447 
449  double filter_limit_max_;
450 
452  bool filter_limit_negative_;
453 
454  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
455 
459  void
460  applyFilter (PointCloud &output);
461  };
462 
475  template <>
476  class PCL_EXPORTS VoxelGrid<sensor_msgs::PointCloud2> : public Filter<sensor_msgs::PointCloud2>
477  {
478  using Filter<sensor_msgs::PointCloud2>::filter_name_;
479  using Filter<sensor_msgs::PointCloud2>::getClassName;
480 
481  typedef sensor_msgs::PointCloud2 PointCloud2;
482  typedef PointCloud2::Ptr PointCloud2Ptr;
483  typedef PointCloud2::ConstPtr PointCloud2ConstPtr;
484 
485  public:
487  VoxelGrid () :
488  leaf_size_ (Eigen::Vector4f::Zero ()),
489  inverse_leaf_size_ (Eigen::Array4f::Zero ()),
490  downsample_all_data_ (true),
491  save_leaf_layout_ (false),
492  leaf_layout_ (),
493  min_b_ (Eigen::Vector4i::Zero ()),
494  max_b_ (Eigen::Vector4i::Zero ()),
495  div_b_ (Eigen::Vector4i::Zero ()),
496  divb_mul_ (Eigen::Vector4i::Zero ()),
497  filter_field_name_ (""),
498  filter_limit_min_ (-FLT_MAX),
499  filter_limit_max_ (FLT_MAX),
500  filter_limit_negative_ (false)
501  {
502  filter_name_ = "VoxelGrid";
503  }
504 
506  virtual ~VoxelGrid ()
507  {
508  }
509 
513  inline void
514  setLeafSize (const Eigen::Vector4f &leaf_size)
515  {
516  leaf_size_ = leaf_size;
517  // Avoid division errors
518  if (leaf_size_[3] == 0)
519  leaf_size_[3] = 1;
520  // Use multiplications instead of divisions
521  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
522  }
523 
529  inline void
530  setLeafSize (float lx, float ly, float lz)
531  {
532  leaf_size_[0] = lx; leaf_size_[1] = ly; leaf_size_[2] = lz;
533  // Avoid division errors
534  if (leaf_size_[3] == 0)
535  leaf_size_[3] = 1;
536  // Use multiplications instead of divisions
537  inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
538  }
539 
541  inline Eigen::Vector3f
542  getLeafSize () { return (leaf_size_.head<3> ()); }
543 
547  inline void
548  setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
549 
553  inline bool
554  getDownsampleAllData () { return (downsample_all_data_); }
555 
559  inline void
560  setSaveLeafLayout (bool save_leaf_layout) { save_leaf_layout_ = save_leaf_layout; }
561 
563  inline bool
564  getSaveLeafLayout () { return (save_leaf_layout_); }
565 
569  inline Eigen::Vector3i
570  getMinBoxCoordinates () { return (min_b_.head<3> ()); }
571 
575  inline Eigen::Vector3i
576  getMaxBoxCoordinates () { return (max_b_.head<3> ()); }
577 
581  inline Eigen::Vector3i
582  getNrDivisions () { return (div_b_.head<3> ()); }
583 
587  inline Eigen::Vector3i
588  getDivisionMultiplier () { return (divb_mul_.head<3> ()); }
589 
597  inline int
598  getCentroidIndex (float x, float y, float z)
599  {
600  return (leaf_layout_.at ((Eigen::Vector4i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
601  static_cast<int> (floor (y * inverse_leaf_size_[1])),
602  static_cast<int> (floor (z * inverse_leaf_size_[2])),
603  0)
604  - min_b_).dot (divb_mul_)));
605  }
606 
615  inline std::vector<int>
616  getNeighborCentroidIndices (float x, float y, float z, const Eigen::MatrixXi &relative_coordinates)
617  {
618  Eigen::Vector4i ijk (static_cast<int> (floor (x * inverse_leaf_size_[0])),
619  static_cast<int> (floor (y * inverse_leaf_size_[1])),
620  static_cast<int> (floor (z * inverse_leaf_size_[2])), 0);
621  Eigen::Array4i diff2min = min_b_ - ijk;
622  Eigen::Array4i diff2max = max_b_ - ijk;
623  std::vector<int> neighbors (relative_coordinates.cols());
624  for (int ni = 0; ni < relative_coordinates.cols (); ni++)
625  {
626  Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
627  // checking if the specified cell is in the grid
628  if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
629  neighbors[ni] = leaf_layout_[((ijk + displacement - min_b_).dot (divb_mul_))]; // .at() can be omitted
630  else
631  neighbors[ni] = -1; // cell is out of bounds, consider it empty
632  }
633  return (neighbors);
634  }
635 
644  inline std::vector<int>
645  getNeighborCentroidIndices (float x, float y, float z, const std::vector<Eigen::Vector3i> &relative_coordinates)
646  {
647  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);
648  std::vector<int> neighbors;
649  neighbors.reserve (relative_coordinates.size ());
650  for (std::vector<Eigen::Vector3i>::const_iterator it = relative_coordinates.begin (); it != relative_coordinates.end (); it++)
651  neighbors.push_back (leaf_layout_[(ijk + (Eigen::Vector4i() << *it, 0).finished() - min_b_).dot (divb_mul_)]);
652  return (neighbors);
653  }
654 
658  inline std::vector<int>
659  getLeafLayout () { return (leaf_layout_); }
660 
666  inline Eigen::Vector3i
667  getGridCoordinates (float x, float y, float z)
668  {
669  return (Eigen::Vector3i (static_cast<int> (floor (x * inverse_leaf_size_[0])),
670  static_cast<int> (floor (y * inverse_leaf_size_[1])),
671  static_cast<int> (floor (z * inverse_leaf_size_[2]))));
672  }
673 
677  inline int
678  getCentroidIndexAt (const Eigen::Vector3i &ijk)
679  {
680  int idx = ((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (divb_mul_);
681  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
682  {
683  //if (verbose)
684  // 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 ());
685  return (-1);
686  }
687  return (leaf_layout_[idx]);
688  }
689 
694  inline void
695  setFilterFieldName (const std::string &field_name)
696  {
697  filter_field_name_ = field_name;
698  }
699 
701  inline std::string const
702  getFilterFieldName ()
703  {
704  return (filter_field_name_);
705  }
706 
711  inline void
712  setFilterLimits (const double &limit_min, const double &limit_max)
713  {
714  filter_limit_min_ = limit_min;
715  filter_limit_max_ = limit_max;
716  }
717 
722  inline void
723  getFilterLimits (double &limit_min, double &limit_max)
724  {
725  limit_min = filter_limit_min_;
726  limit_max = filter_limit_max_;
727  }
728 
733  inline void
734  setFilterLimitsNegative (const bool limit_negative)
735  {
736  filter_limit_negative_ = limit_negative;
737  }
738 
742  inline void
743  getFilterLimitsNegative (bool &limit_negative)
744  {
745  limit_negative = filter_limit_negative_;
746  }
747 
751  inline bool
752  getFilterLimitsNegative ()
753  {
754  return (filter_limit_negative_);
755  }
756 
757  protected:
759  Eigen::Vector4f leaf_size_;
760 
762  Eigen::Array4f inverse_leaf_size_;
763 
765  bool downsample_all_data_;
766 
770  bool save_leaf_layout_;
771 
775  std::vector<int> leaf_layout_;
776 
780  Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
781 
783  std::string filter_field_name_;
784 
786  double filter_limit_min_;
787 
789  double filter_limit_max_;
790 
792  bool filter_limit_negative_;
793 
797  void
798  applyFilter (PointCloud2 &output);
799  };
800 }
801 
802 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
Generated on Sat Aug 24 2013 17:28:33 for Point Cloud Library (PCL) by   doxygen 1.8.4