iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
VoxelFilter.H
Go to the documentation of this file.
1 /*! @file
2  @author Shane Grant
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 #ifdef NRT_HAVE_CLOUD
36 #ifndef INCLUDE_NRT_POINTCLOUD2_FILTER_VOXELFILTER_H
37 #define INCLUDE_NRT_POINTCLOUD2_FILTER_VOXELFILTER_H
38 
39 #include <map>
40 #include <unordered_map>
41 
42 #include <nrt/PointCloud2/Filter/FilterBase.H>
43 #include <nrt/Core/Image/PixelTypes.H>
44 
45 namespace nrt
46 {
47  //! A filter that downsamples an input point cloud into a uniform grid
48  /*! Voxel filters allow large point clouds to be down sampled into smaller
49  more tractable datasets by applying some downsampling function to
50  a group of points that all fall within some geometrical location.
51 
52  The size of the geometry used to cluster points is specified on filter
53  creation.
54 
55  Voxel filters rely on user specified down sampling functions or can
56  be performed using a default downsampling method which makes no guarantees
57  to the validity of any field other than Geometry in the output cloud.
58 
59  See GeometryAverage and GeometryColorAverage for examples on how to
60  specify a downsampling function.
61 
62  @ingroup pointcloud2filters */
63  class VoxelFilter : public FilterBase
64  {
65  typedef PointCloud2::Geometry Geometry;
66 
67  public:
68  //! A struct representing a voxel object
69  struct Voxel
70  {
71  Voxel() : centroid( 0, 0 ), points(), index( -1 ) { }
72 
73  //! The normalized centroid of all points that are part of this voxel
74  Geometry centroid;
75 
76  //! The indices in the input point cloud that correspond to this voxel
77  std::vector<size_t> points;
78 
79  //! Index of this voxel in the output point cloud
80  int index;
81  };
82 
83  //! The function type for downsampling voxels
84  /*! A downsampling function must accept a constant version of the input point cloud,
85  a reference to the output point cloud, and a constant reference to a voxel describing
86  the relationship between points in the input to points in the output cloud. */
87  typedef std::function<void(PointCloud2 const, PointCloud2 &, Voxel const &)> DownSamplingFunc;
88 
89  //! A sample geometry only downsampling function
90  /*! This downsampling function does nothing to ensure that the output has
91  valid entries in any field except its geometry; if the cloud had other
92  dense or sparse fields prior to this, their values are not
93  guaranteed to be valid and should be assumed to be all invalid */
94  static DownSamplingFunc GeometryAverage();
95 
96  //! A sample geometry and color downsampling function
97  /*! This downsampling function does nothing to ensure that the output has
98  valid entries in any field except its geometry and color information; if the cloud had other
99  dense or sparse fields prior to this, their values are not
100  guaranteed to be valid and should be assumed to be all invalid */
101  static DownSamplingFunc GeometryColorAverage();
102 
103  //! Create a voxel filter with a specific voxel grid size
104  VoxelFilter( Geometry const & gridSize );
105 
106  //! Performs default filtering (geometry only, no guarantee on validity of other fields)
107  /*! @param input The cloud to filter
108  @return The filtered cloud */
109  PointCloud2 filter( PointCloud2 const input );
110 
111  //! Filter the input, applying func to every point to see what to do
112  /*! @param input The cloud to filter
113  @param func The filtering function/functor
114  @return The filtered point cloud */
115  PointCloud2 filter( PointCloud2 const input,
116  DownSamplingFunc && func );
117 
118  //! Gets the index of the voxel corresponding to an input point
119  /*! The input point should be one that was in the original point
120  cloud used to create the voxel grid, otherwise the result
121  is not guaranteed to be valid.
122 
123  This result is only valid for the most recently filtered
124  point cloud.
125 
126  @return the index of the point, or -1 if invalid */
127  int getVoxelIndex( Geometry const & point ) const;
128 
129  //! Gets the mapping from voxel coordinates to output cloud indices
130  std::map<int, int> getVoxelIndexMap() const;
131 
132  protected:
133  //! Not yet implemented TODO
134  PointCloud2 filter( PointCloud2 const input, Indices const indices )
135  { return PointCloud2(); }
136 
137  private:
138  typedef Point<int, 4> PointInt;
139 
140  Geometry itsGridSize;
141  std::unordered_map<size_t, Voxel> itsVoxels;
142  PointInt itsMin;
143  PointInt itsMax;
144  PointInt itsMultiplier;
145  }; // class VoxelFilter
146 } // namespace nrt
147 
148 #endif // INCLUDE_NRT_POINTCLOUD2_FILTER_VOXELFILTER_H
149 #endif // NRT_HAVE_CLOUD