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


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