00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef __NABO_PRIVATE_H
00033 #define __NABO_PRIVATE_H
00034
00035 #include "nabo.h"
00036
00037 #ifdef BOOST_STDINT
00038 #include <boost/cstdint.hpp>
00039 using boost::uint32_t;
00040 #else // BOOST_STDINT
00041 #include <stdint.h>
00042 #endif // BOOST_STDINT
00043
00044
00045 #ifdef HAVE_OPENCL
00046 #define __CL_ENABLE_EXCEPTIONS
00047 #include "CL/cl.hpp"
00048 #endif // HAVE_OPENCL
00049
00050
00051 #if defined(__GNUC__)
00052 #define _UNUSED __attribute__ ((unused))
00053 #else
00054 #define _UNUSED
00055 #endif
00056
00062 namespace Nabo
00063 {
00065
00066
00068 template<typename T, typename A, typename B>
00069 inline T dist2(const A& v0, const B& v1)
00070 {
00071 return (v0 - v1).squaredNorm();
00072 }
00073
00075 template<typename T>
00076 struct BruteForceSearch: public NearestNeighbourSearch<T>
00077 {
00078 typedef typename NearestNeighbourSearch<T>::Vector Vector;
00079 typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
00080 typedef typename NearestNeighbourSearch<T>::Index Index;
00081 typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
00082 typedef typename NearestNeighbourSearch<T>::IndexMatrix IndexMatrix;
00083
00084 using NearestNeighbourSearch<T>::dim;
00085 using NearestNeighbourSearch<T>::creationOptionFlags;
00086 using NearestNeighbourSearch<T>::checkSizesKnn;
00087 using NearestNeighbourSearch<T>::minBound;
00088 using NearestNeighbourSearch<T>::maxBound;
00089
00091 BruteForceSearch(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags);
00092 virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
00093 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;
00094 };
00095
00097 template<typename T, typename Heap>
00098 struct KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt: public NearestNeighbourSearch<T>
00099 {
00100 typedef typename NearestNeighbourSearch<T>::Vector Vector;
00101 typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
00102 typedef typename NearestNeighbourSearch<T>::Index Index;
00103 typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
00104 typedef typename NearestNeighbourSearch<T>::IndexMatrix IndexMatrix;
00105
00106 using NearestNeighbourSearch<T>::dim;
00107 using NearestNeighbourSearch<T>::cloud;
00108 using NearestNeighbourSearch<T>::creationOptionFlags;
00109 using NearestNeighbourSearch<T>::minBound;
00110 using NearestNeighbourSearch<T>::maxBound;
00111 using NearestNeighbourSearch<T>::checkSizesKnn;
00112
00113 protected:
00115 typedef std::vector<Index> BuildPoints;
00117 typedef typename BuildPoints::iterator BuildPointsIt;
00119 typedef typename BuildPoints::const_iterator BuildPointsCstIt;
00120
00122 const unsigned bucketSize;
00123
00125 const uint32_t dimBitCount;
00127 const uint32_t dimMask;
00128
00130 inline uint32_t createDimChildBucketSize(const uint32_t dim, const uint32_t childIndex) const
00131 { return dim | (childIndex << dimBitCount); }
00133 inline uint32_t getDim(const uint32_t dimChildBucketSize) const
00134 { return dimChildBucketSize & dimMask; }
00136 inline uint32_t getChildBucketSize(const uint32_t dimChildBucketSize) const
00137 { return dimChildBucketSize >> dimBitCount; }
00138
00139 struct BucketEntry;
00140
00142 struct Node
00143 {
00144 uint32_t dimChildBucketSize;
00145 union
00146 {
00147 T cutVal;
00148 uint32_t bucketIndex;
00149 };
00150
00152 Node(const uint32_t dimChild, const T cutVal):
00153 dimChildBucketSize(dimChild), cutVal(cutVal) {}
00155 Node(const uint32_t bucketSize, const uint32_t bucketIndex):
00156 dimChildBucketSize(bucketSize), bucketIndex(bucketIndex) {}
00157 };
00159 typedef std::vector<Node> Nodes;
00160
00162 struct BucketEntry
00163 {
00164 const T* pt;
00165 Index index;
00166
00168
00171 BucketEntry(const T* pt = 0, const Index index = 0): pt(pt), index(index) {}
00172 };
00173
00175 typedef std::vector<BucketEntry> Buckets;
00176
00178 Nodes nodes;
00179
00181 Buckets buckets;
00182
00184 std::pair<T,T> getBounds(const BuildPointsIt first, const BuildPointsIt last, const unsigned dim);
00186 unsigned buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues);
00187
00189
00201 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;
00202
00204
00212 template<bool allowSelfMatch, bool collectStatistics>
00213 unsigned long recurseKnn(const T* query, const unsigned n, T rd, Heap& heap, std::vector<T>& off, const T maxError, const T maxRadius2) const;
00214
00215 public:
00217 KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters);
00218 virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
00219 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;
00220 };
00221
00222 #ifdef HAVE_OPENCL
00223
00225 template<typename T>
00226 struct OpenCLSearch: public NearestNeighbourSearch<T>
00227 {
00228 typedef typename NearestNeighbourSearch<T>::Vector Vector;
00229 typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
00230 typedef typename NearestNeighbourSearch<T>::Index Index;
00231 typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
00232 typedef typename NearestNeighbourSearch<T>::IndexMatrix IndexMatrix;
00233
00234 using NearestNeighbourSearch<T>::dim;
00235 using NearestNeighbourSearch<T>::cloud;
00236 using NearestNeighbourSearch<T>::creationOptionFlags;
00237 using NearestNeighbourSearch<T>::checkSizesKnn;
00238
00239 protected:
00240 const cl_device_type deviceType;
00241 cl::Context& context;
00242 mutable cl::Kernel knnKernel;
00243 cl::CommandQueue queue;
00244 cl::Buffer cloudCL;
00245
00247 OpenCLSearch(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
00249
00253 void initOpenCL(const char* clFileName, const char* kernelName, const std::string& additionalDefines = "");
00254
00255 public:
00256 virtual unsigned long knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const;
00257 };
00258
00260 template<typename T>
00261 struct BruteForceSearchOpenCL: public OpenCLSearch<T>
00262 {
00263 typedef typename NearestNeighbourSearch<T>::Vector Vector;
00264 typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
00265 typedef typename NearestNeighbourSearch<T>::Index Index;
00266
00267 using OpenCLSearch<T>::initOpenCL;
00268
00270 BruteForceSearchOpenCL(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
00271 };
00272
00274 template<typename T>
00275 struct KDTreeBalancedPtInLeavesStackOpenCL: public OpenCLSearch<T>
00276 {
00277 typedef typename NearestNeighbourSearch<T>::Vector Vector;
00278 typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
00279 typedef typename NearestNeighbourSearch<T>::Index Index;
00280 typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
00281 typedef typename NearestNeighbourSearch<T>::IndexMatrix IndexMatrix;
00282
00283 using NearestNeighbourSearch<T>::dim;
00284 using NearestNeighbourSearch<T>::cloud;
00285 using NearestNeighbourSearch<T>::creationOptionFlags;
00286 using NearestNeighbourSearch<T>::minBound;
00287 using NearestNeighbourSearch<T>::maxBound;
00288
00289 using OpenCLSearch<T>::context;
00290 using OpenCLSearch<T>::knnKernel;
00291
00292 using OpenCLSearch<T>::initOpenCL;
00293
00294 protected:
00296 struct BuildPoint
00297 {
00298 Vector pos;
00299 size_t index;
00300
00301 BuildPoint(const Vector& pos = Vector(), const size_t index = 0): pos(pos), index(index) {}
00302 };
00304 typedef std::vector<BuildPoint> BuildPoints;
00306 typedef typename BuildPoints::iterator BuildPointsIt;
00308 typedef typename BuildPoints::const_iterator BuildPointsCstIt;
00309
00311 struct CompareDim
00312 {
00313 size_t dim;
00314
00315 CompareDim(const size_t dim):dim(dim){}
00317 bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
00318 };
00319
00321 struct Node
00322 {
00323 int dim;
00324 T cutVal;
00325
00326 Node(const int dim = -1, const T cutVal = 0):dim(dim), cutVal(cutVal) {}
00327 };
00329 typedef std::vector<Node> Nodes;
00330
00331 Nodes nodes;
00332 cl::Buffer nodesCL;
00333
00334
00335 inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
00336 inline size_t childRight(size_t pos) const { return 2*pos + 2; }
00337 inline size_t parent(size_t pos) const { return (pos-1)/2; }
00338 size_t getTreeDepth(size_t size) const;
00339 size_t getTreeSize(size_t size) const;
00340
00342 void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues);
00343
00344 public:
00346 KDTreeBalancedPtInLeavesStackOpenCL(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
00347 };
00348
00350 template<typename T>
00351 struct KDTreeBalancedPtInNodesStackOpenCL: public OpenCLSearch<T>
00352 {
00353 typedef typename NearestNeighbourSearch<T>::Vector Vector;
00354 typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
00355 typedef typename NearestNeighbourSearch<T>::Index Index;
00356 typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
00357 typedef typename NearestNeighbourSearch<T>::IndexMatrix IndexMatrix;
00358
00359 using NearestNeighbourSearch<T>::dim;
00360 using NearestNeighbourSearch<T>::cloud;
00361 using NearestNeighbourSearch<T>::creationOptionFlags;
00362 using NearestNeighbourSearch<T>::minBound;
00363 using NearestNeighbourSearch<T>::maxBound;
00364
00365 using OpenCLSearch<T>::context;
00366 using OpenCLSearch<T>::knnKernel;
00367
00368 using OpenCLSearch<T>::initOpenCL;
00369
00370 protected:
00372 typedef Index BuildPoint;
00374 typedef std::vector<BuildPoint> BuildPoints;
00376 typedef typename BuildPoints::iterator BuildPointsIt;
00378 typedef typename BuildPoints::const_iterator BuildPointsCstIt;
00379
00381 struct CompareDim
00382 {
00383 const Matrix& cloud;
00384 size_t dim;
00385
00386 CompareDim(const Matrix& cloud, const size_t dim): cloud(cloud), dim(dim){}
00388 bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return cloud.coeff(dim, p0) < cloud.coeff(dim, p1); }
00389 };
00390
00392 struct Node
00393 {
00394 int dim;
00395 Index index;
00396
00397 Node(const int dim = -2, const Index index = 0):dim(dim), index(index) {}
00398 };
00400 typedef std::vector<Node> Nodes;
00401
00402 Nodes nodes;
00403 cl::Buffer nodesCL;
00404
00405
00406 inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
00407 inline size_t childRight(size_t pos) const { return 2*pos + 2; }
00408 inline size_t parent(size_t pos) const { return (pos-1)/2; }
00409 size_t getTreeDepth(size_t size) const;
00410 size_t getTreeSize(size_t size) const;
00411
00413 void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues);
00414
00415 public:
00417 KDTreeBalancedPtInNodesStackOpenCL(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
00418 };
00419
00420 #endif // HAVE_OPENCL
00421
00423 }
00424
00425 #endif // __NABO_H