nabo_private.h
Go to the documentation of this file.
1 /*
2 
3 Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
4 You can contact the author at <stephane at magnenat dot net>
5 
6 All rights reserved.
7 
8 Redistribution and use in source and binary forms, with or without
9 modification, are permitted provided that the following conditions are met:
10  * Redistributions of source code must retain the above copyright
11  notice, this list of conditions and the following disclaimer.
12  * Redistributions in binary form must reproduce the above copyright
13  notice, this list of conditions and the following disclaimer in the
14  documentation and/or other materials provided with the distribution.
15  * Neither the name of the <organization> nor the
16  names of its contributors may be used to endorse or promote products
17  derived from this software without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
23 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
24 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
26 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
28 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 
30 */
31 
32 #ifndef __NABO_PRIVATE_H
33 #define __NABO_PRIVATE_H
34 
35 #include "nabo.h"
36 
37 #include <cstdint>
38 using std::uint32_t;
39 
40 // OpenCL
41 #ifdef HAVE_OPENCL
42  #define __CL_ENABLE_EXCEPTIONS
43  #include "CL/cl.hpp"
44 #endif // HAVE_OPENCL
45 
46 // Unused macro, add support for your favorite compiler
47 #if defined(__GNUC__)
48  #define _UNUSED __attribute__ ((unused))
49 #else
50  #define _UNUSED
51 #endif
52 
58 namespace Nabo
59 {
61 
62 
64  template<typename T, typename A, typename B>
65  inline T dist2(const A& v0, const B& v1)
66  {
67  return (v0 - v1).squaredNorm();
68  }
69 
71  template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
72  struct BruteForceSearch : public NearestNeighbourSearch<T, CloudType>
73  {
74  typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
75  typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
76  typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
77  typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
78  typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
79 
80  using NearestNeighbourSearch<T, CloudType>::dim;
81  using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
82  using NearestNeighbourSearch<T, CloudType>::checkSizesKnn;
83  using NearestNeighbourSearch<T, CloudType>::minBound;
84  using NearestNeighbourSearch<T, CloudType>::maxBound;
85 
87  BruteForceSearch(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags);
88  virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
89  virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0) const;
90  };
91 
93  template<typename T, typename Heap, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
94  struct KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt: public NearestNeighbourSearch<T, CloudType>
95  {
96  typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
97  typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
98  typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
99  typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
100  typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
101 
102  using NearestNeighbourSearch<T, CloudType>::dim;
103  using NearestNeighbourSearch<T, CloudType>::cloud;
104  using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
105  using NearestNeighbourSearch<T, CloudType>::minBound;
106  using NearestNeighbourSearch<T, CloudType>::maxBound;
107  using NearestNeighbourSearch<T, CloudType>::checkSizesKnn;
108 
109  protected:
111  typedef std::vector<Index> BuildPoints;
113  typedef typename BuildPoints::iterator BuildPointsIt;
115  typedef typename BuildPoints::const_iterator BuildPointsCstIt;
116 
118  const unsigned bucketSize;
119 
121  const uint32_t dimBitCount;
123  const uint32_t dimMask;
124 
126  inline uint32_t createDimChildBucketSize(const uint32_t dim, const uint32_t childIndex) const
127  { return dim | (childIndex << dimBitCount); }
129  inline uint32_t getDim(const uint32_t dimChildBucketSize) const
130  { return dimChildBucketSize & dimMask; }
132  inline uint32_t getChildBucketSize(const uint32_t dimChildBucketSize) const
133  { return dimChildBucketSize >> dimBitCount; }
134 
135  struct BucketEntry;
136 
138  struct Node
139  {
140  uint32_t dimChildBucketSize;
141  union
142  {
143  T cutVal;
144  uint32_t bucketIndex;
145  };
146 
148  Node(const uint32_t dimChild, const T cutVal):
149  dimChildBucketSize(dimChild), cutVal(cutVal) {}
151  Node(const uint32_t bucketSize, const uint32_t bucketIndex):
152  dimChildBucketSize(bucketSize), bucketIndex(bucketIndex) {}
153  };
155  typedef std::vector<Node> Nodes;
156 
158  struct BucketEntry
159  {
160  const T* pt;
161  Index index;
162 
164 
167  BucketEntry(const T* pt = 0, const Index index = 0): pt(pt), index(index) {}
168  };
169 
171  typedef std::vector<BucketEntry> Buckets;
172 
174  Nodes nodes;
175 
177  Buckets buckets;
178 
180  std::pair<T,T> getBounds(const BuildPointsIt first, const BuildPointsIt last, const unsigned dim);
182  unsigned buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues);
183 
185 
197  unsigned long onePointKnn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, int i, Heap& heap, std::vector<T>& off, const T maxError, const T maxRadius2, const bool allowSelfMatch, const bool collectStatistics, const bool sortResults) const;
198 
200 
208  template<bool allowSelfMatch, bool collectStatistics>
209  unsigned long recurseKnn(const T* query, const unsigned n, T rd, Heap& heap, std::vector<T>& off, const T maxError, const T maxRadius2) const;
210 
211  public:
213  KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters);
214  virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
215  virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0) const;
216  };
217 
218  #ifdef HAVE_OPENCL
219 
221  template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
222  struct OpenCLSearch: public NearestNeighbourSearch<T, CloudType>
223  {
224  typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
225  typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
226  typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
227  typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
228  typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
229 
230  using NearestNeighbourSearch<T, CloudType>::dim;
231  using NearestNeighbourSearch<T, CloudType>::cloud;
232  using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
233  using NearestNeighbourSearch<T, CloudType>::checkSizesKnn;
234 
235  protected:
236  const cl_device_type deviceType;
237  cl::Context& context;
238  mutable cl::Kernel knnKernel;
239  cl::CommandQueue queue;
240  cl::Buffer cloudCL;
241 
243  OpenCLSearch(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
245 
249  void initOpenCL(const char* clFileName, const char* kernelName, const std::string& additionalDefines = "");
250 
251  public:
252  virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
253  virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k = 1, const T epsilon = 0, const unsigned optionFlags = 0) const;
254  };
255 
257  template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
258  struct BruteForceSearchOpenCL: public OpenCLSearch<T, CloudType>
259  {
260  typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
261  typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
262  typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
263 
264  using OpenCLSearch<T, CloudType>::initOpenCL;
265 
267  BruteForceSearchOpenCL(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
268  };
269 
271  template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
272  struct KDTreeBalancedPtInLeavesStackOpenCL: public OpenCLSearch<T, CloudType>
273  {
274  typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
275  typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
276  typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
277  typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
278  typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
279 
280  using NearestNeighbourSearch<T, CloudType>::dim;
281  using NearestNeighbourSearch<T, CloudType>::cloud;
282  using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
283  using NearestNeighbourSearch<T, CloudType>::minBound;
284  using NearestNeighbourSearch<T, CloudType>::maxBound;
285 
286  using OpenCLSearch<T, CloudType>::context;
287  using OpenCLSearch<T, CloudType>::knnKernel;
288 
289  using OpenCLSearch<T, CloudType>::initOpenCL;
290 
291  protected:
293  struct BuildPoint
294  {
295  Vector pos;
296  size_t index;
297  BuildPoint(const Vector& pos = Vector(), const size_t index = 0): pos(pos), index(index) {}
299  };
301  typedef std::vector<BuildPoint> BuildPoints;
303  typedef typename BuildPoints::iterator BuildPointsIt;
305  typedef typename BuildPoints::const_iterator BuildPointsCstIt;
306 
308  struct CompareDim
309  {
310  size_t dim;
311  CompareDim(const size_t dim):dim(dim){}
314  bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
315  };
316 
318  struct Node
319  {
320  int dim;
321  T cutVal;
322  Node(const int dim = -1, const T cutVal = 0):dim(dim), cutVal(cutVal) {}
324  };
326  typedef std::vector<Node> Nodes;
327 
328  Nodes nodes;
329  cl::Buffer nodesCL;
330 
331 
332  inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
333  inline size_t childRight(size_t pos) const { return 2*pos + 2; }
334  inline size_t parent(size_t pos) const { return (pos-1)/2; }
335  size_t getTreeDepth(size_t size) const;
336  size_t getTreeSize(size_t size) const;
337 
339  void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues);
340 
341  public:
343  KDTreeBalancedPtInLeavesStackOpenCL(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
344  };
345 
347  template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
348  struct KDTreeBalancedPtInNodesStackOpenCL: public OpenCLSearch<T, CloudType>
349  {
350  typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
351  typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
352  typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
353  typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
354  typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
355 
356  using NearestNeighbourSearch<T, CloudType>::dim;
357  using NearestNeighbourSearch<T, CloudType>::cloud;
358  using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
359  using NearestNeighbourSearch<T, CloudType>::minBound;
360  using NearestNeighbourSearch<T, CloudType>::maxBound;
361 
362  using OpenCLSearch<T, CloudType>::context;
363  using OpenCLSearch<T, CloudType>::knnKernel;
364 
365  using OpenCLSearch<T, CloudType>::initOpenCL;
366 
367  protected:
369  typedef Index BuildPoint;
371  typedef std::vector<BuildPoint> BuildPoints;
373  typedef typename BuildPoints::iterator BuildPointsIt;
375  typedef typename BuildPoints::const_iterator BuildPointsCstIt;
376 
378  struct CompareDim
379  {
380  const CloudType& cloud;
381  size_t dim;
382  CompareDim(const CloudType& cloud, const size_t dim): cloud(cloud), dim(dim){}
385  bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return cloud.coeff(dim, p0) < cloud.coeff(dim, p1); }
386  };
387 
389  struct Node
390  {
391  int dim;
392  Index index;
393  Node(const int dim = -2, const Index index = 0):dim(dim), index(index) {}
395  };
397  typedef std::vector<Node> Nodes;
398 
399  Nodes nodes;
400  cl::Buffer nodesCL;
401 
402 
403  inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
404  inline size_t childRight(size_t pos) const { return 2*pos + 2; }
405  inline size_t parent(size_t pos) const { return (pos-1)/2; }
406  size_t getTreeDepth(size_t size) const;
407  size_t getTreeSize(size_t size) const;
408 
410  void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues);
411 
412  public:
414  KDTreeBalancedPtInNodesStackOpenCL(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
415  };
416 
417  #endif // HAVE_OPENCL
418 
420 }
421 
422 #endif // __NABO_H
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::Nodes
std::vector< Node > Nodes
dense vector of search nodes, provides better memory performances than many small objects ...
Definition: nabo_private.h:155
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::createDimChildBucketSize
uint32_t createDimChildBucketSize(const uint32_t dim, const uint32_t childIndex) const
create the compound index containing the dimension and the index of the child or the bucket size ...
Definition: nabo_private.h:126
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt
KDTree, unbalanced, points in leaves, stack, implicit bounds, ANN_KD_SL_MIDPT, optimised implementati...
Definition: nabo_private.h:94
Nabo::NearestNeighbourSearch< T, CloudType >::creationOptionFlags
const unsigned creationOptionFlags
creation options
Definition: nabo.h:278
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::IndexMatrix
NearestNeighbourSearch< T, CloudType >::IndexMatrix IndexMatrix
Definition: nabo_private.h:100
Nabo::BruteForceSearch::IndexMatrix
NearestNeighbourSearch< T, CloudType >::IndexMatrix IndexMatrix
Definition: nabo_private.h:78
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::Index
NearestNeighbourSearch< T, CloudType >::Index Index
Definition: nabo_private.h:98
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::BuildPoints
std::vector< Index > BuildPoints
indices of points during kd-tree construction
Definition: nabo_private.h:111
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::Vector
NearestNeighbourSearch< T, CloudType >::Vector Vector
Definition: nabo_private.h:96
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::Buckets
std::vector< BucketEntry > Buckets
bucket data
Definition: nabo_private.h:171
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::BuildPointsIt
BuildPoints::iterator BuildPointsIt
iterator to indices of points during kd-tree construction
Definition: nabo_private.h:113
Nabo::NearestNeighbourSearch
Nearest neighbour search interface, templatized on scalar type.
Definition: nabo.h:258
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::Node::Node
Node(const uint32_t bucketSize, const uint32_t bucketIndex)
construct a leaf node
Definition: nabo_private.h:151
Nabo::BruteForceSearch::Vector
NearestNeighbourSearch< T, CloudType >::Vector Vector
Definition: nabo_private.h:74
Nabo::NearestNeighbourSearch< T, CloudType >::cloud
const CloudType & cloud
the reference to the data-point cloud, which must remain valid during the lifetime of the NearestNeig...
Definition: nabo.h:274
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::Matrix
NearestNeighbourSearch< T, CloudType >::Matrix Matrix
Definition: nabo_private.h:97
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::IndexVector
NearestNeighbourSearch< T, CloudType >::IndexVector IndexVector
Definition: nabo_private.h:99
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::getDim
uint32_t getDim(const uint32_t dimChildBucketSize) const
get the dimension out of the compound index
Definition: nabo_private.h:129
Nabo::BruteForceSearch::IndexVector
NearestNeighbourSearch< T, CloudType >::IndexVector IndexVector
Definition: nabo_private.h:77
Nabo::BruteForceSearch
Brute-force nearest neighbour.
Definition: nabo_private.h:72
Nabo::Parameters
Parameter vector.
Definition: nabo.h:231
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::Node::Node
Node(const uint32_t dimChild, const T cutVal)
construct a split node
Definition: nabo_private.h:148
Nabo::BruteForceSearch::Matrix
NearestNeighbourSearch< T, CloudType >::Matrix Matrix
Definition: nabo_private.h:75
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::getChildBucketSize
uint32_t getChildBucketSize(const uint32_t dimChildBucketSize) const
get the child index or the bucket size out of the coumpount index
Definition: nabo_private.h:132
Nabo
Namespace for Nabo.
Definition: experimental/kdtree_cpu.cpp:40
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::BucketEntry::BucketEntry
BucketEntry(const T *pt=0, const Index index=0)
create a new bucket entry for a point in the data
Definition: nabo_private.h:167
nabo.h
public interface
Nabo::NearestNeighbourSearch< T, CloudType >::dim
const Index dim
the dimensionality of the data-point cloud
Definition: nabo.h:276
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::BuildPointsCstIt
BuildPoints::const_iterator BuildPointsCstIt
const-iterator to indices of points during kd-tree construction
Definition: nabo_private.h:115
Nabo::BruteForceSearch::knn
virtual unsigned long knn(const Matrix &query, IndexMatrix &indices, Matrix &dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const
Definition: brute_force_cpu.cpp:64
Nabo::dist2
T dist2(const A &v0, const B &v1)
Euclidean distance.
Definition: nabo_private.h:65
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::Node::bucketIndex
uint32_t bucketIndex
for leaf node, pointer to bucket
Definition: nabo_private.h:144
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::Node::dimChildBucketSize
uint32_t dimChildBucketSize
cut dimension for split nodes (dimBitCount lsb), index of right node or number of bucket(rest)...
Definition: nabo_private.h:140
Nabo::BruteForceSearch::BruteForceSearch
BruteForceSearch(const CloudType &cloud, const Index dim, const unsigned creationOptionFlags)
constructor, calls NearestNeighbourSearch<T>(cloud)
Definition: brute_force_cpu.cpp:45
Nabo::NearestNeighbourSearch< T, CloudType >::CloudType
CloudType CloudType
a column-major Eigen matrix in which each column is a point; this matrix has dim rows ...
Definition: nabo.h:265
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::BucketEntry::pt
const T * pt
pointer to first value of point data, 0 if end of bucket
Definition: nabo_private.h:160
Nabo::BruteForceSearch::Index
NearestNeighbourSearch< T, CloudType >::Index Index
Definition: nabo_private.h:76
Nabo::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt::dimBitCount
const uint32_t dimBitCount
number of bits required to store dimension index + number of dimensions
Definition: nabo_private.h:121

libnabo
Author(s): Stéphane Magnenat
autogenerated on Mon Feb 28 2022 22:41:38