nabo_experimental.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_EXPERIMENTAL_H
00033 #define __NABO_EXPERIMENTAL_H
00034 
00035 #include "../nabo/nabo_private.h"
00036 #include "../nabo/index_heap.h"
00037 
00038 namespace Nabo
00039 {
00040         // KDTree, balanced, points in nodes
00041         template<typename T, typename CloudType>
00042         struct KDTreeBalancedPtInNodes:public NearestNeighbourSearch<T, CloudType>
00043         {
00044                 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00045                 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00046                 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00047                 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00048                 
00049         protected:
00050                 struct BuildPoint
00051                 {
00052                         Vector pos;
00053                         size_t index;
00054                         BuildPoint(const Vector& pos =  Vector(), const size_t index = 0): pos(pos), index(index) {}
00055                 };
00056                 typedef std::vector<BuildPoint> BuildPoints;
00057                 typedef typename BuildPoints::iterator BuildPointsIt;
00058                 typedef typename BuildPoints::const_iterator BuildPointsCstIt;
00059                 
00060                 struct CompareDim
00061                 {
00062                         size_t dim;
00063                         CompareDim(const size_t dim):dim(dim){}
00064                         bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
00065                 };
00066                 
00067                 struct Node
00068                 {
00069                         Vector pos;
00070                         int dim; // -1 == leaf, -2 == invalid
00071                         Index index;
00072                         Node(const Vector& pos = Vector(), const int dim = -2, const Index index = 0):pos(pos), dim(dim), index(index) {}
00073                 };
00074                 typedef std::vector<Node> Nodes;
00075                 
00076                 Nodes nodes;
00077                 
00078                 inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
00079                 inline size_t childRight(size_t pos) const { return 2*pos + 2; }
00080                 inline size_t parent(size_t pos) const { return (pos-1)/2; }
00081                 size_t getTreeSize(size_t size) const;
00082                 IndexVector cloudIndexesFromNodesIndexes(const IndexVector& indexes) const;
00083                 void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos);
00084                 void dump(const Vector minValues, const Vector maxValues, const size_t pos) const;
00085                 
00086         protected:
00087                 KDTreeBalancedPtInNodes(const CloudType& cloud);
00088         };
00089         
00090         // KDTree, balanced, points in nodes, priority queue
00091         template<typename T, typename CloudType>
00092         struct KDTreeBalancedPtInNodesPQ: public KDTreeBalancedPtInNodes<T, CloudType>
00093         {
00094                 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00095                 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00096                 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00097                 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00098                 typedef typename KDTreeBalancedPtInNodes<T, CloudType>::Node Node;
00099                 typedef typename KDTreeBalancedPtInNodes<T, CloudType>::Nodes Nodes;
00100                 
00101                 using NearestNeighbourSearch<T, CloudType>::statistics;
00102                 using KDTreeBalancedPtInNodes<T, CloudType>::nodes;
00103                 using KDTreeBalancedPtInNodes<T, CloudType>::childLeft;
00104                 using KDTreeBalancedPtInNodes<T, CloudType>::childRight;
00105                 
00106         protected:
00107                 struct SearchElement
00108                 {
00109                         size_t index;
00110                         T minDist;
00111                         
00112                         SearchElement(const size_t index, const T minDist): index(index), minDist(minDist) {}
00113                         // invert test as std::priority_queue shows biggest element at top
00114                         friend bool operator<(const SearchElement& e0, const SearchElement& e1) { return e0.minDist > e1.minDist; }
00115                 };
00116                 
00117         public:
00118                 KDTreeBalancedPtInNodesPQ(const CloudType& cloud);
00119                 virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
00120         };
00121         
00122         // KDTree, balanced, points in nodes, stack
00123         template<typename T, typename CloudType>
00124         struct KDTreeBalancedPtInNodesStack: public KDTreeBalancedPtInNodes<T, CloudType>
00125         {
00126                 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00127                 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00128                 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00129                 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00130                 typedef typename KDTreeBalancedPtInNodes<T, CloudType>::Node Node;
00131                 typedef typename KDTreeBalancedPtInNodes<T, CloudType>::Nodes Nodes;
00132                 
00133                 using NearestNeighbourSearch<T, CloudType>::statistics;
00134                 using KDTreeBalancedPtInNodes<T, CloudType>::nodes;
00135                 using KDTreeBalancedPtInNodes<T, CloudType>::childLeft;
00136                 using KDTreeBalancedPtInNodes<T, CloudType>::childRight;
00137                 
00138                 typedef IndexHeapSTL<Index, T> Heap;
00139                 
00140         protected:
00141                 void recurseKnn(const Vector& query, const size_t n, T rd, Heap& heap, Vector& off, const T maxError, const bool allowSelfMatch);
00142                 
00143         public:
00144                 KDTreeBalancedPtInNodesStack(const CloudType& cloud);
00145                 virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
00146         };
00147         
00148         
00149         //  KDTree, balanced, points in leaves, stack
00150         template<typename T, typename CloudType>
00151         struct KDTreeBalancedPtInLeavesStack: public NearestNeighbourSearch<T, CloudType>
00152         {
00153                 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00154                 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00155                 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00156                 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00157                 
00158                 using NearestNeighbourSearch<T, CloudType>::statistics;
00159                 using NearestNeighbourSearch<T, CloudType>::cloud;
00160                 using NearestNeighbourSearch<T, CloudType>::minBound;
00161                 using NearestNeighbourSearch<T, CloudType>::maxBound;
00162                 
00163         protected:
00164                 struct BuildPoint
00165                 {
00166                         Vector pos;
00167                         size_t index;
00168                         BuildPoint(const Vector& pos =  Vector(), const size_t index = 0): pos(pos), index(index) {}
00169                 };
00170                 typedef std::vector<BuildPoint> BuildPoints;
00171                 typedef typename BuildPoints::iterator BuildPointsIt;
00172                 typedef typename BuildPoints::const_iterator BuildPointsCstIt;
00173                 
00174                 struct CompareDim
00175                 {
00176                         size_t dim;
00177                         CompareDim(const size_t dim):dim(dim){}
00178                         bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
00179                 };
00180                 
00181                 typedef IndexHeapSTL<Index, T> Heap;
00182                 
00183                 struct Node
00184                 {
00185                         int dim; // -1 == invalid, <= -2 = index of pt
00186                         T cutVal;
00187                         Node(const int dim = -1, const T cutVal = 0):
00188                                 dim(dim), cutVal(cutVal) {}
00189                 };
00190                 typedef std::vector<Node> Nodes;
00191                 
00192                 Nodes nodes;
00193                 
00194                 inline size_t childLeft(size_t pos) const { return 2*pos + 1; }
00195                 inline size_t childRight(size_t pos) const { return 2*pos + 2; }
00196                 inline size_t parent(size_t pos) const { return (pos-1)/2; }
00197                 size_t getTreeSize(size_t size) const;
00198                 void buildNodes(const BuildPointsIt first, const BuildPointsIt last, const size_t pos, const Vector minValues, const Vector maxValues, const bool balanceVariance);
00199                 void recurseKnn(const Vector& query, const size_t n, T rd, Heap& heap, Vector& off, const T maxError, const bool allowSelfMatch);
00200                 
00201         public:
00202                 KDTreeBalancedPtInLeavesStack(const CloudType& cloud, const bool balanceVariance);
00203                 virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
00204         };
00205         
00206         //  KDTree, unbalanced, points in leaves, stack, implicit bounds, ANN_KD_SL_MIDPT
00207         template<typename T, typename Heap, typename CloudType>
00208         struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack: public NearestNeighbourSearch<T, CloudType>
00209         {
00210                 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00211                 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00212                 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00213                 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00214                 typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
00215                 
00216                 using NearestNeighbourSearch<T, CloudType>::statistics;
00217                 using NearestNeighbourSearch<T, CloudType>::cloud;
00218                 using NearestNeighbourSearch<T, CloudType>::minBound;
00219                 using NearestNeighbourSearch<T, CloudType>::maxBound;
00220                 
00221         protected:
00222                 struct BuildPoint
00223                 {
00224                         Vector pos;
00225                         size_t index;
00226                         BuildPoint(const Vector& pos =  Vector(), const size_t index = 0): pos(pos), index(index) {}
00227                 };
00228                 typedef std::vector<BuildPoint> BuildPoints;
00229                 typedef typename BuildPoints::iterator BuildPointsIt;
00230                 typedef typename BuildPoints::const_iterator BuildPointsCstIt;
00231                 
00232                 struct CompareDim
00233                 {
00234                         size_t dim;
00235                         CompareDim(const size_t dim):dim(dim){}
00236                         bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
00237                 };
00238                 
00239                 struct Node
00240                 {
00241                         enum
00242                         {
00243                                 INVALID_CHILD = 0xffffffff,
00244                                 INVALID_PT = 0xffffffff
00245                         };
00246                         unsigned dim;
00247                         unsigned rightChild;
00248                         union
00249                         {
00250                                 T cutVal;
00251                                 unsigned ptIndex;
00252                         };
00253                         
00254                         Node(const int dim, const T cutVal, unsigned rightChild):
00255                                 dim(dim), rightChild(rightChild), cutVal(cutVal) {}
00256                         Node(const unsigned ptIndex = INVALID_PT):
00257                                 dim(0), rightChild(INVALID_CHILD), ptIndex(ptIndex) {}
00258                 };
00259                 typedef std::vector<Node> Nodes;
00260                 
00261                 Nodes nodes;
00262                 
00263                 unsigned buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues);
00264                 void recurseKnn(const Vector& query, const unsigned n, T rd, Heap& heap, Vector& off, const T maxError, const bool allowSelfMatch);
00265                 
00266         public:
00267                 KDTreeUnbalancedPtInLeavesImplicitBoundsStack(const CloudType& cloud);
00268                 virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
00269                 virtual IndexMatrix knnM(const Matrix& query, const Index k, const T epsilon, const unsigned optionFlags);
00270         };
00271         
00272         //  KDTree, unbalanced, points in leaves, stack, explicit bounds, ANN_KD_SL_MIDPT
00273         template<typename T, typename CloudType>
00274         struct KDTreeUnbalancedPtInLeavesExplicitBoundsStack: public NearestNeighbourSearch<T, CloudType>
00275         {
00276                 typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
00277                 typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
00278                 typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
00279                 typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
00280                 
00281                 
00282                 using NearestNeighbourSearch<T, CloudType>::statistics;
00283                 using NearestNeighbourSearch<T, CloudType>::cloud;
00284                 using NearestNeighbourSearch<T, CloudType>::minBound;
00285                 using NearestNeighbourSearch<T, CloudType>::maxBound;
00286                 
00287         protected:
00288                 struct BuildPoint
00289                 {
00290                         Vector pos;
00291                         size_t index;
00292                         BuildPoint(const Vector& pos =  Vector(), const size_t index = 0): pos(pos), index(index) {}
00293                 };
00294                 typedef std::vector<BuildPoint> BuildPoints;
00295                 typedef typename BuildPoints::iterator BuildPointsIt;
00296                 typedef typename BuildPoints::const_iterator BuildPointsCstIt;
00297                 
00298                 struct CompareDim
00299                 {
00300                         size_t dim;
00301                         CompareDim(const size_t dim):dim(dim){}
00302                         bool operator() (const BuildPoint& p0, const BuildPoint& p1) { return p0.pos(dim) < p1.pos(dim); }
00303                 };
00304                 
00305                 typedef IndexHeapSTL<Index, T> Heap;
00306                 
00307                 struct Node
00308                 {
00309                         int dim; // <= -1 = index of pt
00310                         unsigned rightChild;
00311                         T cutVal;
00312                         T lowBound;
00313                         T highBound;
00314                         Node(const int dim = -1, const T cutVal = 0, const T lowBound = 0, const T highBound = 0, unsigned rightChild = 0):
00315                                 dim(dim), rightChild(rightChild), cutVal(cutVal), lowBound(lowBound), highBound(highBound) {}
00316                 };
00317                 typedef std::vector<Node> Nodes;
00318                 
00319                 Nodes nodes;
00320                 
00321                 unsigned buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues);
00322                 void recurseKnn(const Vector& query, const size_t n, T rd, Heap& heap, const T maxError, const bool allowSelfMatch);
00323                 
00324         public:
00325                 KDTreeUnbalancedPtInLeavesExplicitBoundsStack(const CloudType& cloud);
00326                 virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
00327         };
00328 }
00329 
00330 #endif // __NABO_H


libnabo
Author(s): St├ęphane Magnenat
autogenerated on Thu Sep 10 2015 10:54:55