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, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
00076 struct BruteForceSearch : public NearestNeighbourSearch<T, CloudType>
00077 {
00078 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00079 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00080 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00081 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00082 typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
00083
00084 using NearestNeighbourSearch<T, CloudType>::dim;
00085 using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
00086 using NearestNeighbourSearch<T, CloudType>::checkSizesKnn;
00087 using NearestNeighbourSearch<T, CloudType>::minBound;
00088 using NearestNeighbourSearch<T, CloudType>::maxBound;
00089
00091 BruteForceSearch(const CloudType& 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, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
00098 struct KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt: public NearestNeighbourSearch<T, CloudType>
00099 {
00100 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00101 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00102 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00103 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00104 typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
00105
00106 using NearestNeighbourSearch<T, CloudType>::dim;
00107 using NearestNeighbourSearch<T, CloudType>::cloud;
00108 using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
00109 using NearestNeighbourSearch<T, CloudType>::minBound;
00110 using NearestNeighbourSearch<T, CloudType>::maxBound;
00111 using NearestNeighbourSearch<T, CloudType>::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 CloudType& 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, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
00226 struct OpenCLSearch: public NearestNeighbourSearch<T, CloudType>
00227 {
00228 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00229 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00230 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00231 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00232 typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
00233
00234 using NearestNeighbourSearch<T, CloudType>::dim;
00235 using NearestNeighbourSearch<T, CloudType>::cloud;
00236 using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
00237 using NearestNeighbourSearch<T, CloudType>::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 CloudType& 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 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;
00258 };
00259
00261 template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
00262 struct BruteForceSearchOpenCL: public OpenCLSearch<T, CloudType>
00263 {
00264 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00265 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00266 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00267
00268 using OpenCLSearch<T, CloudType>::initOpenCL;
00269
00271 BruteForceSearchOpenCL(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
00272 };
00273
00275 template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
00276 struct KDTreeBalancedPtInLeavesStackOpenCL: public OpenCLSearch<T, CloudType>
00277 {
00278 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00279 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00280 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00281 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00282 typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
00283
00284 using NearestNeighbourSearch<T, CloudType>::dim;
00285 using NearestNeighbourSearch<T, CloudType>::cloud;
00286 using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
00287 using NearestNeighbourSearch<T, CloudType>::minBound;
00288 using NearestNeighbourSearch<T, CloudType>::maxBound;
00289
00290 using OpenCLSearch<T, CloudType>::context;
00291 using OpenCLSearch<T, CloudType>::knnKernel;
00292
00293 using OpenCLSearch<T, CloudType>::initOpenCL;
00294
00295 protected:
00297 struct BuildPoint
00298 {
00299 Vector pos;
00300 size_t index;
00301
00302 BuildPoint(const Vector& pos = Vector(), const size_t index = 0): pos(pos), index(index) {}
00303 };
00305 typedef std::vector<BuildPoint> BuildPoints;
00307 typedef typename BuildPoints::iterator BuildPointsIt;
00309 typedef typename BuildPoints::const_iterator BuildPointsCstIt;
00310
00312 struct CompareDim
00313 {
00314 size_t dim;
00315
00316 CompareDim(const size_t dim):dim(dim){}
00318 bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
00319 };
00320
00322 struct Node
00323 {
00324 int dim;
00325 T cutVal;
00326
00327 Node(const int dim = -1, const T cutVal = 0):dim(dim), cutVal(cutVal) {}
00328 };
00330 typedef std::vector<Node> Nodes;
00331
00332 Nodes nodes;
00333 cl::Buffer nodesCL;
00334
00335
00336 inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
00337 inline size_t childRight(size_t pos) const { return 2*pos + 2; }
00338 inline size_t parent(size_t pos) const { return (pos-1)/2; }
00339 size_t getTreeDepth(size_t size) const;
00340 size_t getTreeSize(size_t size) const;
00341
00343 void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues);
00344
00345 public:
00347 KDTreeBalancedPtInLeavesStackOpenCL(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
00348 };
00349
00351 template<typename T, typename CloudType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> >
00352 struct KDTreeBalancedPtInNodesStackOpenCL: public OpenCLSearch<T, CloudType>
00353 {
00354 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00355 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00356 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00357 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00358 typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
00359
00360 using NearestNeighbourSearch<T, CloudType>::dim;
00361 using NearestNeighbourSearch<T, CloudType>::cloud;
00362 using NearestNeighbourSearch<T, CloudType>::creationOptionFlags;
00363 using NearestNeighbourSearch<T, CloudType>::minBound;
00364 using NearestNeighbourSearch<T, CloudType>::maxBound;
00365
00366 using OpenCLSearch<T, CloudType>::context;
00367 using OpenCLSearch<T, CloudType>::knnKernel;
00368
00369 using OpenCLSearch<T, CloudType>::initOpenCL;
00370
00371 protected:
00373 typedef Index BuildPoint;
00375 typedef std::vector<BuildPoint> BuildPoints;
00377 typedef typename BuildPoints::iterator BuildPointsIt;
00379 typedef typename BuildPoints::const_iterator BuildPointsCstIt;
00380
00382 struct CompareDim
00383 {
00384 const CloudType& cloud;
00385 size_t dim;
00386
00387 CompareDim(const CloudType& cloud, const size_t dim): cloud(cloud), dim(dim){}
00389 bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return cloud.coeff(dim, p0) < cloud.coeff(dim, p1); }
00390 };
00391
00393 struct Node
00394 {
00395 int dim;
00396 Index index;
00397
00398 Node(const int dim = -2, const Index index = 0):dim(dim), index(index) {}
00399 };
00401 typedef std::vector<Node> Nodes;
00402
00403 Nodes nodes;
00404 cl::Buffer nodesCL;
00405
00406
00407 inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
00408 inline size_t childRight(size_t pos) const { return 2*pos + 2; }
00409 inline size_t parent(size_t pos) const { return (pos-1)/2; }
00410 size_t getTreeDepth(size_t size) const;
00411 size_t getTreeSize(size_t size) const;
00412
00414 void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues);
00415
00416 public:
00418 KDTreeBalancedPtInNodesStackOpenCL(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags, const cl_device_type deviceType);
00419 };
00420
00421 #endif // HAVE_OPENCL
00422
00424 }
00425
00426 #endif // __NABO_H