kdtree_cpu.cpp
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 #include "nabo_private.h"
00033 #include "index_heap.h"
00034 #include <iostream>
00035 #include <stdexcept>
00036 #include <limits>
00037 #include <queue>
00038 #include <algorithm>
00039 #include <utility>
00040 #include <boost/numeric/conversion/bounds.hpp>
00041 #include <boost/limits.hpp>
00042 #include <boost/format.hpp>
00043 
00049 namespace Nabo
00050 {
00052 
00053     
00054     using namespace std;
00055     
00057 
00060     template<typename T>
00061     T getStorageBitCount(T v)
00062     {
00063         for (T i = 0; i < 64; ++i)
00064         {
00065             if (v == 0)
00066                 return i;
00067             v >>= 1;
00068         }
00069         return 64;
00070     }
00071     
00073 
00076     template<typename T>
00077     size_t argMax(const typename NearestNeighbourSearch<T>::Vector& v)
00078     {
00079         T maxVal(0);
00080         size_t maxIdx(0);
00081         for (int i = 0; i < v.size(); ++i)
00082         {
00083             if (v[i] > maxVal)
00084             {
00085                 maxVal = v[i];
00086                 maxIdx = i;
00087             }
00088         }
00089         return maxIdx;
00090     }
00091     
00092     // OPT
00093     template<typename T, typename Heap>
00094     pair<T,T> KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, Heap>::getBounds(const BuildPointsIt first, const BuildPointsIt last, const unsigned dim)
00095     {
00096         T minVal(boost::numeric::bounds<T>::highest());
00097         T maxVal(boost::numeric::bounds<T>::lowest());
00098         
00099         for (BuildPointsCstIt it(first); it != last; ++it)
00100         {
00101             const T val(cloud.coeff(dim, *it));
00102             minVal = min(val, minVal);
00103             maxVal = max(val, maxVal);
00104         }
00105         
00106         return make_pair(minVal, maxVal);
00107     }
00108     
00109     template<typename T, typename Heap>
00110     unsigned KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, Heap>::buildNodes(const BuildPointsIt first, const BuildPointsIt last, const Vector minValues, const Vector maxValues)
00111     {
00112         const int count(last - first);
00113         assert(count >= 1);
00114         const unsigned pos(nodes.size());
00115         
00116         //cerr << count << endl;
00117         if (count <= int(bucketSize))
00118         {
00119             const uint32_t initBucketsSize(buckets.size());
00120             //cerr << "creating bucket with " << count << " values" << endl;
00121             for (int i = 0; i < count; ++i)
00122             {
00123                 const Index index(*(first+i));
00124                 assert(index < cloud.cols());
00125                 buckets.push_back(BucketEntry(&cloud.coeff(0, index), index));
00126                 //cerr << "  " << &cloud.coeff(0, index) << ", " << index << endl;
00127             }
00128             //cerr << "at address " << bucketStart << endl;
00129             nodes.push_back(Node(createDimChildBucketSize(dim, count),initBucketsSize));
00130             return pos;
00131         }
00132         
00133         // find the largest dimension of the box
00134         const unsigned cutDim = argMax<T>(maxValues - minValues);
00135         const T idealCutVal((maxValues(cutDim) + minValues(cutDim))/2);
00136         
00137         // get bounds from actual points
00138         const pair<T,T> minMaxVals(getBounds(first, last, cutDim));
00139         
00140         // correct cut following bounds
00141         T cutVal;
00142         if (idealCutVal < minMaxVals.first)
00143             cutVal = minMaxVals.first;
00144         else if (idealCutVal > minMaxVals.second)
00145             cutVal = minMaxVals.second;
00146         else
00147             cutVal = idealCutVal;
00148         
00149         int l(0);
00150         int r(count-1);
00151         // partition points around cutVal
00152         while (1)
00153         {
00154             while (l < count && cloud.coeff(cutDim, *(first+l)) < cutVal)
00155                 ++l;
00156             while (r >= 0 && cloud.coeff(cutDim, *(first+r)) >= cutVal)
00157                 --r;
00158             if (l > r)
00159                 break;
00160             swap(*(first+l), *(first+r));
00161             ++l; --r;
00162         }
00163         const int br1 = l;  // now: points[0..br1-1] < cutVal <= points[br1..count-1]
00164         r = count-1;
00165         // partition points[br1..count-1] around cutVal
00166         while (1)
00167         {
00168             while (l < count && cloud.coeff(cutDim, *(first+l)) <= cutVal)
00169                 ++l;
00170             while (r >= br1 && cloud.coeff(cutDim, *(first+r)) > cutVal)
00171                 --r;
00172             if (l > r)
00173                 break;
00174             swap(*(first+l), *(first+r));
00175             ++l; --r;
00176         }
00177         const int br2 = l; // now: points[br1..br2-1] == cutVal < points[br2..count-1]
00178         
00179         // find best split index
00180         int leftCount;
00181         if (idealCutVal < minMaxVals.first)
00182             leftCount = 1;
00183         else if (idealCutVal > minMaxVals.second)
00184             leftCount = count-1;
00185         else if (br1 > count / 2)
00186             leftCount = br1;
00187         else if (br2 < count / 2)
00188             leftCount = br2;
00189         else
00190             leftCount = count / 2;
00191         assert(leftCount > 0);
00192         /*if (leftCount >= count)
00193         {
00194             cerr << "Error found in kdtree:" << endl;
00195             cerr << "cloud size: " << cloud.cols() << endl;
00196             cerr << "count:" << count << endl;
00197             cerr << "leftCount: " << leftCount << endl;
00198             cerr << "br1: " << br1 << endl;
00199             cerr << "br2: " << br2 << endl;
00200             cerr << "idealCutVal: " << idealCutVal << endl;
00201             cerr << "cutVal: " << cutVal << endl;
00202             cerr << "minMaxVals.first: " << minMaxVals.first << endl;
00203             cerr << "minMaxVals.second: " << minMaxVals.second << endl;
00204         }*/
00205         assert(leftCount < count);
00206         
00207         // update bounds for left
00208         Vector leftMaxValues(maxValues);
00209         leftMaxValues[cutDim] = cutVal;
00210         // update bounds for right
00211         Vector rightMinValues(minValues);
00212         rightMinValues[cutDim] = cutVal;
00213         
00214         // add this
00215         nodes.push_back(Node(0, cutVal));
00216         
00217         // recurse
00218         const unsigned _UNUSED leftChild = buildNodes(first, first + leftCount, minValues, leftMaxValues);
00219         assert(leftChild == pos + 1);
00220         const unsigned rightChild = buildNodes(first + leftCount, last, rightMinValues, maxValues);
00221         
00222         // write right child index and return
00223         nodes[pos].dimChildBucketSize = createDimChildBucketSize(cutDim, rightChild);
00224         return pos;
00225     }
00226 
00227     template<typename T, typename Heap>
00228     KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, Heap>::KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags, const Parameters& additionalParameters):
00229         NearestNeighbourSearch<T>::NearestNeighbourSearch(cloud, dim, creationOptionFlags),
00230         bucketSize(additionalParameters.get<unsigned>("bucketSize", 8)),
00231         dimBitCount(getStorageBitCount<uint32_t>(this->dim)),
00232         dimMask((1<<dimBitCount)-1)
00233     {
00234         if (bucketSize < 2)
00235             throw runtime_error((boost::format("Requested bucket size %1%, but must be larger than 2") % bucketSize).str());
00236         if (cloud.cols() <= bucketSize)
00237         {
00238             // make a single-bucket tree
00239             for (int i = 0; i < cloud.cols(); ++i)
00240                 buckets.push_back(BucketEntry(&cloud.coeff(0, i), i));
00241             nodes.push_back(Node(createDimChildBucketSize(this->dim, cloud.cols()),uint32_t(0)));
00242             return;
00243         }
00244         
00245         const uint64_t maxNodeCount((1 << (32-dimBitCount)) - 1);
00246         const uint64_t estimatedNodeCount(cloud.cols() / (bucketSize / 2));
00247         if (estimatedNodeCount > maxNodeCount)
00248         {
00249             throw runtime_error((boost::format("Cloud has a risk to have more nodes (%1%) than the kd-tree allows (%2%). The kd-tree has %3% bits for dimensions and %4% bits for node indices") % estimatedNodeCount % maxNodeCount % dimBitCount % (32-dimBitCount)).str());
00250         }
00251         
00252         // build point vector and compute bounds
00253         BuildPoints buildPoints;
00254         buildPoints.reserve(cloud.cols());
00255         for (int i = 0; i < cloud.cols(); ++i)
00256         {
00257             const Vector& v(cloud.block(0,i,this->dim,1));
00258             buildPoints.push_back(i);
00259 #ifdef EIGEN3_API
00260             const_cast<Vector&>(minBound) = minBound.array().min(v.array());
00261             const_cast<Vector&>(maxBound) = maxBound.array().max(v.array());
00262 #else // EIGEN3_API
00263             const_cast<Vector&>(minBound) = minBound.cwise().min(v);
00264             const_cast<Vector&>(maxBound) = maxBound.cwise().max(v);
00265 #endif // EIGEN3_API
00266         }
00267         
00268         // create nodes
00269         buildNodes(buildPoints.begin(), buildPoints.end(), minBound, maxBound);
00270         buildPoints.clear();
00271     }
00272     
00273     template<typename T, typename Heap>
00274     unsigned long KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, Heap>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const
00275     {
00276         checkSizesKnn(query, indices, dists2, k);
00277         
00278         const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T>::ALLOW_SELF_MATCH);
00279         const bool sortResults(optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS);
00280         const bool collectStatistics(creationOptionFlags & NearestNeighbourSearch<T>::TOUCH_STATISTICS);
00281         const T maxRadius2(maxRadius * maxRadius);
00282         const T maxError2((1+epsilon)*(1+epsilon));
00283         
00284         assert(nodes.size() > 0);
00285         Heap heap(k);
00286         std::vector<T> off(dim, 0);
00287         
00288         IndexMatrix result(k, query.cols());
00289         const int colCount(query.cols());
00290         unsigned long leafTouchedCount(0);
00291         for (int i = 0; i < colCount; ++i)
00292         {
00293             leafTouchedCount += onePointKnn(query, indices, dists2, i, heap, off, maxError2, maxRadius2, allowSelfMatch, collectStatistics, sortResults);
00294         }
00295         return leafTouchedCount;
00296     }
00297     
00298     template<typename T, typename Heap>
00299     unsigned long KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, Heap>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k, const T epsilon, const unsigned optionFlags) const
00300     {
00301         checkSizesKnn(query, indices, dists2, k, &maxRadii);
00302         
00303         const bool allowSelfMatch(optionFlags & NearestNeighbourSearch<T>::ALLOW_SELF_MATCH);
00304         const bool sortResults(optionFlags & NearestNeighbourSearch<T>::SORT_RESULTS);
00305         const bool collectStatistics(creationOptionFlags & NearestNeighbourSearch<T>::TOUCH_STATISTICS);
00306         const T maxError2((1+epsilon)*(1+epsilon));
00307         
00308         assert(nodes.size() > 0);
00309         Heap heap(k);
00310         std::vector<T> off(dim, 0);
00311         
00312         IndexMatrix result(k, query.cols());
00313         const int colCount(query.cols());
00314         unsigned long leafTouchedCount(0);
00315         for (int i = 0; i < colCount; ++i)
00316         {
00317             const T maxRadius(maxRadii[i]);
00318             const T maxRadius2(maxRadius * maxRadius);
00319             leafTouchedCount += onePointKnn(query, indices, dists2, i, heap, off, maxError2, maxRadius2, allowSelfMatch, collectStatistics, sortResults);
00320         }
00321         return leafTouchedCount;
00322     }
00323     
00324     template<typename T, typename Heap>
00325     unsigned long KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, Heap>::onePointKnn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, int i, Heap& heap, std::vector<T>& off, const T maxError2, const T maxRadius2, const bool allowSelfMatch, const bool collectStatistics, const bool sortResults) const
00326     {
00327         fill(off.begin(), off.end(), 0);
00328         heap.reset();
00329         unsigned long leafTouchedCount(0);
00330         
00331         if (allowSelfMatch)
00332         {
00333             if (collectStatistics)
00334                 leafTouchedCount += recurseKnn<true, true>(&query.coeff(0, i), 0, 0, heap, off, maxError2, maxRadius2);
00335             else
00336                 recurseKnn<true, false>(&query.coeff(0, i), 0, 0, heap, off, maxError2, maxRadius2);
00337         }
00338         else
00339         {
00340             if (collectStatistics)
00341                 leafTouchedCount += recurseKnn<false, true>(&query.coeff(0, i), 0, 0, heap, off, maxError2, maxRadius2);
00342             else
00343                 recurseKnn<false, false>(&query.coeff(0, i), 0, 0, heap, off, maxError2, maxRadius2);
00344         }
00345         
00346         if (sortResults)
00347             heap.sort();
00348         
00349         heap.getData(indices.col(i), dists2.col(i));
00350         return leafTouchedCount;
00351     }
00352     
00353     template<typename T, typename Heap> template<bool allowSelfMatch, bool collectStatistics>
00354     unsigned long KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<T, Heap>::recurseKnn(const T* query, const unsigned n, T rd, Heap& heap, std::vector<T>& off, const T maxError2, const T maxRadius2) const
00355     {
00356         const Node& node(nodes[n]);
00357         const uint32_t cd(getDim(node.dimChildBucketSize));
00358         
00359         if (cd == uint32_t(dim))
00360         {
00361             //cerr << "entering bucket " << node.bucket << endl;
00362             const BucketEntry* bucket(&buckets[node.bucketIndex]);
00363             const uint32_t bucketSize(getChildBucketSize(node.dimChildBucketSize));
00364             for (uint32_t i = 0; i < bucketSize; ++i)
00365             {
00366                 //cerr << "  " << bucket-> pt << endl;
00367                 //const T dist(dist2<T>(query, cloud.col(index)));
00368                 //const T dist((query - cloud.col(index)).squaredNorm());
00369                 T dist(0);
00370                 const T* qPtr(query);
00371                 const T* dPtr(bucket->pt);
00372                 for (int i = 0; i < this->dim; ++i)
00373                 {
00374                     const T diff(*qPtr - *dPtr);
00375                     dist += diff*diff;
00376                     qPtr++; dPtr++;
00377                 }
00378                 if ((dist <= maxRadius2) &&
00379                     (dist < heap.headValue()) &&
00380                     (allowSelfMatch || (dist > numeric_limits<T>::epsilon()))
00381                 )
00382                     heap.replaceHead(bucket->index, dist);
00383                 ++bucket;
00384             }
00385             return (unsigned long)(bucketSize);
00386         }
00387         else
00388         {
00389             const unsigned rightChild(getChildBucketSize(node.dimChildBucketSize));
00390             unsigned long leafVisitedCount(0);
00391             T& offcd(off[cd]);
00392             //const T old_off(off.coeff(cd));
00393             const T old_off(offcd);
00394             const T new_off(query[cd] - node.cutVal);
00395             if (new_off > 0)
00396             {
00397                 if (collectStatistics)
00398                     leafVisitedCount += recurseKnn<allowSelfMatch, true>(query, rightChild, rd, heap, off, maxError2, maxRadius2);
00399                 else
00400                     recurseKnn<allowSelfMatch, false>(query, rightChild, rd, heap, off, maxError2, maxRadius2);
00401                 rd += - old_off*old_off + new_off*new_off;
00402                 if ((rd <= maxRadius2) &&
00403                     (rd * maxError2 < heap.headValue()))
00404                 {
00405                     offcd = new_off;
00406                     if (collectStatistics)
00407                         leafVisitedCount += recurseKnn<allowSelfMatch, true>(query, n + 1, rd, heap, off, maxError2, maxRadius2);
00408                     else
00409                         recurseKnn<allowSelfMatch, false>(query, n + 1, rd, heap, off, maxError2, maxRadius2);
00410                     offcd = old_off;
00411                 }
00412             }
00413             else
00414             {
00415                 if (collectStatistics)
00416                     leafVisitedCount += recurseKnn<allowSelfMatch, true>(query, n+1, rd, heap, off, maxError2, maxRadius2);
00417                 else
00418                     recurseKnn<allowSelfMatch, false>(query, n+1, rd, heap, off, maxError2, maxRadius2);
00419                 rd += - old_off*old_off + new_off*new_off;
00420                 if ((rd <= maxRadius2) &&
00421                     (rd * maxError2 < heap.headValue()))
00422                 {
00423                     offcd = new_off;
00424                     if (collectStatistics)
00425                         leafVisitedCount += recurseKnn<allowSelfMatch, true>(query, rightChild, rd, heap, off, maxError2, maxRadius2);
00426                     else
00427                         recurseKnn<allowSelfMatch, false>(query, rightChild, rd, heap, off, maxError2, maxRadius2);
00428                     offcd = old_off;
00429                 }
00430             }
00431             return leafVisitedCount;
00432         }
00433     }
00434     
00435     template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<float,IndexHeapSTL<int,float> >;
00436     template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<float,IndexHeapBruteForceVector<int,float> >;
00437     template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<double,IndexHeapSTL<int,double> >;
00438     template struct KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt<double,IndexHeapBruteForceVector<int,double> >;
00439     
00441 }


libnabo
Author(s): Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:27:28