iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
SearchImpl.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_DETAILS_SEARCHIMPL_H_
36 #define NRT_POINTCLOUD2_SEARCH_DETAILS_SEARCHIMPL_H_
37 
39 
40 template <class Field> inline
41 auto nrt::Search::knn( Field const & query, int k ) -> std::vector<std::pair<size_t, BaseType>>
42 {
43  return knnImpl<Field, L2SimpleDistance>( query, k, L2SimpleDistance(), itsFLANNIndex );
44 }
45 
46 template <class Field, class Distance> inline
47 auto nrt::Search::knn( Field const & query, int k, Distance const & dist ) -> std::vector<std::pair<size_t, BaseType>>
48 {
49  static std::unique_ptr<flann::Index<Distance>> localFLANNIndex;
50 
51  return knnImpl<Field, Distance>( query, k, dist, localFLANNIndex );
52 }
53 
54 template <class Field, class Distance> inline
55 auto nrt::Search::knnImpl( Field const & query, int k, Distance const & dist, std::unique_ptr<flann::Index<Distance>> & indexPtr ) -> std::vector<std::pair<size_t, BaseType>>
56 {
57  if( k <= 0 || !this->template rebuildFLANNIndex<Field, Distance>( dist, indexPtr ) )
58  return {};
59 
60  const size_t cloudSize = SourceCloud::useSubset ? SourceCloud::indices.size() : SourceCloud::cloud.template size<Field>();
61  const size_t dimension = PointHelpers<const Field>::dimension( SourceCloud::cloud.template const_begin<Field>()->template get<Field>() );
62 
63  if( static_cast<size_t>( k ) > cloudSize )
64  k = cloudSize;
65 
66  std::vector<std::pair<size_t, BaseType>> output( k );
67 
68  // temporary storage for results and query
69  std::vector<BaseType> queryTemp( dimension );
70  std::vector<int> indices( k );
71  std::vector<BaseType> distances( k );
72 
73  // copy query into local storage
74  PointHelpers<Field>::toArray( query, queryTemp.data() );
75 
76  // flann matrix headers
77  ::flann::Matrix<BaseType> queryMatrix( queryTemp.data(), 1, dimension );
78  ::flann::Matrix<int> indicesMatrix( indices.data(), 1, k );
79  ::flann::Matrix<BaseType> distancesMatrix( distances.data(), 1, k );
80 
81  // actual search
82  indexPtr->knnSearch( queryMatrix, indicesMatrix, distancesMatrix, k,
83  flann::SearchParams( -1, itsEpsilon ) );
84 
85  // put into our output format
86  for( int i = 0; i < k; ++i )
87  {
88  output[i].first = indices[i];
89  output[i].second = distances[i];
90  }
91 
92  // If we used a subset, we need to map back onto real cloud coordinates
93  if( SourceCloud::useSubset )
94  {
95  for( int i = 0; i < k; ++i )
96  output[i].first = SourceCloud::indices.at( output[i].first );
97  }
98 
99  // if the data was sparse we'll need to fix the indices map
100  if( SourceCloud::cloud.template hasSparseField<Field>() )
101  {
102  auto sparseIndices = SourceCloud::cloud.template getSparseIndices<Field>();
103 
104  for( int i = 0; i < k; ++i )
105  output[i].first = sparseIndices[output[i].first];
106  }
107 
108  return output;
109 }
110 
111 template <class Field> inline
112 auto nrt::Search::radius( Field const & query, double const radius, size_t maxResults, bool sort ) -> std::vector<std::pair<size_t, BaseType>>
113 {
114  if( radius < 0.0 || !this->template rebuildFLANNIndex<Field>() )
115  return std::vector<std::pair<size_t, BaseType>>();
116 
117  const size_t cloudSize = SourceCloud::useSubset ? SourceCloud::indices.size() : SourceCloud::cloud.template size<Field>();
118  const size_t dimension = PointHelpers<const Field>::dimension( SourceCloud::cloud.template const_begin<Field>()->template get<Field>() );
119 
120  // check size of maxResults
121  if( maxResults == 0 || maxResults > cloudSize )
122  maxResults = cloudSize;
123 
124  // establish parameters
125  ::flann::SearchParams params( -1, itsEpsilon, sort );
126  if( maxResults == cloudSize )
127  params.max_neighbors = -1;
128  else
129  params.max_neighbors = maxResults;
130 
131  // temporary storage for results and query
132  std::vector<BaseType> queryTemp( dimension );
133  std::vector<std::vector<int>> indices( 1 );
134  std::vector<std::vector<BaseType>> distances( 1 );
135 
136  // copy query into local storage
137  PointHelpers<Field>::toArray( query, queryTemp.data() );
138 
139  // matrix headers
140  ::flann::Matrix<BaseType> queryMatrix( queryTemp.data(), 1, dimension );
141 
142  // actual search
143  int numNeighbors = itsFLANNIndex->radiusSearch( queryMatrix, indices, distances,
144  static_cast<BaseType>( radius * radius ),
145  params );
146 
147  // prepare output format
148  std::vector<std::pair<size_t, BaseType>> output( numNeighbors );
149 
150  for( size_t i = 0; i < indices[0].size(); ++i )
151  {
152  output[i].first = indices[0][i];
153  output[i].second = distances[0][i];
154  }
155 
156  // If we used a subset, we need to map back onto real cloud coordinates
157  if( SourceCloud::useSubset )
158  {
159  for( size_t i = 0; i < indices[0].size(); ++i )
160  output[i].first = SourceCloud::indices.at( output[i].first );
161  }
162 
163  // if the data was sparse we'll need to fix the indices map
164  if( SourceCloud::cloud.template hasSparseField<Field>() )
165  {
166  auto sparseIndices = SourceCloud::cloud.template getSparseIndices<Field>();
167 
168  for( size_t i = 0; i < indices[0].size(); ++i )
169  output[i].first = sparseIndices[indices[0][i]];
170  }
171 
172  return output;
173 }
174 
175 template <class Field> inline
177 {
178  return rebuildFLANNIndex<Field, L2SimpleDistance>( L2SimpleDistance(), itsFLANNIndex );
179 }
180 
181 template <class Field, class Distance> inline
182 bool nrt::Search::rebuildFLANNIndex( Distance const & dist, std::unique_ptr<flann::Index<Distance>> & indexPtr )
183 {
184  typedef flann::Index<Distance> LocalFLANNIndex;
185  const static std::string name = demangledName<typename StripSparse<Field>::type>() + demangledName<Distance>();
186 
187  // check if we need to rebuild: rebuild flag set or type change
188  if( !itsRebuild && itsRebuildName == name )
189  return true;
190 
191  auto cloudIter = SourceCloud::useSubset ? SourceCloud::cloud.template subset_const_begin<Field>( SourceCloud::indices ) :
192  SourceCloud::cloud.template const_begin<Field>();
193  auto cloudEnd = SourceCloud::useSubset ? SourceCloud::cloud.template subset_const_end<Field>( SourceCloud::indices ) :
194  SourceCloud::cloud.template const_end<Field>();
195 
196  const size_t size = std::distance( cloudIter, cloudEnd );
197 
198  if( size == 0 )
199  return false;
200 
201  const size_t dimension = PointHelpers<const Field>::dimension( cloudIter->template get<Field>() );
202 
203  // reset the size of the raw data
204  itsRawData.reset( new BaseType[size * dimension] );
205 
206  // fill out that raw data
207  for( BaseType * ptr = itsRawData.get(); cloudIter != cloudEnd; ++cloudIter, ptr += dimension )
208  PointHelpers<Field>::toArray( cloudIter->template get<Field>(), ptr );
209 
210  // initialize the flann index
211  indexPtr.reset( new LocalFLANNIndex( flann::Matrix<BaseType>( itsRawData.get(), size, dimension ),
212  flann::KDTreeSingleIndexParams( 15 ),
213  dist ) );
214 
215  indexPtr->buildIndex();
216 
217  itsRebuild = false;
218  itsRebuildName = name;
219 
220  return true;
221 }
222 
223 #endif // NRT_POINTCLOUD2_SEARCH_DETAILS_SEARCHIMPL_H_