libnabo 1.0.1
nabo_private.h
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2010--2011, Stephane Magnenat, ASL, ETHZ, Switzerland
00004 You can contact the author at <stephane at magnenat dot net>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010     * Redistributions of source code must retain the above copyright
00011       notice, this list of conditions and the following disclaimer.
00012     * Redistributions in binary form must reproduce the above copyright
00013       notice, this list of conditions and the following disclaimer in the
00014       documentation and/or other materials provided with the distribution.
00015     * Neither the name of the <organization> nor the
00016       names of its contributors may be used to endorse or promote products
00017       derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 // OpenCL
00045 #ifdef HAVE_OPENCL
00046         #define __CL_ENABLE_EXCEPTIONS
00047         #include "CL/cl.hpp"
00048 #endif // HAVE_OPENCL
00049 
00050 // Unused macro, add support for your favorite compiler
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