ReClaM: CoverTree.cpp Source File

00001 
00002 #include <ReClaM/CoverTree.h>
00003 
00004 
00005 #define POINTSCALE -1022
00006 
00007 
00008 union IntWiseDouble
00009 {
00010     double d;
00011     int i[2];
00012 };
00013 
00014 // return 2^(exponent+1)
00015 double i_pow2(int exponent)
00016 {
00017     IntWiseDouble ret;
00018     ret.d = 2.0;
00019     ret.i[1] += (exponent << 20);
00020     return ret.d;
00021 //  return pow(2.0, exponent + 1.0);
00022 }
00023 
00024 int i_log2(double dist2)
00025 {
00026     IntWiseDouble value;
00027     value.d = dist2;
00028     return (((value.i[1] & 0x7ff00000) >> 20) - 1023) >> 1;
00029 //  return (int)floor(0.5 * log(dist2) / log(2.0));
00030 }
00031 
00032 
00034 
00035 
00036 CoverTree::CoverTree(const Array<double>& points)
00037 {
00038     this->points = &points;
00039 
00040     unsigned int i, ic = points.dim(0);
00041     mem.resize(2*ic);           // upper bound
00042     usedmem = 0;
00043 
00044     // create a minimal tree of one element
00045     root = freeNode();
00046     root->index = 0;
00047     root->firstchild = NULL;
00048     root->nextsibling = NULL;
00049     root->scale = POINTSCALE;
00050 
00051     // insert the remaining points
00052     for (i=1; i<ic; i++) Insert(i);
00053 }
00054 
00055 CoverTree::~CoverTree()
00056 {
00057 }
00058 
00059 
00060 unsigned int CoverTree::OneNN(const Array<double>& query) const
00061 {
00062     tNode n;
00063     std::vector<tNode> list1;
00064     std::vector<tNode> list2;
00065     std::vector<tNode>* cand = &list1;
00066     std::vector<tNode>* refine = &list2;
00067     double best_d = distance((*points)[root->index], query);
00068     n.node = root;
00069     n.dist = best_d;
00070     cand->push_back(n);
00071     int s = root->scale;
00072 
00073     // iteratively refine the set
00074     while (true)
00075     {
00076         // compute shortest distance to all children
00077         int i, ic = cand->size();
00078 
00079         // refine to next scale
00080         bool children = false;
00081         for (i=0; i<ic; i++)
00082         {
00083             tNode& nr = (*cand)[i];
00084             if (nr.node->scale >= s - 1)
00085             {
00086                 // loop through sub-nodes
00087                 Node* child = nr.node->firstchild;
00088                 while (child != NULL)
00089                 {
00090                     double d = distance((*points)[child->index], query);
00091                     if (d <= best_d + i_pow2(child->scale))
00092                     {
00093                         n.node = child;
00094                         n.dist = d;
00095                         refine->push_back(n);
00096                         children = children || (child->firstchild != NULL);
00097 
00098                         if (d < best_d) best_d = d;
00099                     }
00100                     child = child->nextsibling;
00101                 }
00102             }
00103             else
00104             {
00105                 if (nr.dist <= best_d + i_pow2(nr.node->scale))
00106                 {
00107                     refine->push_back(nr);
00108                     children = children || (nr.node->firstchild != NULL);
00109                 }
00110             }
00111         }
00112 
00113         // stopping condition
00114         if (! children) break;
00115 
00116         cand->clear();
00117         std::swap(refine, cand);
00118         s--;
00119     }
00120 
00121     return (*refine)[0].node->index;
00122 }
00123 
00124 unsigned int CoverTree::SecondNN(unsigned int query) const
00125 {
00126     const ArrayReference<double> pt = (*points)[query];
00127     tNode n;
00128     std::vector<tNode> list1;
00129     std::vector<tNode> list2;
00130     std::vector<tNode>* cand = &list1;
00131     std::vector<tNode>* refine = &list2;
00132     double best_d = 1e100;
00133     n.node = root;
00134     n.dist = best_d;
00135     cand->push_back(n);
00136     int s = root->scale;
00137 
00138     // iteratively refine the set
00139     while (true)
00140     {
00141         // compute shortest distance to all children
00142         int i, ic = cand->size();
00143 
00144         // refine to next scale
00145         bool children = false;
00146         for (i=0; i<ic; i++)
00147         {
00148             tNode& nr = (*cand)[i];
00149             if (nr.node->scale >= s - 1)
00150             {
00151                 // loop through sub-nodes
00152                 Node* child = nr.node->firstchild;
00153                 while (child != NULL)
00154                 {
00155                     double d = distance((*points)[child->index], pt);
00156                     if (d <= best_d + i_pow2(child->scale))
00157                     {
00158                         n.node = child;
00159                         n.dist = d;
00160                         refine->push_back(n);
00161                         children = children || (child->firstchild != NULL);
00162 
00163                         if (d < best_d && child->index != query)
00164                         {
00165                             best_d = d;
00166                         }
00167                     }
00168                     child = child->nextsibling;
00169                 }
00170             }
00171             else
00172             {
00173                 if (nr.dist <= best_d + i_pow2(nr.node->scale))
00174                 {
00175                     refine->push_back(nr);
00176                     children = children || (nr.node->firstchild != NULL);
00177                 }
00178             }
00179         }
00180 
00181         // stopping condition
00182         if (! children) break;
00183 
00184         cand->clear();
00185         std::swap(refine, cand);
00186         s--;
00187     }
00188 
00189     if (refine->size() < 2)
00190     {
00191         throw SHARKEXCEPTION("[CoverTree::SecondNN] internal error");
00192     }
00193     else if (refine->size() == 2)
00194     {
00195         if ((*refine)[0].node->index == query) return (*refine)[1].node->index;
00196         else return (*refine)[0].node->index;
00197     }
00198     else
00199     {
00200         double best_d2 = 1e100;
00201         unsigned int ret = 0;
00202         int i, ic = refine->size();
00203         for (i=0; i<ic; i++)
00204         {
00205             unsigned int index = (*refine)[i].node->index;
00206             if (index == query) continue;
00207             double d2 = distance2((*points)[index], pt);
00208             if (d2 < best_d2)
00209             {
00210                 best_d2 = d2;
00211                 ret = index;
00212             }
00213         }
00214         return ret;
00215     }
00216 }
00217 
00218 bool cmp(const CoverTree::tNode& n1, const CoverTree::tNode& n2)
00219 {
00220     return (n1.dist < n2.dist);
00221 }
00222 
00223 void CoverTree::KNN(const Array<double>& query, unsigned int k, std::vector<unsigned int>& neighbors) const
00224 {
00225     tNode n;
00226     std::vector<tNode> list1;
00227     std::vector<tNode> list2;
00228     std::vector<tNode>* cand = &list1;
00229     std::vector<tNode>* refine = &list2;
00230     double kth_best_d = 1e100;
00231     n.node = root;
00232     n.dist = kth_best_d;
00233     cand->push_back(n);
00234     int s = root->scale;
00235 
00236     // iteratively refine the set
00237     while (true)
00238     {
00239         // compute shortest distance to all children
00240         int i, ic = cand->size();
00241 
00242         // refine to next scale
00243         bool children = false;
00244         for (i=0; i<ic; i++)
00245         {
00246             tNode& nr = (*cand)[i];
00247             if (nr.node->scale >= s - 1)
00248             {
00249                 // loop through sub-nodes
00250                 Node* child = nr.node->firstchild;
00251                 while (child != NULL)
00252                 {
00253                     double d = distance((*points)[child->index], query);
00254                     if (d <= kth_best_d + i_pow2(child->scale))
00255                     {
00256                         n.node = child;
00257                         n.dist = d;
00258                         refine->push_back(n);
00259                         children = children || (child->firstchild != NULL);
00260                     }
00261                     child = child->nextsibling;
00262                 }
00263             }
00264             else
00265             {
00266                 if (nr.dist <= kth_best_d + i_pow2(nr.node->scale))
00267                 {
00268                     refine->push_back(nr);
00269                     children = children || (nr.node->firstchild != NULL);
00270                 }
00271             }
00272         }
00273 
00274         // compute k-th best distance
00275         std::sort(refine->begin(), refine->end(), cmp);
00276         if (refine->size() >= k) kth_best_d = (*refine)[k - 1].dist;
00277         else kth_best_d = 1e100;
00278 
00279         // stopping condition
00280         if (! children) break;
00281 
00282         cand->clear();
00283         std::swap(refine, cand);
00284         s--;
00285     }
00286 
00287     neighbors.resize(k);
00288     unsigned int i;
00289     for (i=0; i<k; i++) neighbors[i] = (*refine)[i].node->index;
00290 }
00291 
00292 double CoverTree::distance2(const Array<double>& p1, const Array<double>& p2) const
00293 {
00294     unsigned int d, dim = p1.dim(0);
00295     double dist2 = 0.0;
00296     for (d=0; d<dim; d++) dist2 += Shark::sqr(p2(d) - p1(d));
00297     return dist2;
00298 }
00299 
00300 void CoverTree::Insert(unsigned int index)
00301 {
00302     const Array<double> point = (*points)[index];
00303     Node* parent = root;
00304     double pd2 = distance2((*points)[parent->index], point);
00305     int ps = i_log2(pd2);
00306 
00307     // check if the current scale is sufficient
00308     if (ps > parent->scale)
00309     {
00310         // create new top level
00311         parent = freeNode();
00312         parent->index = root->index;
00313         parent->scale = ps;
00314         parent->firstchild = root;
00315         parent->nextsibling = NULL;
00316 
00317         Node* node = freeNode();
00318         node->index = index;
00319         node->firstchild = NULL;
00320         node->nextsibling = NULL;
00321         node->scale = POINTSCALE;
00322 
00323         root->nextsibling = node;
00324         root = parent;
00325 
00326         return;
00327     }
00328 
00329     // find the parent
00330     while (true)
00331     {
00332         // find the closest child
00333         Node* closest = parent->firstchild;
00334         if (closest == NULL)
00335         {
00336             throw SHARKEXCEPTION("[CoverTree::Insert] internal error");
00337         }
00338 
00339         double cd2 = distance2((*points)[closest->index], point);
00340         Node* child = closest->nextsibling;
00341         while (child != NULL)
00342         {
00343             double d2 = distance2((*points)[child->index], point);
00344             if (d2 < cd2)
00345             {
00346                 closest = child;
00347                 cd2 = d2;
00348             }
00349             child = child->nextsibling;
00350         }
00351         int cs = i_log2(cd2);
00352 
00353         // check whether we are already at the right level
00354         if (cs == parent->scale)
00355         {
00356             // insert into existing sibling structure
00357             Node* node = freeNode();
00358             node->index = index;
00359             node->scale = POINTSCALE;
00360             node->firstchild = NULL;
00361             node->nextsibling = parent->firstchild;
00362             parent->firstchild = node;
00363 
00364             return;
00365         }
00366 
00367         if (cs > closest->scale)
00368         {
00369             if (closest->scale == POINTSCALE)
00370             {
00371                 // create new bottom level
00372                 Node* copy = freeNode();
00373                 Node* node = freeNode();
00374 
00375                 copy->index = closest->index;
00376                 copy->scale = POINTSCALE;
00377                 copy->firstchild = NULL;
00378                 copy->nextsibling = node;
00379 
00380                 node->index = index;
00381                 node->scale = POINTSCALE;
00382                 node->firstchild = NULL;
00383                 node->nextsibling = NULL;
00384 
00385                 closest->firstchild = copy;
00386                 closest->scale = cs;
00387 
00388                 return;
00389             }
00390             else
00391             {
00392                 // insert a level
00393                 Node* copy = freeNode();
00394                 Node* node = freeNode();
00395 
00396                 copy->index = closest->index;
00397                 copy->scale = closest->scale;
00398                 copy->firstchild = closest->firstchild;
00399                 copy->nextsibling = node;
00400 
00401                 closest->scale = cs;
00402                 closest->firstchild = copy;
00403 
00404                 node->index = index;
00405                 node->scale = POINTSCALE;
00406                 node->firstchild = NULL;
00407                 node->nextsibling = NULL;
00408 
00409                 return;
00410             }
00411         }
00412 
00413         // otherwise update parent
00414         parent = closest;
00415         pd2 = cd2;
00416         ps = cs;
00417     }
00418 }
00419 
00420 
00422 
00423 
00424 KernelCoverTree::KernelCoverTree(KernelFunction* kernel, const Array<double>& points)
00425 : CoverTree(points)
00426 {
00427     this->kernel = kernel;
00428 }
00429 
00430 KernelCoverTree::~KernelCoverTree()
00431 {
00432 }
00433 
00434 
00435 double KernelCoverTree::distance2(const Array<double>& p1, const Array<double>& p2) const
00436 {
00437     return (*kernel)(p1, p1) + (*kernel)(p2, p2) - 2.0 * (*kernel)(p1, p2);
00438 }