iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Search.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 #ifndef NRT_POINTCLOUD2_SEARCH_SEARCH_H_
36 #define NRT_POINTCLOUD2_SEARCH_SEARCH_H_
37 
38 #include <flann/flann.hpp>
39 #include <nrt/Core/Typing/Enum.H>
42 
43 namespace nrt
44 {
45  NRT_DEFINE_ENUM_CLASS( PointCloudSearchMethod, (knn) (radius) );
46 
47  //! A class for searching point clouds
48  /*! Provides functionality to search through a point
49  cloud based upon an approximate KDTree implementation
50  powered by FLANN. */
51  class Search : public SourceCloud
52  {
53  public:
55  typedef PointCloud2::BaseType BaseType;
56 
57  //! Create an empty search
58  /*! For this search to work, you will need to explicitly call
59  rebuild before performing a search.
60 
61  @param epsilon The minimum required accuracy of the search */
62  Search( double epsilon = 0.0 );
63 
64  //! Create a new search based upon a point cloud
65  /*! The cloud will be indexed as soon as the first
66  search is conducted. If the search type changes,
67  the index will be rebuilt to the latest search field
68  type
69 
70  @param input The point cloud to search
71  @param epsilon The minimum required accuracy of the search */
72  Search( PointCloud2 const input, double epsilon = 0.0 );
73 
74  //! Create a new search based upon a point cloud subset
75  /*! The cloud will be indexed as soon as the first
76  search is conducted. If the search type changes,
77  the index will be rebuilt to the latest search field
78  type
79 
80  @param input The point cloud to search
81  @param indices The subset of the cloud to search through
82  @param epsilon The minimum required accuracy of the search */
83  Search( PointCloud2 const input, Indices const indices, double epsilon = 0.0 );
84 
85  //! Perform a k-nearest neighbor search on the geometry
86  /*! Will rebuild the search index if the previous search
87  type was not geometry.
88 
89  If this search is operating upon a subset, the returned indices will refer to
90  the original cloud and not the subset (e.g. instead of 0 referring to the first
91  subset index, it will refer to the real index of whatever subset[0] represents).
92 
93  @param query The geometrical query
94  @param k The maximum number of neighbors to find
95  @return The indices of the neighbors and their squared distances */
96  std::vector<std::pair<size_t, BaseType>> knn( Geometry const & query, int k );
97 
98  //! Perform a k-nearest neighbor search on a specific field
99  /*! Will rebuild the search index if the previous search
100  type was not Field.
101 
102  If this search is operating upon a subset, the returned indices will refer to
103  the original cloud and not the subset (e.g. instead of 0 referring to the first
104  subset index, it will refer to the real index of whatever subset[0] represents).
105 
106  @param query The specific field query
107  @param k The maximum number of neighbors to find
108  @return The indices of the neighbors and their squared distances */
109  template <class Field>
110  std::vector<std::pair<size_t, BaseType>> knn( Field const & query, int k );
111 
112  //! Perform a k-nearest neighbor search on a specific field using a non-standard Distance functor
113  /*! Will rebuild the search index if the previous search
114  type was not Field or the distance functor changed.
115 
116  If this search is operating upon a subset, the returned indices will refer to
117  the original cloud and not the subset (e.g. instead of 0 referring to the first
118  subset index, it will refer to the real index of whatever subset[0] represents).
119 
120  @note This function is not thread safe
121  @todo revisit and make thread safe
122 
123  @tparam Field The field to query. Can be the geometry as well.
124  @tparam Distance The distance functor type
125 
126  @param query The specific field query
127  @param k The maximum number of neighbors to find
128  @param dist The distance functor (see flann documentation for examples)
129  @return The indices of the neighbors and their squared distances */
130  template <class Field, class Distance>
131  std::vector<std::pair<size_t, BaseType>> knn( Field const & query, int k, Distance const & dist );
132 
133  //! Perform a fixed radius search on the geometry
134  /*! Will rebuild the search index if the previous search
135  type was not geometry.
136 
137  If this search is operating upon a subset, the returned indices will refer to
138  the original cloud and not the subset (e.g. instead of 0 referring to the first
139  subset index, it will refer to the real index of whatever subset[0] represents).
140 
141  @param query The geometrical query
142  @param radius The radius within query to find neighbors
143  @param maxResults The maximum number of neighbors to find. Zero or > cloud size to return maximum.
144  @param sort Whether to sort results by distance (will slow down computation)
145  @return The indices of the neighbors and their squared distances */
146  std::vector<std::pair<size_t, BaseType>> radius( Geometry const & query, double const radius, size_t maxResults = 0, bool sort = false );
147 
148  //! Perform a fixed radius search on a specific field
149  /*! Will rebuild the search index if the previous search
150  type was not Field.
151 
152  If this search is operating upon a subset, the returned indices will refer to
153  the original cloud and not the subset (e.g. instead of 0 referring to the first
154  subset index, it will refer to the real index of whatever subset[0] represents).
155 
156  @param query The specific field query
157  @param radius The radius within query to find neighbors
158  @param maxResults The maximum number of neighbors to find. Zero or > cloud size to return maximum.
159  @param sort Whether to sort results by distance (will slow down computation)
160  @return The indices of the neighbors and their squared distances */
161  template <class Field>
162  std::vector<std::pair<size_t, BaseType>> radius( Field const & query, double const radius, size_t maxResults = 0, bool sort = false );
163 
164  //! Forces a rebuild on the next search to match an updated point cloud
165  /*! Since the cloud we reference may be changed externally, which would cause the
166  external cloud to create its own copy of the data, it may be necessary to
167  update the cloud that search is referencing */
168  void rebuild( PointCloud2 const cloud );
169 
170  //! Forces a rebuild on the next search to match an updated point cloud
171  /*! Since the cloud we reference may be changed externally, which would cause the
172  external cloud to create its own copy of the data, it may be necessary to
173  update the cloud that search is referencing */
174  void rebuild( PointCloud2 const cloud, Indices const indices );
175 
176  //! Convenience function to extract the results of a search as a set of indices
177  static Indices extractIndices( std::vector<std::pair<size_t, BaseType>> const & results );
178 
179  protected:
180 
181  //! Does the actual work for a specific field knn search
182  /*! Will rebuild the search index if the previous search
183  type was not Field or the distance functor changed.
184 
185  If this search is operating upon a subset, the returned indices will refer to
186  the original cloud and not the subset (e.g. instead of 0 referring to the first
187  subset index, it will refer to the real index of whatever subset[0] represents).
188 
189  @tparam Field The field to query. Can be the geometry as well.
190  @tparam Distance The distance functor type
191 
192  @param query The specific field query
193  @param k The maximum number of neighbors to find
194  @param dist The distance functor (see flann documentation for examples)
195  @param indexPtr a unique_ptr that is the sole owner of the FLANN index to use
196  @return The indices of the neighbors and their squared distances */
197  template <class Field, class Distance>
198  std::vector<std::pair<size_t, BaseType>> knnImpl( Field const & query, int k, Distance const & dist,
199  std::unique_ptr<flann::Index<Distance>> & indexPtr );
200 
201  //! Rebuilds FLANN index if necessary
202  /*! @return true if the flann index is valid */
203  bool rebuildFLANNIndex();
204 
205  //! Rebuilds FLANN index if necessary
206  /*! @return true if the flann index is valid */
207  template <class Field>
208  bool rebuildFLANNIndex();
209 
210  //! Rebuilds FLANN index if necessary
211  /*! @return true if the flann index is valid */
212  template <class Field, class Distance>
213  bool rebuildFLANNIndex( Distance const & dist, std::unique_ptr<flann::Index<Distance>> & indexPtr );
214 
215  private:
216  typedef flann::L2_Simple<BaseType> L2SimpleDistance;
217  typedef flann::Index<L2SimpleDistance> FLANNIndex;
218 
219  double itsEpsilon; //!< Approximation factor for search
220 
221  std::unique_ptr<BaseType> itsRawData; //!< Raw data used by FLANN
222 
223  std::unique_ptr<FLANNIndex> itsFLANNIndex; //!< FLANN structure
224 
225  bool itsRebuild; //!< Whether we need to rebuild our index
226 
227  std::string itsRebuildName; //!< The name of our current feature, or blank for geometry
228  };
229 } // namespace nrt
230 
232 
233 #endif // NRT_POINTCLOUD2_SEARCH_SEARCH_H_