libnabo 1.0.1
|
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