ompl/datastructures/NearestNeighborsGNAT.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Mark Moll, Bryant Gipson */
00036 
00037 #ifndef OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_GNAT_
00038 #define OMPL_DATASTRUCTURES_NEAREST_NEIGHBORS_GNAT_
00039 
00040 #include "ompl/datastructures/NearestNeighbors.h"
00041 #include "ompl/datastructures/GreedyKCenters.h"
00042 #ifdef GNAT_SAMPLER
00043 #include "ompl/datastructures/PDF.h"
00044 #endif
00045 #include "ompl/util/Exception.h"
00046 #include <boost/unordered_set.hpp>
00047 #include <queue>
00048 #include <algorithm>
00049 
00050 namespace ompl
00051 {
00052 
00069     template<typename _T>
00070     class NearestNeighborsGNAT : public NearestNeighbors<_T>
00071     {
00072     protected:
00074         // internally, we use a priority queue for nearest neighbors, paired
00075         // with their distance to the query point
00076         typedef std::pair<const _T*,double> DataDist;
00077         struct DataDistCompare
00078         {
00079             bool operator()(const DataDist& d0, const DataDist& d1)
00080             {
00081                 return d0.second < d1.second;
00082             }
00083         };
00084         typedef std::priority_queue<DataDist, std::vector<DataDist>, DataDistCompare> NearQueue;
00085 
00086         // another internal data structure is a priority queue of nodes to
00087         // check next for possible nearest neighbors
00088         class Node;
00089         typedef std::pair<Node*,double> NodeDist;
00090         struct NodeDistCompare
00091         {
00092             bool operator()(const NodeDist& n0, const NodeDist& n1) const
00093             {
00094                 return (n0.second - n0.first->maxRadius_) > (n1.second - n1.first->maxRadius_);
00095             }
00096         };
00097         typedef std::priority_queue<NodeDist, std::vector<NodeDist>, NodeDistCompare> NodeQueue;
00099 
00100     public:
00101         NearestNeighborsGNAT(unsigned int degree = 4, unsigned int minDegree = 2,
00102             unsigned int maxDegree = 6, unsigned int maxNumPtsPerLeaf = 50,
00103             unsigned int removedCacheSize = 50, bool rebalancing = false
00104 #ifdef GNAT_SAMPLER
00105             , double estimatedDimension = 6.0
00106 #endif
00107             )
00108             : NearestNeighbors<_T>(), tree_(NULL), degree_(degree),
00109             minDegree_(std::min(degree,minDegree)), maxDegree_(std::max(maxDegree,degree)),
00110             maxNumPtsPerLeaf_(maxNumPtsPerLeaf), size_(0),
00111             rebuildSize_(rebalancing ? maxNumPtsPerLeaf*degree : std::numeric_limits<std::size_t>::max()),
00112             removedCacheSize_(removedCacheSize)
00113 #ifdef GNAT_SAMPLER
00114             , estimatedDimension_(estimatedDimension)
00115 #endif
00116         {
00117         }
00118 
00119         virtual ~NearestNeighborsGNAT()
00120         {
00121             if (tree_)
00122                 delete tree_;
00123         }
00125         virtual void setDistanceFunction(const typename NearestNeighbors<_T>::DistanceFunction &distFun)
00126         {
00127             NearestNeighbors<_T>::setDistanceFunction(distFun);
00128             pivotSelector_.setDistanceFunction(distFun);
00129             if (tree_)
00130                 rebuildDataStructure();
00131         }
00132 
00133         virtual void clear()
00134         {
00135             if (tree_)
00136             {
00137                 delete tree_;
00138                 tree_ = NULL;
00139             }
00140             size_ = 0;
00141             removed_.clear();
00142             if (rebuildSize_ != std::numeric_limits<std::size_t>::max())
00143                 rebuildSize_ = maxNumPtsPerLeaf_ * degree_;
00144         }
00145 
00146         virtual bool reportsSortedResults() const
00147         {
00148             return true;
00149         }
00150 
00151         virtual void add(const _T &data)
00152         {
00153             if (tree_)
00154             {
00155                 if (isRemoved(data))
00156                     rebuildDataStructure();
00157                 tree_->add(*this, data);
00158             }
00159             else
00160             {
00161                 tree_ = new Node(degree_, maxNumPtsPerLeaf_, data);
00162                 size_ = 1;
00163             }
00164         }
00165         virtual void add(const std::vector<_T> &data)
00166         {
00167             if (tree_)
00168                 NearestNeighbors<_T>::add(data);
00169             else if (data.size()>0)
00170             {
00171                 tree_ = new Node(degree_, maxNumPtsPerLeaf_, data[0]);
00172 #ifdef GNAT_SAMPLER
00173                 tree_->subtreeSize_= data.size();
00174 #endif
00175                 for (unsigned int i=1; i<data.size(); ++i)
00176                     tree_->data_.push_back(data[i]);
00177                 if (tree_->needToSplit(*this))
00178                     tree_->split(*this);
00179             }
00180             size_ += data.size();
00181         }
00183         void rebuildDataStructure()
00184         {
00185             std::vector<_T> lst;
00186             list(lst);
00187             clear();
00188             add(lst);
00189         }
00195         virtual bool remove(const _T &data)
00196         {
00197             if (!size_) return false;
00198             NearQueue nbhQueue;
00199             // find data in tree
00200             bool isPivot = nearestKInternal(data, 1, nbhQueue);
00201             if (*nbhQueue.top().first != data)
00202                 return false;
00203             removed_.insert(nbhQueue.top().first);
00204             size_--;
00205             // if we removed a pivot or if the capacity of removed elements
00206             // has been reached, we rebuild the entire GNAT
00207             if (isPivot || removed_.size()>=removedCacheSize_)
00208                 rebuildDataStructure();
00209             return true;
00210         }
00211 
00212         virtual _T nearest(const _T &data) const
00213         {
00214             if (size_)
00215             {
00216                 std::vector<_T> nbh;
00217                 nearestK(data, 1, nbh);
00218                 if (!nbh.empty()) return nbh[0];
00219             }
00220             throw Exception("No elements found in nearest neighbors data structure");
00221         }
00222 
00224         virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const
00225         {
00226             nbh.clear();
00227             if (k == 0) return;
00228             if (size_)
00229             {
00230                 NearQueue nbhQueue;
00231                 nearestKInternal(data, k, nbhQueue);
00232                 postprocessNearest(nbhQueue, nbh);
00233             }
00234         }
00235 
00237         virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const
00238         {
00239             nbh.clear();
00240             if (size_)
00241             {
00242                 NearQueue nbhQueue;
00243                 nearestRInternal(data, radius, nbhQueue);
00244                 postprocessNearest(nbhQueue, nbh);
00245             }
00246         }
00247 
00248         virtual std::size_t size() const
00249         {
00250             return size_;
00251         }
00252 
00253 #ifdef GNAT_SAMPLER
00254 
00255         const _T& sample(RNG &rng) const
00256         {
00257             if (!size())
00258                 throw Exception("Cannot sample from an empty tree");
00259             else
00260                 return tree_->sample(*this, rng);
00261         }
00262 #endif
00263 
00264         virtual void list(std::vector<_T> &data) const
00265         {
00266             data.clear();
00267             data.reserve(size());
00268             if (tree_)
00269                 tree_->list(*this, data);
00270         }
00271 
00273         friend std::ostream& operator<<(std::ostream& out, const NearestNeighborsGNAT<_T>& gnat)
00274         {
00275             if (gnat.tree_)
00276             {
00277                 out << *gnat.tree_;
00278                 if (!gnat.removed_.empty())
00279                 {
00280                     out << "Elements marked for removal:\n";
00281                     for (typename boost::unordered_set<const _T*>::const_iterator it = gnat.removed_.begin();
00282                         it != gnat.removed_.end(); it++)
00283                         out << **it << '\t';
00284                     out << std::endl;
00285                 }
00286             }
00287             return out;
00288         }
00289 
00290         // for debugging purposes
00291         void integrityCheck()
00292         {
00293             std::vector<_T> lst;
00294             boost::unordered_set<const _T*> tmp;
00295             // get all elements, including those marked for removal
00296             removed_.swap(tmp);
00297             list(lst);
00298             // check if every element marked for removal is also in the tree
00299             for (typename boost::unordered_set<const _T*>::iterator it=tmp.begin(); it!=tmp.end(); it++)
00300             {
00301                 unsigned int i;
00302                 for (i=0; i<lst.size(); ++i)
00303                     if (lst[i]==**it)
00304                         break;
00305                 if (i == lst.size())
00306                 {
00307                     // an element marked for removal is not actually in the tree
00308                     std::cout << "***** FAIL!! ******\n" << *this << '\n';
00309                     for (unsigned int j=0; j<lst.size(); ++j) std::cout<<lst[j]<<'\t';
00310                     std::cout<<std::endl;
00311                 }
00312                 assert(i != lst.size());
00313             }
00314             // restore
00315             removed_.swap(tmp);
00316             // get elements in the tree with elements marked for removal purged from the list
00317             list(lst);
00318             if (lst.size() != size_)
00319                 std::cout << "#########################################\n" << *this << std::endl;
00320             assert(lst.size() == size_);
00321         }
00322     protected:
00323         typedef NearestNeighborsGNAT<_T> GNAT;
00324 
00326         bool isRemoved(const _T& data) const
00327         {
00328             return !removed_.empty() && removed_.find(&data) != removed_.end();
00329         }
00330 
00335         bool nearestKInternal(const _T &data, std::size_t k, NearQueue& nbhQueue) const
00336         {
00337             bool isPivot;
00338             double dist;
00339             NodeDist nodeDist;
00340             NodeQueue nodeQueue;
00341 
00342             isPivot = tree_->insertNeighborK(nbhQueue, k, tree_->pivot_, data,
00343                 NearestNeighbors<_T>::distFun_(data, tree_->pivot_));
00344             tree_->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
00345             while (nodeQueue.size() > 0)
00346             {
00347                 dist = nbhQueue.top().second; // note the difference with nearestRInternal
00348                 nodeDist = nodeQueue.top();
00349                 nodeQueue.pop();
00350                 if (nbhQueue.size() == k &&
00351                     (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
00352                      nodeDist.second < nodeDist.first->minRadius_ - dist))
00353                     break;
00354                 nodeDist.first->nearestK(*this, data, k, nbhQueue, nodeQueue, isPivot);
00355             }
00356             return isPivot;
00357         }
00359         void nearestRInternal(const _T &data, double radius, NearQueue& nbhQueue) const
00360         {
00361             double dist = radius; // note the difference with nearestKInternal
00362             NodeQueue nodeQueue;
00363             NodeDist nodeDist;
00364 
00365             tree_->insertNeighborR(nbhQueue, radius, tree_->pivot_,
00366                 NearestNeighbors<_T>::distFun_(data, tree_->pivot_));
00367             tree_->nearestR(*this, data, radius, nbhQueue, nodeQueue);
00368             while (nodeQueue.size() > 0)
00369             {
00370                 nodeDist = nodeQueue.top();
00371                 nodeQueue.pop();
00372                 if (nodeDist.second > nodeDist.first->maxRadius_ + dist ||
00373                     nodeDist.second < nodeDist.first->minRadius_ - dist)
00374                     break;
00375                 nodeDist.first->nearestR(*this, data, radius, nbhQueue, nodeQueue);
00376             }
00377         }
00380         void postprocessNearest(NearQueue& nbhQueue, std::vector<_T> &nbh) const
00381         {
00382             typename std::vector<_T>::reverse_iterator it;
00383             nbh.resize(nbhQueue.size());
00384             for (it=nbh.rbegin(); it!=nbh.rend(); it++, nbhQueue.pop())
00385                 *it = *nbhQueue.top().first;
00386         }
00387 
00389         class Node
00390         {
00391         public:
00394             Node(int degree, int capacity, const _T& pivot)
00395                 : degree_(degree), pivot_(pivot),
00396                 minRadius_(std::numeric_limits<double>::infinity()),
00397                 maxRadius_(-minRadius_), minRange_(degree, minRadius_),
00398                 maxRange_(degree, maxRadius_)
00399 #ifdef GNAT_SAMPLER
00400                 , subtreeSize_(1), activity_(0)
00401 #endif
00402             {
00403                 // The "+1" is needed because we add an element before we check whether to split
00404                 data_.reserve(capacity+1);
00405             }
00406 
00407             ~Node()
00408             {
00409                 for (unsigned int i=0; i<children_.size(); ++i)
00410                     delete children_[i];
00411             }
00412 
00415             void updateRadius(double dist)
00416             {
00417                 if (minRadius_ > dist)
00418                     minRadius_ = dist;
00419 #ifndef GNAT_SAMPLER
00420                 if (maxRadius_ < dist)
00421                     maxRadius_ = dist;
00422 #else
00423                 if (maxRadius_ < dist)
00424                 {
00425                     maxRadius_ = dist;
00426                     activity_ = 0;
00427                 }
00428                 else
00429                     activity_ = std::max(-32, activity_ - 1);
00430 #endif
00431             }
00435             void updateRange(unsigned int i, double dist)
00436             {
00437                 if (minRange_[i] > dist)
00438                     minRange_[i] = dist;
00439                 if (maxRange_[i] < dist)
00440                     maxRange_[i] = dist;
00441             }
00443             void add(GNAT& gnat, const _T& data)
00444             {
00445 #ifdef GNAT_SAMPLER
00446                 subtreeSize_++;
00447 #endif
00448                 if (children_.size()==0)
00449                 {
00450                     data_.push_back(data);
00451                     gnat.size_++;
00452                     if (needToSplit(gnat))
00453                     {
00454                         if (gnat.removed_.size() > 0)
00455                             gnat.rebuildDataStructure();
00456                         else if (gnat.size_ >= gnat.rebuildSize_)
00457                         {
00458                             gnat.rebuildSize_ <<= 1;
00459                             gnat.rebuildDataStructure();
00460                         }
00461                         else
00462                             split(gnat);
00463                     }
00464                 }
00465                 else
00466                 {
00467                     std::vector<double> dist(children_.size());
00468                     double minDist = dist[0] = gnat.distFun_(data, children_[0]->pivot_);
00469                     int minInd = 0;
00470 
00471                     for (unsigned int i=1; i<children_.size(); ++i)
00472                         if ((dist[i] = gnat.distFun_(data, children_[i]->pivot_)) < minDist)
00473                         {
00474                             minDist = dist[i];
00475                             minInd = i;
00476                         }
00477                     for (unsigned int i=0; i<children_.size(); ++i)
00478                         children_[i]->updateRange(minInd, dist[i]);
00479                     children_[minInd]->updateRadius(minDist);
00480                     children_[minInd]->add(gnat, data);
00481                 }
00482             }
00484             bool needToSplit(const GNAT& gnat) const
00485             {
00486                 unsigned int sz = data_.size();
00487                 return sz > gnat.maxNumPtsPerLeaf_ && sz > degree_;
00488             }
00492             void split(GNAT& gnat)
00493             {
00494                 std::vector<std::vector<double> > dists;
00495                 std::vector<unsigned int> pivots;
00496 
00497                 children_.reserve(degree_);
00498                 gnat.pivotSelector_.kcenters(data_, degree_, pivots, dists);
00499                 for(unsigned int i=0; i<pivots.size(); i++)
00500                     children_.push_back(new Node(degree_, gnat.maxNumPtsPerLeaf_, data_[pivots[i]]));
00501                 degree_ = pivots.size(); // in case fewer than degree_ pivots were found
00502                 for (unsigned int j=0; j<data_.size(); ++j)
00503                 {
00504                     unsigned int k = 0;
00505                     for (unsigned int i=1; i<degree_; ++i)
00506                         if (dists[j][i] < dists[j][k])
00507                             k = i;
00508                     Node *child = children_[k];
00509                     if (j != pivots[k])
00510                     {
00511                         child->data_.push_back(data_[j]);
00512                         child->updateRadius(dists[j][k]);
00513                     }
00514                     for (unsigned int i=0; i<degree_; ++i)
00515                         children_[i]->updateRange(k, dists[j][i]);
00516                 }
00517 
00518                 for (unsigned int i=0; i<degree_; ++i)
00519                 {
00520                     // make sure degree lies between minDegree_ and maxDegree_
00521                     children_[i]->degree_ = std::min(std::max(
00522                         degree_ * (unsigned int)(children_[i]->data_.size() / data_.size()),
00523                         gnat.minDegree_), gnat.maxDegree_);
00524                     // singleton
00525                     if (children_[i]->minRadius_ >= std::numeric_limits<double>::infinity())
00526                         children_[i]->minRadius_ = children_[i]->maxRadius_ = 0.;
00527 #ifdef GNAT_SAMPLER
00528                     // set subtree size
00529                     children_[i]->subtreeSize_ = children_[i]->data_.size() + 1;
00530 #endif
00531                 }
00532                 // this does more than clear(); it also sets capacity to 0 and frees the memory
00533                 std::vector<_T> tmp;
00534                 data_.swap(tmp);
00535                 // check if new leaves need to be split
00536                 for (unsigned int i=0; i<degree_; ++i)
00537                     if (children_[i]->needToSplit(gnat))
00538                         children_[i]->split(gnat);
00539             }
00540 
00542             bool insertNeighborK(NearQueue& nbh, std::size_t k, const _T& data, const _T& key, double dist) const
00543             {
00544                 if (nbh.size() < k)
00545                 {
00546                     nbh.push(std::make_pair(&data, dist));
00547                     return true;
00548                 }
00549                 else if (dist < nbh.top().second ||
00550                     (dist < std::numeric_limits<double>::epsilon() && data==key))
00551                 {
00552                     nbh.pop();
00553                     nbh.push(std::make_pair(&data, dist));
00554                     return true;
00555                 }
00556                 return false;
00557             }
00558 
00564             void nearestK(const GNAT& gnat, const _T &data, std::size_t k,
00565                 NearQueue& nbh, NodeQueue& nodeQueue, bool &isPivot) const
00566             {
00567                 for (unsigned int i=0; i<data_.size(); ++i)
00568                     if (!gnat.isRemoved(data_[i]))
00569                     {
00570                         if (insertNeighborK(nbh, k, data_[i], data, gnat.distFun_(data, data_[i])))
00571                             isPivot = false;
00572                     }
00573                 if (children_.size() > 0)
00574                 {
00575                     double dist;
00576                     Node *child;
00577                     std::vector<double> distToPivot(children_.size());
00578                     std::vector<int> permutation(children_.size());
00579 
00580                     for (unsigned int i=0; i<permutation.size(); ++i)
00581                         permutation[i] = i;
00582                     std::random_shuffle(permutation.begin(), permutation.end());
00583 
00584                     for (unsigned int i=0; i<children_.size(); ++i)
00585                         if (permutation[i] >= 0)
00586                         {
00587                             child = children_[permutation[i]];
00588                             distToPivot[permutation[i]] = gnat.distFun_(data, child->pivot_);
00589                             if (insertNeighborK(nbh, k, child->pivot_, data, distToPivot[permutation[i]]))
00590                                 isPivot = true;
00591                             if (nbh.size()==k)
00592                             {
00593                                 dist = nbh.top().second; // note difference with nearestR
00594                                 for (unsigned int j=0; j<children_.size(); ++j)
00595                                     if (permutation[j] >=0 && i != j &&
00596                                         (distToPivot[permutation[i]] - dist > child->maxRange_[permutation[j]] ||
00597                                          distToPivot[permutation[i]] + dist < child->minRange_[permutation[j]]))
00598                                         permutation[j] = -1;
00599                             }
00600                         }
00601 
00602                     dist = nbh.top().second;
00603                     for (unsigned int i=0; i<children_.size(); ++i)
00604                         if (permutation[i] >= 0)
00605                         {
00606                             child = children_[permutation[i]];
00607                             if (nbh.size()<k ||
00608                                 (distToPivot[permutation[i]] - dist <= child->maxRadius_ &&
00609                                  distToPivot[permutation[i]] + dist >= child->minRadius_))
00610                                 nodeQueue.push(std::make_pair(child, distToPivot[permutation[i]]));
00611                         }
00612                 }
00613             }
00615             void insertNeighborR(NearQueue& nbh, double r, const _T& data, double dist) const
00616             {
00617                 if (dist <= r)
00618                     nbh.push(std::make_pair(&data, dist));
00619             }
00623             void nearestR(const GNAT& gnat, const _T &data, double r, NearQueue& nbh, NodeQueue& nodeQueue) const
00624             {
00625                 double dist = r; //note difference with nearestK
00626 
00627                 for (unsigned int i=0; i<data_.size(); ++i)
00628                     if (!gnat.isRemoved(data_[i]))
00629                         insertNeighborR(nbh, r, data_[i], gnat.distFun_(data, data_[i]));
00630                 if (children_.size() > 0)
00631                 {
00632                     Node *child;
00633                     std::vector<double> distToPivot(children_.size());
00634                     std::vector<int> permutation(children_.size());
00635 
00636                     for (unsigned int i=0; i<permutation.size(); ++i)
00637                         permutation[i] = i;
00638                     std::random_shuffle(permutation.begin(), permutation.end());
00639 
00640                     for (unsigned int i=0; i<children_.size(); ++i)
00641                         if (permutation[i] >= 0)
00642                         {
00643                             child = children_[permutation[i]];
00644                             distToPivot[i] = gnat.distFun_(data, child->pivot_);
00645                             insertNeighborR(nbh, r, child->pivot_, distToPivot[i]);
00646                             for (unsigned int j=0; j<children_.size(); ++j)
00647                                 if (permutation[j] >=0 && i != j &&
00648                                     (distToPivot[i] - dist > child->maxRange_[permutation[j]] ||
00649                                      distToPivot[i] + dist < child->minRange_[permutation[j]]))
00650                                     permutation[j] = -1;
00651                         }
00652 
00653                     for (unsigned int i=0; i<children_.size(); ++i)
00654                         if (permutation[i] >= 0)
00655                         {
00656                             child = children_[permutation[i]];
00657                             if (distToPivot[i] - dist <= child->maxRadius_ &&
00658                                 distToPivot[i] + dist >= child->minRadius_)
00659                                 nodeQueue.push(std::make_pair(child, distToPivot[i]));
00660                         }
00661                 }
00662             }
00663 
00664 #ifdef GNAT_SAMPLER
00665             double getSamplingWeight(const GNAT& gnat) const
00666             {
00667                 double minR = std::numeric_limits<double>::max();
00668                 for(size_t i = 0; i<minRange_.size(); i++)
00669                     if(minRange_[i] < minR && minRange_[i] > 0.0)
00670                         minR =  minRange_[i];
00671                 minR = std::max(minR, maxRadius_);
00672                 return std::pow(minR, gnat.estimatedDimension_) / (double) subtreeSize_;
00673             }
00674             const _T& sample(const GNAT& gnat, RNG &rng) const
00675             {
00676                 if (children_.size() != 0)
00677                 {
00678                     if (rng.uniform01() < 1./(double) subtreeSize_)
00679                         return pivot_;
00680                     PDF<const Node*> distribution;
00681                     for(unsigned int i = 0; i < children_.size(); ++i)
00682                         distribution.add(children_[i], children_[i]->getSamplingWeight(gnat));
00683                     return distribution.sample(rng.uniform01())->sample(gnat, rng);
00684                 }
00685                 else
00686                 {
00687                     unsigned int i = rng.uniformInt(0, data_.size());
00688                     return (i==data_.size()) ? pivot_ : data_[i];
00689                 }
00690             }
00691 #endif
00692 
00693             void list(const GNAT& gnat, std::vector<_T> &data) const
00694             {
00695                 if (!gnat.isRemoved(pivot_))
00696                     data.push_back(pivot_);
00697                 for (unsigned int i=0; i<data_.size(); ++i)
00698                     if(!gnat.isRemoved(data_[i]))
00699                         data.push_back(data_[i]);
00700                 for (unsigned int i=0; i<children_.size(); ++i)
00701                     children_[i]->list(gnat, data);
00702             }
00703 
00704             friend std::ostream& operator<<(std::ostream& out, const Node &node)
00705             {
00706                 out << "\ndegree:\t" << node.degree_;
00707                 out << "\nminRadius:\t" << node.minRadius_;
00708                 out << "\nmaxRadius:\t" << node.maxRadius_;
00709                 out << "\nminRange:\t";
00710                 for (unsigned int i=0; i<node.minRange_.size(); ++i)
00711                     out << node.minRange_[i] << '\t';
00712                 out << "\nmaxRange: ";
00713                 for (unsigned int i=0; i<node.maxRange_.size(); ++i)
00714                     out << node.maxRange_[i] << '\t';
00715                 out << "\npivot:\t" << node.pivot_;
00716                 out << "\ndata: ";
00717                 for (unsigned int i=0; i<node.data_.size(); ++i)
00718                     out << node.data_[i] << '\t';
00719                 out << "\nthis:\t" << &node;
00720 #ifdef GNAT_SAMPLER
00721                 out << "\nsubtree size:\t" << node.subtreeSize_;
00722                 out << "\nactivity:\t" << node.activity_;
00723 #endif
00724                 out << "\nchildren:\n";
00725                 for (unsigned int i=0; i<node.children_.size(); ++i)
00726                     out << node.children_[i] << '\t';
00727                 out << '\n';
00728                 for (unsigned int i=0; i<node.children_.size(); ++i)
00729                     out << *node.children_[i] << '\n';
00730                 return out;
00731             }
00732 
00734             unsigned int        degree_;
00736             const _T            pivot_;
00738             double              minRadius_;
00740             double              maxRadius_;
00743             std::vector<double> minRange_;
00746             std::vector<double> maxRange_;
00749             std::vector<_T>     data_;
00752             std::vector<Node*>  children_;
00753 #ifdef GNAT_SAMPLER
00754 
00755             unsigned int        subtreeSize_;
00760             int                 activity_;
00761 #endif
00762         };
00763 
00765         Node                           *tree_;
00767         unsigned int                    degree_;
00772         unsigned int                    minDegree_;
00777         unsigned int                    maxDegree_;
00780         unsigned int                    maxNumPtsPerLeaf_;
00782         std::size_t                     size_;
00785         std::size_t                     rebuildSize_;
00789         std::size_t                     removedCacheSize_;
00791         GreedyKCenters<_T>              pivotSelector_;
00793         boost::unordered_set<const _T*> removed_;
00794 #ifdef GNAT_SAMPLER
00795 
00796         double                          estimatedDimension_;
00797 #endif
00798     };
00799 
00800 }
00801 
00802 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines