diff options
Diffstat (limited to 'geom_bottleneck/bottleneck/src')
17 files changed, 5329 insertions, 0 deletions
diff --git a/geom_bottleneck/bottleneck/src/ann/ANN.cpp b/geom_bottleneck/bottleneck/src/ann/ANN.cpp new file mode 100644 index 0000000..7bae577 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/ANN.cpp @@ -0,0 +1,230 @@ +//---------------------------------------------------------------------- +// File: ANN.cpp +// Programmer: Sunil Arya and David Mount +// Description: Methods for ANN.h and ANNx.h +// Last modified: 01/27/10 (Version 1.1.2) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2010 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +// Added performance counting to annDist() +// Revision 1.1.2 01/27/10 +// Fixed minor compilation bugs for new versions of gcc +//---------------------------------------------------------------------- + +#ifdef _WIN32 +#include <ciso646> // make VS more conformal +#endif + +#include <cstdlib> // C standard lib defs +#include <ANN/ANNx.h> // all ANN includes +#include <ANN/ANNperf.h> // ANN performance + + + +using namespace std; // make std:: accessible + + +namespace geom_bt { +//---------------------------------------------------------------------- +// Point methods +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// Distance utility. +// (Note: In the nearest neighbor search, most distances are +// computed using partial distance calculations, not this +// procedure.) +//---------------------------------------------------------------------- + +ANNdist annDist( // interpoint squared distance + int dim, + ANNpoint p, + ANNpoint q) +{ + register int d; + register ANNcoord diff; + register ANNcoord dist; + + dist = 0; + for (d = 0; d < dim; d++) { + diff = p[d] - q[d]; + dist = ANN_SUM(dist, ANN_POW(diff)); + } + ANN_FLOP(3*dim) // performance counts + ANN_PTS(1) + ANN_COORD(dim) + return dist; +} + +//---------------------------------------------------------------------- +// annPrintPoint() prints a point to a given output stream. +//---------------------------------------------------------------------- + +void annPrintPt( // print a point + ANNpoint pt, // the point + int dim, // the dimension + std::ostream &out) // output stream +{ + for (int j = 0; j < dim; j++) { + out << pt[j]; + if (j < dim-1) out << " "; + } +} + +//---------------------------------------------------------------------- +// Point allocation/deallocation: +// +// Because points (somewhat like strings in C) are stored +// as pointers. Consequently, creating and destroying +// copies of points may require storage allocation. These +// procedures do this. +// +// annAllocPt() and annDeallocPt() allocate a deallocate +// storage for a single point, and return a pointer to it. +// +// annAllocPts() allocates an array of points as well a place +// to store their coordinates, and initializes the points to +// point to their respective coordinates. It allocates point +// storage in a contiguous block large enough to store all the +// points. It performs no initialization. +// +// annDeallocPts() should only be used on point arrays allocated +// by annAllocPts since it assumes that points are allocated in +// a block. +// +// annCopyPt() copies a point taking care to allocate storage +// for the new point. +// +// annAssignRect() assigns the coordinates of one rectangle to +// another. The two rectangles must have the same dimension +// (and it is not possible to test this here). +//---------------------------------------------------------------------- + +ANNpoint annAllocPt(int dim, ANNcoord c) // allocate 1 point +{ + ANNpoint p = new ANNcoord[dim]; + for (int i = 0; i < dim; i++) p[i] = c; + return p; +} + +ANNpointArray annAllocPts(int n, int dim) // allocate n pts in dim +{ + ANNpointArray pa = new ANNpoint[n]; // allocate points + ANNpoint p = new ANNcoord[n*dim]; // allocate space for coords + for (int i = 0; i < n; i++) { + pa[i] = &(p[i*dim]); + } + return pa; +} + +void annDeallocPt(ANNpoint &p) // deallocate 1 point +{ + delete [] p; + p = NULL; +} + +void annDeallocPts(ANNpointArray &pa) // deallocate points +{ + delete [] pa[0]; // dealloc coordinate storage + delete [] pa; // dealloc points + pa = NULL; +} + +ANNpoint annCopyPt(int dim, ANNpoint source) // copy point +{ + ANNpoint p = new ANNcoord[dim]; + for (int i = 0; i < dim; i++) p[i] = source[i]; + return p; +} + + // assign one rect to another +void annAssignRect(int dim, ANNorthRect &dest, const ANNorthRect &source) +{ + for (int i = 0; i < dim; i++) { + dest.lo[i] = source.lo[i]; + dest.hi[i] = source.hi[i]; + } +} + + // is point inside rectangle? +ANNbool ANNorthRect::inside(const int dim, ANNpoint p) const +{ + for (int i = 0; i < dim; i++) { + if (p[i] < lo[i] || p[i] > hi[i]) return ANNfalse; + } + return ANNtrue; +} + +bool ANNorthRect::contains(const int dim, const ANNorthRect& r) const +{ + return this->inside(dim, r.hi) and this->inside(dim, r.lo); +} + +bool ANNorthRect::intersects(const int dim, const ANNorthRect& r) const +{ + assert(dim == 2); // works for plane only + const ANNpoint otherLo = r.lo; + const ANNpoint otherHi = r.hi; + if ( otherLo[0] > hi[0] or + otherLo[1] > hi[1] or + otherHi[0] < lo[0] or + otherHi[1] < lo[1]) { + return false; + } else { + return true; + } +} + +//---------------------------------------------------------------------- +// Error handler +//---------------------------------------------------------------------- + +void annError(const char* msg, ANNerr level) +{ + if (level == ANNabort) { + cerr << "ANN: ERROR------->" << msg << "<-------------ERROR\n"; + exit(1); + } + else { + cerr << "ANN: WARNING----->" << msg << "<-------------WARNING\n"; + } +} + +//---------------------------------------------------------------------- +// Limit on number of points visited +// We have an option for terminating the search early if the +// number of points visited exceeds some threshold. If the +// threshold is 0 (its default) this means there is no limit +// and the algorithm applies its normal termination condition. +// This is for applications where there are real time constraints +// on the running time of the algorithm. +//---------------------------------------------------------------------- + +int ANNmaxPtsVisited = 0; // maximum number of pts visited +int ANNptsVisited; // number of pts visited in search + +//---------------------------------------------------------------------- +// Global function declarations +//---------------------------------------------------------------------- + +void annMaxPtsVisit( // set limit on max. pts to visit in search + int maxPts) // the limit +{ + ANNmaxPtsVisited = maxPts; +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/bd_fix_rad_search.cpp b/geom_bottleneck/bottleneck/src/ann/bd_fix_rad_search.cpp new file mode 100644 index 0000000..fe8ab78 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/bd_fix_rad_search.cpp @@ -0,0 +1,64 @@ +//---------------------------------------------------------------------- +// File: bd_fix_rad_search.cpp +// Programmer: David Mount +// Description: Standard bd-tree search +// Last modified: 05/03/05 (Version 1.1) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 1.1 05/03/05 +// Initial release +//---------------------------------------------------------------------- + +#include "bd_tree.h" // bd-tree declarations +#include "kd_fix_rad_search.h" // kd-tree FR search declarations + +namespace geom_bt { + +//---------------------------------------------------------------------- +// Approximate searching for bd-trees. +// See the file kd_FR_search.cpp for general information on the +// approximate nearest neighbor search algorithm. Here we +// include the extensions for shrinking nodes. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// bd_shrink::ann_FR_search - search a shrinking node +//---------------------------------------------------------------------- + +void ANNbd_shrink::ann_FR_search(ANNdist box_dist) +{ + // check dist calc term cond. + if (ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited) return; + + ANNdist inner_dist = 0; // distance to inner box + for (int i = 0; i < n_bnds; i++) { // is query point in the box? + if (bnds[i].out(ANNkdFRQ)) { // outside this bounding side? + // add to inner distance + inner_dist = (ANNdist) ANN_SUM(inner_dist, bnds[i].dist(ANNkdFRQ)); + } + } + if (inner_dist <= box_dist) { // if inner box is closer + child[ANN_IN]->ann_FR_search(inner_dist);// search inner child first + child[ANN_OUT]->ann_FR_search(box_dist);// ...then outer child + } + else { // if outer box is closer + child[ANN_OUT]->ann_FR_search(box_dist);// search outer child first + child[ANN_IN]->ann_FR_search(inner_dist);// ...then outer child + } + ANN_FLOP(3*n_bnds) // increment floating ops + ANN_SHR(1) // one more shrinking node +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/bd_pr_search.cpp b/geom_bottleneck/bottleneck/src/ann/bd_pr_search.cpp new file mode 100644 index 0000000..fb9dea6 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/bd_pr_search.cpp @@ -0,0 +1,66 @@ +//---------------------------------------------------------------------- +// File: bd_pr_search.cpp +// Programmer: David Mount +// Description: Priority search for bd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +//History: +// Revision 0.1 03/04/98 +// Initial release +//---------------------------------------------------------------------- + +#include "bd_tree.h" // bd-tree declarations +#include "kd_pr_search.h" // kd priority search declarations + + +namespace geom_bt { + +//---------------------------------------------------------------------- +// Approximate priority searching for bd-trees. +// See the file kd_pr_search.cc for general information on the +// approximate nearest neighbor priority search algorithm. Here +// we include the extensions for shrinking nodes. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// bd_shrink::ann_search - search a shrinking node +//---------------------------------------------------------------------- + +void ANNbd_shrink::ann_pri_search(ANNdist box_dist) +{ + ANNdist inner_dist = 0; // distance to inner box + for (int i = 0; i < n_bnds; i++) { // is query point in the box? + if (bnds[i].out(ANNprQ)) { // outside this bounding side? + // add to inner distance + inner_dist = (ANNdist) ANN_SUM(inner_dist, bnds[i].dist(ANNprQ)); + } + } + if (inner_dist <= box_dist) { // if inner box is closer + if (child[ANN_OUT] != KD_TRIVIAL) // enqueue outer if not trivial + ANNprBoxPQ->insert(box_dist,child[ANN_OUT]); + // continue with inner child + child[ANN_IN]->ann_pri_search(inner_dist); + } + else { // if outer box is closer + if (child[ANN_IN] != KD_TRIVIAL) // enqueue inner if not trivial + ANNprBoxPQ->insert(inner_dist,child[ANN_IN]); + // continue with outer child + child[ANN_OUT]->ann_pri_search(box_dist); + } + ANN_FLOP(3*n_bnds) // increment floating ops + ANN_SHR(1) // one more shrinking node +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/bd_search.cpp b/geom_bottleneck/bottleneck/src/ann/bd_search.cpp new file mode 100644 index 0000000..2935bcb --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/bd_search.cpp @@ -0,0 +1,64 @@ +//---------------------------------------------------------------------- +// File: bd_search.cpp +// Programmer: David Mount +// Description: Standard bd-tree search +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +//---------------------------------------------------------------------- + +#include "bd_tree.h" // bd-tree declarations +#include "kd_search.h" // kd-tree search declarations + +namespace geom_bt { + +//---------------------------------------------------------------------- +// Approximate searching for bd-trees. +// See the file kd_search.cpp for general information on the +// approximate nearest neighbor search algorithm. Here we +// include the extensions for shrinking nodes. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// bd_shrink::ann_search - search a shrinking node +//---------------------------------------------------------------------- + +void ANNbd_shrink::ann_search(ANNdist box_dist) +{ + // check dist calc term cond. + if (ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited) return; + + ANNdist inner_dist = 0; // distance to inner box + for (int i = 0; i < n_bnds; i++) { // is query point in the box? + if (bnds[i].out(ANNkdQ)) { // outside this bounding side? + // add to inner distance + inner_dist = (ANNdist) ANN_SUM(inner_dist, bnds[i].dist(ANNkdQ)); + } + } + if (inner_dist <= box_dist) { // if inner box is closer + child[ANN_IN]->ann_search(inner_dist); // search inner child first + child[ANN_OUT]->ann_search(box_dist); // ...then outer child + } + else { // if outer box is closer + child[ANN_OUT]->ann_search(box_dist); // search outer child first + child[ANN_IN]->ann_search(inner_dist); // ...then outer child + } + ANN_FLOP(3*n_bnds) // increment floating ops + ANN_SHR(1) // one more shrinking node +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/bd_tree.cpp b/geom_bottleneck/bottleneck/src/ann/bd_tree.cpp new file mode 100644 index 0000000..8c1ef6d --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/bd_tree.cpp @@ -0,0 +1,419 @@ +//---------------------------------------------------------------------- +// File: bd_tree.cpp +// Programmer: David Mount +// Description: Basic methods for bd-trees. +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +// Revision l.0 04/01/05 +// Fixed centroid shrink threshold condition to depend on the +// dimension. +// Moved dump routine to kd_dump.cpp. +//---------------------------------------------------------------------- + +#include "bd_tree.h" // bd-tree declarations +#include "kd_util.h" // kd-tree utilities +#include "kd_split.h" // kd-tree splitting rules + +#include <ANN/ANNperf.h> // performance evaluation + +namespace geom_bt { +//---------------------------------------------------------------------- +// Printing a bd-tree +// These routines print a bd-tree. See the analogous procedure +// in kd_tree.cpp for more information. +//---------------------------------------------------------------------- + +void ANNbd_shrink::print( // print shrinking node + int level, // depth of node in tree + ostream &out) // output stream +{ + child[ANN_OUT]->print(level+1, out); // print out-child + + out << " "; + for (int i = 0; i < level; i++) // print indentation + out << ".."; + out << "Shrink"; + for (int j = 0; j < n_bnds; j++) { // print sides, 2 per line + if (j % 2 == 0) { + out << "\n"; // newline and indentation + for (int i = 0; i < level+2; i++) out << " "; + } + out << " ([" << bnds[j].cd << "]" + << (bnds[j].sd > 0 ? ">=" : "< ") + << bnds[j].cv << ")"; + } + out << "\n"; + + child[ANN_IN]->print(level+1, out); // print in-child +} + +//---------------------------------------------------------------------- +// kd_tree statistics utility (for performance evaluation) +// This routine computes various statistics information for +// shrinking nodes. See file kd_tree.cpp for more information. +//---------------------------------------------------------------------- + +void ANNbd_shrink::getStats( // get subtree statistics + int dim, // dimension of space + ANNkdStats &st, // stats (modified) + ANNorthRect &bnd_box) // bounding box +{ + ANNkdStats ch_stats; // stats for children + ANNorthRect inner_box(dim); // inner box of shrink + + annBnds2Box(bnd_box, // enclosing box + dim, // dimension + n_bnds, // number of bounds + bnds, // bounds array + inner_box); // inner box (modified) + // get stats for inner child + ch_stats.reset(); // reset + child[ANN_IN]->getStats(dim, ch_stats, inner_box); + st.merge(ch_stats); // merge them + // get stats for outer child + ch_stats.reset(); // reset + child[ANN_OUT]->getStats(dim, ch_stats, bnd_box); + st.merge(ch_stats); // merge them + + st.depth++; // increment depth + st.n_shr++; // increment number of shrinks +} + +//---------------------------------------------------------------------- +// bd-tree constructor +// This is the main constructor for bd-trees given a set of points. +// It first builds a skeleton kd-tree as a basis, then computes the +// bounding box of the data points, and then invokes rbd_tree() to +// actually build the tree, passing it the appropriate splitting +// and shrinking information. +//---------------------------------------------------------------------- + +ANNkd_ptr rbd_tree( // recursive construction of bd-tree + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + int bsp, // bucket space + ANNorthRect &bnd_box, // bounding box for current node + ANNkd_splitter splitter, // splitting routine + ANNshrinkRule shrink); // shrinking rule + +ANNbd_tree::ANNbd_tree( // construct from point array + ANNpointArray pa, // point array (with at least n pts) + int n, // number of points + int dd, // dimension + int bs, // bucket size + ANNsplitRule split, // splitting rule + ANNshrinkRule shrink) // shrinking rule + : ANNkd_tree(n, dd, bs) // build skeleton base tree +{ + pts = pa; // where the points are + if (n == 0) return; // no points--no sweat + + ANNorthRect bnd_box(dd); // bounding box for points + // construct bounding rectangle + annEnclRect(pa, pidx, n, dd, bnd_box); + // copy to tree structure + bnd_box_lo = annCopyPt(dd, bnd_box.lo); + bnd_box_hi = annCopyPt(dd, bnd_box.hi); + + switch (split) { // build by rule + case ANN_KD_STD: // standard kd-splitting rule + root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, kd_split, shrink); + break; + case ANN_KD_MIDPT: // midpoint split + root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, midpt_split, shrink); + break; + case ANN_KD_SUGGEST: // best (in our opinion) + case ANN_KD_SL_MIDPT: // sliding midpoint split + root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, sl_midpt_split, shrink); + break; + case ANN_KD_FAIR: // fair split + root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, fair_split, shrink); + break; + case ANN_KD_SL_FAIR: // sliding fair split + root = rbd_tree(pa, pidx, n, dd, bs, + bnd_box, sl_fair_split, shrink); + break; + default: + annError("Illegal splitting method", ANNabort); + } +} + +//---------------------------------------------------------------------- +// Shrinking rules +//---------------------------------------------------------------------- + +enum ANNdecomp {SPLIT, SHRINK}; // decomposition methods + +//---------------------------------------------------------------------- +// trySimpleShrink - Attempt a simple shrink +// +// We compute the tight bounding box of the points, and compute +// the 2*dim ``gaps'' between the sides of the tight box and the +// bounding box. If any of the gaps is large enough relative to +// the longest side of the tight bounding box, then we shrink +// all sides whose gaps are large enough. (The reason for +// comparing against the tight bounding box, is that after +// shrinking the longest box size will decrease, and if we use +// the standard bounding box, we may decide to shrink twice in +// a row. Since the tight box is fixed, we cannot shrink twice +// consecutively.) +//---------------------------------------------------------------------- +const float BD_GAP_THRESH = 0.5; // gap threshold (must be < 1) +const int BD_CT_THRESH = 2; // min number of shrink sides + +ANNdecomp trySimpleShrink( // try a simple shrink + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + const ANNorthRect &bnd_box, // current bounding box + ANNorthRect &inner_box) // inner box if shrinking (returned) +{ + int i; + // compute tight bounding box + annEnclRect(pa, pidx, n, dim, inner_box); + + ANNcoord max_length = 0; // find longest box side + for (i = 0; i < dim; i++) { + ANNcoord length = inner_box.hi[i] - inner_box.lo[i]; + if (length > max_length) { + max_length = length; + } + } + + int shrink_ct = 0; // number of sides we shrunk + for (i = 0; i < dim; i++) { // select which sides to shrink + // gap between boxes + ANNcoord gap_hi = bnd_box.hi[i] - inner_box.hi[i]; + // big enough gap to shrink? + if (gap_hi < max_length*BD_GAP_THRESH) + inner_box.hi[i] = bnd_box.hi[i]; // no - expand + else shrink_ct++; // yes - shrink this side + + // repeat for high side + ANNcoord gap_lo = inner_box.lo[i] - bnd_box.lo[i]; + if (gap_lo < max_length*BD_GAP_THRESH) + inner_box.lo[i] = bnd_box.lo[i]; // no - expand + else shrink_ct++; // yes - shrink this side + } + + if (shrink_ct >= BD_CT_THRESH) // did we shrink enough sides? + return SHRINK; + else return SPLIT; +} + +//---------------------------------------------------------------------- +// tryCentroidShrink - Attempt a centroid shrink +// +// We repeatedly apply the splitting rule, always to the larger subset +// of points, until the number of points decreases by the constant +// fraction BD_FRACTION. If this takes more than dim*BD_MAX_SPLIT_FAC +// splits for this to happen, then we shrink to the final inner box +// Otherwise we split. +//---------------------------------------------------------------------- + +const float BD_MAX_SPLIT_FAC = 0.5; // maximum number of splits allowed +const float BD_FRACTION = 0.5; // ...to reduce points by this fraction + // ...This must be < 1. + +ANNdecomp tryCentroidShrink( // try a centroid shrink + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + const ANNorthRect &bnd_box, // current bounding box + ANNkd_splitter splitter, // splitting procedure + ANNorthRect &inner_box) // inner box if shrinking (returned) +{ + int n_sub = n; // number of points in subset + int n_goal = (int) (n*BD_FRACTION); // number of point in goal + int n_splits = 0; // number of splits needed + // initialize inner box to bounding box + annAssignRect(dim, inner_box, bnd_box); + + while (n_sub > n_goal) { // keep splitting until goal reached + int cd; // cut dim from splitter (ignored) + ANNcoord cv; // cut value from splitter (ignored) + int n_lo; // number of points on low side + // invoke splitting procedure + (*splitter)(pa, pidx, inner_box, n_sub, dim, cd, cv, n_lo); + n_splits++; // increment split count + + if (n_lo >= n_sub/2) { // most points on low side + inner_box.hi[cd] = cv; // collapse high side + n_sub = n_lo; // recurse on lower points + } + else { // most points on high side + inner_box.lo[cd] = cv; // collapse low side + pidx += n_lo; // recurse on higher points + n_sub -= n_lo; + } + } + if (n_splits > dim*BD_MAX_SPLIT_FAC)// took too many splits + return SHRINK; // shrink to final subset + else + return SPLIT; +} + +//---------------------------------------------------------------------- +// selectDecomp - select which decomposition to use +//---------------------------------------------------------------------- + +ANNdecomp selectDecomp( // select decomposition method + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + const ANNorthRect &bnd_box, // current bounding box + ANNkd_splitter splitter, // splitting procedure + ANNshrinkRule shrink, // shrinking rule + ANNorthRect &inner_box) // inner box if shrinking (returned) +{ + ANNdecomp decomp = SPLIT; // decomposition + + switch (shrink) { // check shrinking rule + case ANN_BD_NONE: // no shrinking allowed + decomp = SPLIT; + break; + case ANN_BD_SUGGEST: // author's suggestion + case ANN_BD_SIMPLE: // simple shrink + decomp = trySimpleShrink( + pa, pidx, // points and indices + n, dim, // number of points and dimension + bnd_box, // current bounding box + inner_box); // inner box if shrinking (returned) + break; + case ANN_BD_CENTROID: // centroid shrink + decomp = tryCentroidShrink( + pa, pidx, // points and indices + n, dim, // number of points and dimension + bnd_box, // current bounding box + splitter, // splitting procedure + inner_box); // inner box if shrinking (returned) + break; + default: + annError("Illegal shrinking rule", ANNabort); + } + return decomp; +} + +//---------------------------------------------------------------------- +// rbd_tree - recursive procedure to build a bd-tree +// +// This is analogous to rkd_tree, but for bd-trees. See the +// procedure rkd_tree() in kd_split.cpp for more information. +// +// If the number of points falls below the bucket size, then a +// leaf node is created for the points. Otherwise we invoke the +// procedure selectDecomp() which determines whether we are to +// split or shrink. If splitting is chosen, then we essentially +// do exactly as rkd_tree() would, and invoke the specified +// splitting procedure to the points. Otherwise, the selection +// procedure returns a bounding box, from which we extract the +// appropriate shrinking bounds, and create a shrinking node. +// Finally the points are subdivided, and the procedure is +// invoked recursively on the two subsets to form the children. +//---------------------------------------------------------------------- + +ANNkd_ptr rbd_tree( // recursive construction of bd-tree + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + int bsp, // bucket space + ANNorthRect &bnd_box, // bounding box for current node + ANNkd_splitter splitter, // splitting routine + ANNshrinkRule shrink) // shrinking rule +{ + ANNdecomp decomp; // decomposition method + + ANNorthRect inner_box(dim); // inner box (if shrinking) + + if (n <= bsp) { // n small, make a leaf node + if (n == 0) // empty leaf node + return KD_TRIVIAL; // return (canonical) empty leaf + else // construct the node and return + return new ANNkd_leaf(n, pidx); + } + + decomp = selectDecomp( // select decomposition method + pa, pidx, // points and indices + n, dim, // number of points and dimension + bnd_box, // current bounding box + splitter, shrink, // splitting/shrinking methods + inner_box); // inner box if shrinking (returned) + + if (decomp == SPLIT) { // split selected + int cd; // cutting dimension + ANNcoord cv; // cutting value + int n_lo; // number on low side of cut + // invoke splitting procedure + (*splitter)(pa, pidx, bnd_box, n, dim, cd, cv, n_lo); + + ANNcoord lv = bnd_box.lo[cd]; // save bounds for cutting dimension + ANNcoord hv = bnd_box.hi[cd]; + + bnd_box.hi[cd] = cv; // modify bounds for left subtree + ANNkd_ptr lo = rbd_tree( // build left subtree + pa, pidx, n_lo, // ...from pidx[0..n_lo-1] + dim, bsp, bnd_box, splitter, shrink); + bnd_box.hi[cd] = hv; // restore bounds + + bnd_box.lo[cd] = cv; // modify bounds for right subtree + ANNkd_ptr hi = rbd_tree( // build right subtree + pa, pidx + n_lo, n-n_lo,// ...from pidx[n_lo..n-1] + dim, bsp, bnd_box, splitter, shrink); + bnd_box.lo[cd] = lv; // restore bounds + // create the splitting node + return new ANNkd_split(cd, cv, lv, hv, lo, hi); + } + else { // shrink selected + int n_in; // number of points in box + int n_bnds; // number of bounding sides + + annBoxSplit( // split points around inner box + pa, // points to split + pidx, // point indices + n, // number of points + dim, // dimension + inner_box, // inner box + n_in); // number of points inside (returned) + + ANNkd_ptr in = rbd_tree( // build inner subtree pidx[0..n_in-1] + pa, pidx, n_in, dim, bsp, inner_box, splitter, shrink); + ANNkd_ptr out = rbd_tree( // build outer subtree pidx[n_in..n] + pa, pidx+n_in, n - n_in, dim, bsp, bnd_box, splitter, shrink); + + ANNorthHSArray bnds = NULL; // bounds (alloc in Box2Bnds and + // ...freed in bd_shrink destroyer) + + annBox2Bnds( // convert inner box to bounds + inner_box, // inner box + bnd_box, // enclosing box + dim, // dimension + n_bnds, // number of bounds (returned) + bnds); // bounds array (modified) + + // return shrinking node + return new ANNbd_shrink(n_bnds, bnds, in, out); + } +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_dump.cpp b/geom_bottleneck/bottleneck/src/ann/kd_dump.cpp new file mode 100644 index 0000000..64db9a7 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_dump.cpp @@ -0,0 +1,447 @@ +//---------------------------------------------------------------------- +// File: kd_dump.cc +// Programmer: David Mount +// Description: Dump and Load for kd- and bd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +// Moved dump out of kd_tree.cc into this file. +// Added kd-tree load constructor. +//---------------------------------------------------------------------- +// This file contains routines for dumping kd-trees and bd-trees and +// reloading them. (It is an abuse of policy to include both kd- and +// bd-tree routines in the same file, sorry. There should be no problem +// in deleting the bd- versions of the routines if they are not +// desired.) +//---------------------------------------------------------------------- + +#include "kd_tree.h" // kd-tree declarations +#include "bd_tree.h" // bd-tree declarations + +using namespace std; // make std:: available + +namespace geom_bt { + + //---------------------------------------------------------------------- + // Constants + //---------------------------------------------------------------------- + + const int STRING_LEN = 500; // maximum string length + const double EPSILON = 1E-5; // small number for float comparison + + enum ANNtreeType { KD_TREE, BD_TREE }; // tree types (used in loading) + + //---------------------------------------------------------------------- + // Procedure declarations + //---------------------------------------------------------------------- + + static ANNkd_ptr annReadDump( // read dump file + istream &in, // input stream + ANNtreeType tree_type, // type of tree expected + ANNpointArray &the_pts, // new points (if applic) + ANNidxArray &the_pidx, // point indices (returned) + int &the_dim, // dimension (returned) + int &the_n_pts, // number of points (returned) + int &the_bkt_size, // bucket size (returned) + ANNpoint &the_bnd_box_lo, // low bounding point + ANNpoint &the_bnd_box_hi); // high bounding point + + static ANNkd_ptr annReadTree( // read tree-part of dump file + istream &in, // input stream + ANNtreeType tree_type, // type of tree expected + ANNidxArray the_pidx, // point indices (modified) + int &next_idx); // next index (modified) + + //---------------------------------------------------------------------- + // ANN kd- and bd-tree Dump Format + // The dump file begins with a header containing the version of + // ANN, an optional section containing the points, followed by + // a description of the tree. The tree is printed in preorder. + // + // Format: + // #ANN <version number> <comments> [END_OF_LINE] + // points <dim> <n_pts> (point coordinates: this is optional) + // 0 <xxx> <xxx> ... <xxx> (point indices and coordinates) + // 1 <xxx> <xxx> ... <xxx> + // ... + // tree <dim> <n_pts> <bkt_size> + // <xxx> <xxx> ... <xxx> (lower end of bounding box) + // <xxx> <xxx> ... <xxx> (upper end of bounding box) + // If the tree is null, then a single line "null" is + // output. Otherwise the nodes of the tree are printed + // one per line in preorder. Leaves and splitting nodes + // have the following formats: + // Leaf node: + // leaf <n_pts> <bkt[0]> <bkt[1]> ... <bkt[n-1]> + // Splitting nodes: + // split <cut_dim> <cut_val> <lo_bound> <hi_bound> + // + // For bd-trees: + // + // Shrinking nodes: + // shrink <n_bnds> + // <cut_dim> <cut_val> <side> + // <cut_dim> <cut_val> <side> + // ... (repeated n_bnds times) + //---------------------------------------------------------------------- + + void ANNkd_tree::Dump( // dump entire tree + ANNbool with_pts, // print points as well? + ostream &out) // output stream + { + out << "#ANN " << ANNversion << "\n"; + out.precision(ANNcoordPrec); // use full precision in dumping + if (with_pts) { // print point coordinates + out << "points " << dim << " " << n_pts << "\n"; + for (int i = 0; i < n_pts; i++) { + out << i << " "; + annPrintPt(pts[i], dim, out); + out << "\n"; + } + } + out << "tree " // print tree elements + << dim << " " + << n_pts << " " + << bkt_size << "\n"; + + annPrintPt(bnd_box_lo, dim, out); // print lower bound + out << "\n"; + annPrintPt(bnd_box_hi, dim, out); // print upper bound + out << "\n"; + + if (root == NULL) // empty tree? + out << "null\n"; + else { + root->dump(out); // invoke printing at root + } + out.precision(0); // restore default precision + } + + void ANNkd_split::dump( // dump a splitting node + ostream &out) // output stream + { + out << "split " << cut_dim << " " << cut_val << " "; + out << cd_bnds[ANN_LO] << " " << cd_bnds[ANN_HI] << "\n"; + + child[ANN_LO]->dump(out); // print low child + child[ANN_HI]->dump(out); // print high child + } + + void ANNkd_leaf::dump( // dump a leaf node + ostream &out) // output stream + { + if (this == KD_TRIVIAL) { // canonical trivial leaf node + out << "leaf 0\n"; // leaf no points + } + else { + out << "leaf " << n_pts; + for (int j = 0; j < n_pts; j++) { + out << " " << bkt[j]; + } + out << "\n"; + } + } + + void ANNbd_shrink::dump( // dump a shrinking node + ostream &out) // output stream + { + out << "shrink " << n_bnds << "\n"; + for (int j = 0; j < n_bnds; j++) { + out << bnds[j].cd << " " << bnds[j].cv << " " << bnds[j].sd << "\n"; + } + child[ANN_IN]->dump(out); // print in-child + child[ANN_OUT]->dump(out); // print out-child + } + + //---------------------------------------------------------------------- + // Load kd-tree from dump file + // This rebuilds a kd-tree which was dumped to a file. The dump + // file contains all the basic tree information according to a + // preorder traversal. We assume that the dump file also contains + // point data. (This is to guarantee the consistency of the tree.) + // If not, then an error is generated. + // + // Indirectly, this procedure allocates space for points, point + // indices, all nodes in the tree, and the bounding box for the + // tree. When the tree is destroyed, all but the points are + // deallocated. + // + // This routine calls annReadDump to do all the work. + //---------------------------------------------------------------------- + + ANNkd_tree::ANNkd_tree( // build from dump file + istream &in) // input stream for dump file + { + int the_dim; // local dimension + int the_n_pts; // local number of points + int the_bkt_size; // local number of points + ANNpoint the_bnd_box_lo; // low bounding point + ANNpoint the_bnd_box_hi; // high bounding point + ANNpointArray the_pts; // point storage + ANNidxArray the_pidx; // point index storage + ANNkd_ptr the_root; // root of the tree + + the_root = annReadDump( // read the dump file + in, // input stream + KD_TREE, // expecting a kd-tree + the_pts, // point array (returned) + the_pidx, // point indices (returned) + the_dim, the_n_pts, the_bkt_size, // basic tree info (returned) + the_bnd_box_lo, the_bnd_box_hi); // bounding box info (returned) + + // create a skeletal tree + SkeletonTree(the_n_pts, the_dim, the_bkt_size, the_pts, the_pidx); + + bnd_box_lo = the_bnd_box_lo; + bnd_box_hi = the_bnd_box_hi; + + root = the_root; // set the root + } + + ANNbd_tree::ANNbd_tree( // build bd-tree from dump file + istream &in) : ANNkd_tree() // input stream for dump file + { + int the_dim; // local dimension + int the_n_pts; // local number of points + int the_bkt_size; // local number of points + ANNpoint the_bnd_box_lo; // low bounding point + ANNpoint the_bnd_box_hi; // high bounding point + ANNpointArray the_pts; // point storage + ANNidxArray the_pidx; // point index storage + ANNkd_ptr the_root; // root of the tree + + the_root = annReadDump( // read the dump file + in, // input stream + BD_TREE, // expecting a bd-tree + the_pts, // point array (returned) + the_pidx, // point indices (returned) + the_dim, the_n_pts, the_bkt_size, // basic tree info (returned) + the_bnd_box_lo, the_bnd_box_hi); // bounding box info (returned) + + // create a skeletal tree + SkeletonTree(the_n_pts, the_dim, the_bkt_size, the_pts, the_pidx); + bnd_box_lo = the_bnd_box_lo; + bnd_box_hi = the_bnd_box_hi; + + root = the_root; // set the root + } + + //---------------------------------------------------------------------- + // annReadDump - read a dump file + // + // This procedure reads a dump file, constructs a kd-tree + // and returns all the essential information needed to actually + // construct the tree. Because this procedure is used for + // constructing both kd-trees and bd-trees, the second argument + // is used to indicate which type of tree we are expecting. + //---------------------------------------------------------------------- + + static ANNkd_ptr annReadDump( + istream &in, // input stream + ANNtreeType tree_type, // type of tree expected + ANNpointArray &the_pts, // new points (returned) + ANNidxArray &the_pidx, // point indices (returned) + int &the_dim, // dimension (returned) + int &the_n_pts, // number of points (returned) + int &the_bkt_size, // bucket size (returned) + ANNpoint &the_bnd_box_lo, // low bounding point (ret'd) + ANNpoint &the_bnd_box_hi) // high bounding point (ret'd) + { + int j; + char str[STRING_LEN]; // storage for string + char version[STRING_LEN]; // ANN version number + ANNkd_ptr the_root = NULL; + + //------------------------------------------------------------------ + // Input file header + //------------------------------------------------------------------ + in >> str; // input header + if (strcmp(str, "#ANN") != 0) { // incorrect header + annError("Incorrect header for dump file", ANNabort); + } + in.getline(version, STRING_LEN); // get version (ignore) + + //------------------------------------------------------------------ + // Input the points + // An array the_pts is allocated and points are read from + // the dump file. + //------------------------------------------------------------------ + in >> str; // get major heading + if (strcmp(str, "points") == 0) { // points section + in >> the_dim; // input dimension + in >> the_n_pts; // number of points + // allocate point storage + the_pts = annAllocPts(the_n_pts, the_dim); + for (int i = 0; i < the_n_pts; i++) { // input point coordinates + ANNidx idx; // point index + in >> idx; // input point index + if (idx < 0 || idx >= the_n_pts) { + annError("Point index is out of range", ANNabort); + } + for (j = 0; j < the_dim; j++) { + in >> the_pts[idx][j]; // read point coordinates + } + } + in >> str; // get next major heading + } + else { // no points were input + annError("Points must be supplied in the dump file", ANNabort); + } + + //------------------------------------------------------------------ + // Input the tree + // After the basic header information, we invoke annReadTree + // to do all the heavy work. We create our own array of + // point indices (so we can pass them to annReadTree()) + // but we do not deallocate them. They will be deallocated + // when the tree is destroyed. + //------------------------------------------------------------------ + if (strcmp(str, "tree") == 0) { // tree section + in >> the_dim; // read dimension + in >> the_n_pts; // number of points + in >> the_bkt_size; // bucket size + the_bnd_box_lo = annAllocPt(the_dim); // allocate bounding box pts + the_bnd_box_hi = annAllocPt(the_dim); + + for (j = 0; j < the_dim; j++) { // read bounding box low + in >> the_bnd_box_lo[j]; + } + for (j = 0; j < the_dim; j++) { // read bounding box low + in >> the_bnd_box_hi[j]; + } + the_pidx = new ANNidx[the_n_pts]; // allocate point index array + int next_idx = 0; // number of indices filled + // read the tree and indices + the_root = annReadTree(in, tree_type, the_pidx, next_idx); + if (next_idx != the_n_pts) { // didn't see all the points? + annError("Didn't see as many points as expected", ANNwarn); + } + } + else { + annError("Illegal dump format. Expecting section heading", ANNabort); + } + return the_root; + } + + //---------------------------------------------------------------------- + // annReadTree - input tree and return pointer + // + // annReadTree reads in a node of the tree, makes any recursive + // calls as needed to input the children of this node (if internal). + // It returns a pointer to the node that was created. An array + // of point indices is given along with a pointer to the next + // available location in the array. As leaves are read, their + // point indices are stored here, and the point buckets point + // to the first entry in the array. + // + // Recall that these are the formats. The tree is given in + // preorder. + // + // Leaf node: + // leaf <n_pts> <bkt[0]> <bkt[1]> ... <bkt[n-1]> + // Splitting nodes: + // split <cut_dim> <cut_val> <lo_bound> <hi_bound> + // + // For bd-trees: + // + // Shrinking nodes: + // shrink <n_bnds> + // <cut_dim> <cut_val> <side> + // <cut_dim> <cut_val> <side> + // ... (repeated n_bnds times) + //---------------------------------------------------------------------- + + static ANNkd_ptr annReadTree( + istream &in, // input stream + ANNtreeType tree_type, // type of tree expected + ANNidxArray the_pidx, // point indices (modified) + int &next_idx) // next index (modified) + { + char tag[STRING_LEN]; // tag (leaf, split, shrink) + int n_pts; // number of points in leaf + int cd; // cut dimension + ANNcoord cv; // cut value + ANNcoord lb; // low bound + ANNcoord hb; // high bound + int n_bnds; // number of bounding sides + int sd; // which side + + in >> tag; // input node tag + + if (strcmp(tag, "null") == 0) { // null tree + return NULL; + } + //------------------------------------------------------------------ + // Read a leaf + //------------------------------------------------------------------ + if (strcmp(tag, "leaf") == 0) { // leaf node + + in >> n_pts; // input number of points + int old_idx = next_idx; // save next_idx + if (n_pts == 0) { // trivial leaf + return KD_TRIVIAL; + } + else { + for (int i = 0; i < n_pts; i++) { // input point indices + in >> the_pidx[next_idx++]; // store in array of indices + } + } + return new ANNkd_leaf(n_pts, &the_pidx[old_idx]); + } + //------------------------------------------------------------------ + // Read a splitting node + //------------------------------------------------------------------ + else if (strcmp(tag, "split") == 0) { // splitting node + + in >> cd >> cv >> lb >> hb; + + // read low and high subtrees + ANNkd_ptr lc = annReadTree(in, tree_type, the_pidx, next_idx); + ANNkd_ptr hc = annReadTree(in, tree_type, the_pidx, next_idx); + // create new node and return + return new ANNkd_split(cd, cv, lb, hb, lc, hc); + } + //------------------------------------------------------------------ + // Read a shrinking node (bd-tree only) + //------------------------------------------------------------------ + else if (strcmp(tag, "shrink") == 0) { // shrinking node + if (tree_type != BD_TREE) { + annError("Shrinking node not allowed in kd-tree", ANNabort); + } + + in >> n_bnds; // number of bounding sides + // allocate bounds array + ANNorthHSArray bds = new ANNorthHalfSpace[n_bnds]; + for (int i = 0; i < n_bnds; i++) { + in >> cd >> cv >> sd; // input bounding halfspace + // copy to array + bds[i] = ANNorthHalfSpace(cd, cv, sd); + } + // read inner and outer subtrees + ANNkd_ptr ic = annReadTree(in, tree_type, the_pidx, next_idx); + ANNkd_ptr oc = annReadTree(in, tree_type, the_pidx, next_idx); + // create new node and return + return new ANNbd_shrink(n_bnds, bds, ic, oc); + } + else { + annError("Illegal node type in dump file", ANNabort); + exit(0); // to keep the compiler happy + } + } +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_fix_rad_search.cpp b/geom_bottleneck/bottleneck/src/ann/kd_fix_rad_search.cpp new file mode 100644 index 0000000..1a4749e --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_fix_rad_search.cpp @@ -0,0 +1,185 @@ +//---------------------------------------------------------------------- +// File: kd_fix_rad_search.cpp +// Programmer: Sunil Arya and David Mount +// Description: Standard kd-tree fixed-radius kNN search +// Last modified: 05/03/05 (Version 1.1) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 1.1 05/03/05 +// Initial release +//---------------------------------------------------------------------- + +#include "kd_fix_rad_search.h" // kd fixed-radius search decls + +namespace geom_bt { +//---------------------------------------------------------------------- +// Approximate fixed-radius k nearest neighbor search +// The squared radius is provided, and this procedure finds the +// k nearest neighbors within the radius, and returns the total +// number of points lying within the radius. +// +// The method used for searching the kd-tree is a variation of the +// nearest neighbor search used in kd_search.cpp, except that the +// radius of the search ball is known. We refer the reader to that +// file for the explanation of the recursive search procedure. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// To keep argument lists short, a number of global variables +// are maintained which are common to all the recursive calls. +// These are given below. +//---------------------------------------------------------------------- + +int ANNkdFRDim; // dimension of space +ANNpoint ANNkdFRQ; // query point +ANNdist ANNkdFRSqRad; // squared radius search bound +double ANNkdFRMaxErr; // max tolerable squared error +ANNpointArray ANNkdFRPts; // the points +ANNmin_k* ANNkdFRPointMK; // set of k closest points +int ANNkdFRPtsVisited; // total points visited +int ANNkdFRPtsInRange; // number of points in the range + +//---------------------------------------------------------------------- +// annkFRSearch - fixed radius search for k nearest neighbors +//---------------------------------------------------------------------- + +int ANNkd_tree::annkFRSearch( + ANNpoint q, // the query point + ANNdist sqRad, // squared radius search bound + int k, // number of near neighbors to return + ANNidxArray nn_idx, // nearest neighbor indices (returned) + ANNdistArray dd, // the approximate nearest neighbor + double eps) // the error bound +{ + ANNkdFRDim = dim; // copy arguments to static equivs + ANNkdFRQ = q; + ANNkdFRSqRad = sqRad; + ANNkdFRPts = pts; + ANNkdFRPtsVisited = 0; // initialize count of points visited + ANNkdFRPtsInRange = 0; // ...and points in the range + + ANNkdFRMaxErr = ANN_POW(1.0 + eps); + ANN_FLOP(2) // increment floating op count + + ANNkdFRPointMK = new ANNmin_k(k); // create set for closest k points + // search starting at the root + root->ann_FR_search(annBoxDistance(q, bnd_box_lo, bnd_box_hi, dim)); + + for (int i = 0; i < k; i++) { // extract the k-th closest points + if (dd != NULL) + dd[i] = ANNkdFRPointMK->ith_smallest_key(i); + if (nn_idx != NULL) + nn_idx[i] = ANNkdFRPointMK->ith_smallest_info(i); + } + + delete ANNkdFRPointMK; // deallocate closest point set + return ANNkdFRPtsInRange; // return final point count +} + +//---------------------------------------------------------------------- +// kd_split::ann_FR_search - search a splitting node +// Note: This routine is similar in structure to the standard kNN +// search. It visits the subtree that is closer to the query point +// first. For fixed-radius search, there is no benefit in visiting +// one subtree before the other, but we maintain the same basic +// code structure for the sake of uniformity. +//---------------------------------------------------------------------- + +void ANNkd_split::ann_FR_search(ANNdist box_dist) +{ + // check dist calc term condition + if (ANNmaxPtsVisited != 0 && ANNkdFRPtsVisited > ANNmaxPtsVisited) return; + + // distance to cutting plane + ANNcoord cut_diff = ANNkdFRQ[cut_dim] - cut_val; + + if (cut_diff < 0) { // left of cutting plane + child[ANN_LO]->ann_FR_search(box_dist);// visit closer child first + + ANNcoord box_diff = cd_bnds[ANN_LO] - ANNkdFRQ[cut_dim]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + box_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + // visit further child if in range + if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad) + child[ANN_HI]->ann_FR_search(box_dist); + + } + else { // right of cutting plane + child[ANN_HI]->ann_FR_search(box_dist);// visit closer child first + + ANNcoord box_diff = ANNkdFRQ[cut_dim] - cd_bnds[ANN_HI]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + box_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + // visit further child if close enough + if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad) + child[ANN_LO]->ann_FR_search(box_dist); + + } + ANN_FLOP(13) // increment floating ops + ANN_SPL(1) // one more splitting node visited +} + +//---------------------------------------------------------------------- +// kd_leaf::ann_FR_search - search points in a leaf node +// Note: The unreadability of this code is the result of +// some fine tuning to replace indexing by pointer operations. +//---------------------------------------------------------------------- + +void ANNkd_leaf::ann_FR_search(ANNdist box_dist) +{ + register ANNdist dist; // distance to data point + register ANNcoord* pp; // data coordinate pointer + register ANNcoord* qq; // query coordinate pointer + register ANNcoord t; + register int d; + + for (int i = 0; i < n_pts; i++) { // check points in bucket + + pp = ANNkdFRPts[bkt[i]]; // first coord of next data point + qq = ANNkdFRQ; // first coord of query point + dist = 0; + + for(d = 0; d < ANNkdFRDim; d++) { + ANN_COORD(1) // one more coordinate hit + ANN_FLOP(5) // increment floating ops + + t = *(qq++) - *(pp++); // compute length and adv coordinate + // exceeds dist to k-th smallest? + if( (dist = ANN_SUM(dist, ANN_POW(t))) > ANNkdFRSqRad) { + break; + } + } + + if (d >= ANNkdFRDim && // among the k best? + (ANN_ALLOW_SELF_MATCH || dist!=0)) { // and no self-match problem + // add it to the list + ANNkdFRPointMK->insert(dist, bkt[i]); + ANNkdFRPtsInRange++; // increment point count + } + } + ANN_LEAF(1) // one more leaf node visited + ANN_PTS(n_pts) // increment points visited + ANNkdFRPtsVisited += n_pts; // increment number of points visited +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_pr_search.cpp b/geom_bottleneck/bottleneck/src/ann/kd_pr_search.cpp new file mode 100644 index 0000000..73d643f --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_pr_search.cpp @@ -0,0 +1,221 @@ +//---------------------------------------------------------------------- +// File: kd_pr_search.cpp +// Programmer: Sunil Arya and David Mount +// Description: Priority search for kd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +//---------------------------------------------------------------------- + +#include "kd_pr_search.h" // kd priority search declarations + +namespace geom_bt { +//---------------------------------------------------------------------- +// Approximate nearest neighbor searching by priority search. +// The kd-tree is searched for an approximate nearest neighbor. +// The point is returned through one of the arguments, and the +// distance returned is the SQUARED distance to this point. +// +// The method used for searching the kd-tree is called priority +// search. (It is described in Arya and Mount, ``Algorithms for +// fast vector quantization,'' Proc. of DCC '93: Data Compression +// Conference}, eds. J. A. Storer and M. Cohn, IEEE Press, 1993, +// 381--390.) +// +// The cell of the kd-tree containing the query point is located, +// and cells are visited in increasing order of distance from the +// query point. This is done by placing each subtree which has +// NOT been visited in a priority queue, according to the closest +// distance of the corresponding enclosing rectangle from the +// query point. The search stops when the distance to the nearest +// remaining rectangle exceeds the distance to the nearest point +// seen by a factor of more than 1/(1+eps). (Implying that any +// point found subsequently in the search cannot be closer by more +// than this factor.) +// +// The main entry point is annkPriSearch() which sets things up and +// then call the recursive routine ann_pri_search(). This is a +// recursive routine which performs the processing for one node in +// the kd-tree. There are two versions of this virtual procedure, +// one for splitting nodes and one for leaves. When a splitting node +// is visited, we determine which child to continue the search on +// (the closer one), and insert the other child into the priority +// queue. When a leaf is visited, we compute the distances to the +// points in the buckets, and update information on the closest +// points. +// +// Some trickery is used to incrementally update the distance from +// a kd-tree rectangle to the query point. This comes about from +// the fact that which each successive split, only one component +// (along the dimension that is split) of the squared distance to +// the child rectangle is different from the squared distance to +// the parent rectangle. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// To keep argument lists short, a number of global variables +// are maintained which are common to all the recursive calls. +// These are given below. +//---------------------------------------------------------------------- + +double ANNprEps; // the error bound +int ANNprDim; // dimension of space +ANNpoint ANNprQ; // query point +double ANNprMaxErr; // max tolerable squared error +ANNpointArray ANNprPts; // the points +ANNpr_queue *ANNprBoxPQ; // priority queue for boxes +ANNmin_k *ANNprPointMK; // set of k closest points + +//---------------------------------------------------------------------- +// annkPriSearch - priority search for k nearest neighbors +//---------------------------------------------------------------------- + +void ANNkd_tree::annkPriSearch( + ANNpoint q, // query point + int k, // number of near neighbors to return + ANNidxArray nn_idx, // nearest neighbor indices (returned) + ANNdistArray dd, // dist to near neighbors (returned) + double eps) // error bound (ignored) +{ + // max tolerable squared error + ANNprMaxErr = ANN_POW(1.0 + eps); + ANN_FLOP(2) // increment floating ops + + ANNprDim = dim; // copy arguments to static equivs + ANNprQ = q; + ANNprPts = pts; + ANNptsVisited = 0; // initialize count of points visited + + ANNprPointMK = new ANNmin_k(k); // create set for closest k points + + // distance to root box + ANNdist box_dist = annBoxDistance(q, + bnd_box_lo, bnd_box_hi, dim); + + ANNprBoxPQ = new ANNpr_queue(n_pts);// create priority queue for boxes + ANNprBoxPQ->insert(box_dist, root); // insert root in priority queue + + while (ANNprBoxPQ->non_empty() && + (!(ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited))) { + ANNkd_ptr np; // next box from prior queue + + // extract closest box from queue + ANNprBoxPQ->extr_min(box_dist, (void *&) np); + + ANN_FLOP(2) // increment floating ops + if (box_dist*ANNprMaxErr >= ANNprPointMK->max_key()) + break; + + np->ann_pri_search(box_dist); // search this subtree. + } + + for (int i = 0; i < k; i++) { // extract the k-th closest points + dd[i] = ANNprPointMK->ith_smallest_key(i); + nn_idx[i] = ANNprPointMK->ith_smallest_info(i); + } + + delete ANNprPointMK; // deallocate closest point set + delete ANNprBoxPQ; // deallocate priority queue +} + +//---------------------------------------------------------------------- +// kd_split::ann_pri_search - search a splitting node +//---------------------------------------------------------------------- + +void ANNkd_split::ann_pri_search(ANNdist box_dist) +{ + ANNdist new_dist; // distance to child visited later + // distance to cutting plane + ANNcoord cut_diff = ANNprQ[cut_dim] - cut_val; + + if (cut_diff < 0) { // left of cutting plane + ANNcoord box_diff = cd_bnds[ANN_LO] - ANNprQ[cut_dim]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + new_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + if (child[ANN_HI] != KD_TRIVIAL)// enqueue if not trivial + ANNprBoxPQ->insert(new_dist, child[ANN_HI]); + // continue with closer child + child[ANN_LO]->ann_pri_search(box_dist); + } + else { // right of cutting plane + ANNcoord box_diff = ANNprQ[cut_dim] - cd_bnds[ANN_HI]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + new_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + if (child[ANN_LO] != KD_TRIVIAL)// enqueue if not trivial + ANNprBoxPQ->insert(new_dist, child[ANN_LO]); + // continue with closer child + child[ANN_HI]->ann_pri_search(box_dist); + } + ANN_SPL(1) // one more splitting node visited + ANN_FLOP(8) // increment floating ops +} + +//---------------------------------------------------------------------- +// kd_leaf::ann_pri_search - search points in a leaf node +// +// This is virtually identical to the ann_search for standard search. +//---------------------------------------------------------------------- + +void ANNkd_leaf::ann_pri_search(ANNdist box_dist) +{ + register ANNdist dist; // distance to data point + register ANNcoord* pp; // data coordinate pointer + register ANNcoord* qq; // query coordinate pointer + register ANNdist min_dist; // distance to k-th closest point + register ANNcoord t; + register int d; + + min_dist = ANNprPointMK->max_key(); // k-th smallest distance so far + + for (int i = 0; i < n_pts; i++) { // check points in bucket + + pp = ANNprPts[bkt[i]]; // first coord of next data point + qq = ANNprQ; // first coord of query point + dist = 0; + + for(d = 0; d < ANNprDim; d++) { + ANN_COORD(1) // one more coordinate hit + ANN_FLOP(4) // increment floating ops + + t = *(qq++) - *(pp++); // compute length and adv coordinate + // exceeds dist to k-th smallest? + if( (dist = ANN_SUM(dist, ANN_POW(t))) > min_dist) { + break; + } + } + + if (d >= ANNprDim && // among the k best? + (ANN_ALLOW_SELF_MATCH || dist!=0)) { // and no self-match problem + // add it to the list + ANNprPointMK->insert(dist, bkt[i]); + min_dist = ANNprPointMK->max_key(); + } + } + ANN_LEAF(1) // one more leaf node visited + ANN_PTS(n_pts) // increment points visited + ANNptsVisited += n_pts; // increment number of points visited +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_search.cpp b/geom_bottleneck/bottleneck/src/ann/kd_search.cpp new file mode 100644 index 0000000..f559eb9 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_search.cpp @@ -0,0 +1,298 @@ +//---------------------------------------------------------------------- +// File: kd_search.cpp +// Programmer: Sunil Arya and David Mount +// Description: Standard kd-tree search +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +// Changed names LO, HI to ANN_LO, ANN_HI +// -------------------------------------------------------------------- +// 2015 - modified by A. Nigmetov to support deletion of points +//---------------------------------------------------------------------- + +#include "kd_search.h" // kd-search declarations + +namespace geom_bt { +//---------------------------------------------------------------------- +// Approximate nearest neighbor searching by kd-tree search +// The kd-tree is searched for an approximate nearest neighbor. +// The point is returned through one of the arguments, and the +// distance returned is the squared distance to this point. +// +// The method used for searching the kd-tree is an approximate +// adaptation of the search algorithm described by Friedman, +// Bentley, and Finkel, ``An algorithm for finding best matches +// in logarithmic expected time,'' ACM Transactions on Mathematical +// Software, 3(3):209-226, 1977). +// +// The algorithm operates recursively. When first encountering a +// node of the kd-tree we first visit the child which is closest to +// the query point. On return, we decide whether we want to visit +// the other child. If the box containing the other child exceeds +// 1/(1+eps) times the current best distance, then we skip it (since +// any point found in this child cannot be closer to the query point +// by more than this factor.) Otherwise, we visit it recursively. +// The distance between a box and the query point is computed exactly +// (not approximated as is often done in kd-tree), using incremental +// distance updates, as described by Arya and Mount in ``Algorithms +// for fast vector quantization,'' Proc. of DCC '93: Data Compression +// Conference, eds. J. A. Storer and M. Cohn, IEEE Press, 1993, +// 381-390. +// +// The main entry points is annkSearch() which sets things up and +// then call the recursive routine ann_search(). This is a recursive +// routine which performs the processing for one node in the kd-tree. +// There are two versions of this virtual procedure, one for splitting +// nodes and one for leaves. When a splitting node is visited, we +// determine which child to visit first (the closer one), and visit +// the other child on return. When a leaf is visited, we compute +// the distances to the points in the buckets, and update information +// on the closest points. +// +// Some trickery is used to incrementally update the distance from +// a kd-tree rectangle to the query point. This comes about from +// the fact that which each successive split, only one component +// (along the dimension that is split) of the squared distance to +// the child rectangle is different from the squared distance to +// the parent rectangle. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// To keep argument lists short, a number of global variables +// are maintained which are common to all the recursive calls. +// These are given below. +//---------------------------------------------------------------------- + +int ANNkdDim; // dimension of space +ANNpoint ANNkdQ; // query point +double ANNkdMaxErr; // max tolerable squared error +ANNpointArray ANNkdPts; // the points +ANNmin_k *ANNkdPointMK; // set of k closest points + +//---------------------------------------------------------------------- +// annkSearch - search for the k nearest neighbors +//---------------------------------------------------------------------- + +void ANNkd_tree::annkSearch( + ANNpoint q, // the query point + int k, // number of near neighbors to return + ANNidxArray nn_idx, // nearest neighbor indices (returned) + ANNdistArray dd, // the approximate nearest neighbor + double eps) // the error bound +{ + + ANNkdDim = dim; // copy arguments to static equivs + ANNkdQ = q; + ANNkdPts = pts; + ANNptsVisited = 0; // initialize count of points visited + + if (k > actual_num_points) { // too many near neighbors? + annError("Requesting more near neighbors than data points", ANNabort); + } + + ANNkdMaxErr = ANN_POW(1.0 + eps); + ANN_FLOP(2) // increment floating op count + + ANNkdPointMK = new ANNmin_k(k); // create set for closest k points + // search starting at the root + root->ann_search(annBoxDistance(q, bnd_box_lo, bnd_box_hi, dim)); + + for (int i = 0; i < k; i++) { // extract the k-th closest points + dd[i] = ANNkdPointMK->ith_smallest_key(i); + nn_idx[i] = ANNkdPointMK->ith_smallest_info(i); + } + delete ANNkdPointMK; // deallocate closest point set +} + +//---------------------------------------------------------------------- +// kd_split::ann_search - search a splitting node +//---------------------------------------------------------------------- + +void ANNkd_split::ann_search(ANNdist box_dist) +{ + // check if the subtree is empty + if (0 == actual_num_points) return; + // check dist calc term condition + if (ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited) return; + + // distance to cutting plane + ANNcoord cut_diff = ANNkdQ[cut_dim] - cut_val; + + if (cut_diff < 0) { // left of cutting plane + child[ANN_LO]->ann_search(box_dist);// visit closer child first + + ANNcoord box_diff = cd_bnds[ANN_LO] - ANNkdQ[cut_dim]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + box_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + // visit further child if close enough + if (box_dist * ANNkdMaxErr < ANNkdPointMK->max_key()) + child[ANN_HI]->ann_search(box_dist); + + } + else { // right of cutting plane + child[ANN_HI]->ann_search(box_dist);// visit closer child first + + ANNcoord box_diff = ANNkdQ[cut_dim] - cd_bnds[ANN_HI]; + if (box_diff < 0) // within bounds - ignore + box_diff = 0; + // distance to further box + box_dist = (ANNdist) ANN_SUM(box_dist, + ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); + + // visit further child if close enough + if (box_dist * ANNkdMaxErr < ANNkdPointMK->max_key()) + child[ANN_LO]->ann_search(box_dist); + + } + ANN_FLOP(10) // increment floating ops + ANN_SPL(1) // one more splitting node visited +} + +//---------------------------------------------------------------------- +// kd_leaf::ann_search - search points in a leaf node +// Note: The unreadability of this code is the result of +// some fine tuning to replace indexing by pointer operations. +//---------------------------------------------------------------------- + +void ANNkd_leaf::ann_search(ANNdist box_dist) +{ + register ANNdist dist; // distance to data point + register ANNcoord* pp; // data coordinate pointer + register ANNcoord* qq; // query coordinate pointer + register ANNdist min_dist; // distance to k-th closest point + register ANNcoord t; + register int d; + + min_dist = ANNkdPointMK->max_key(); // k-th smallest distance so far + + for (int i = 0; i < n_pts; i++) { // check points in bucket + + pp = ANNkdPts[bkt[i]]; // first coord of next data point + qq = ANNkdQ; // first coord of query point + dist = 0; + + for(d = 0; d < ANNkdDim; d++) { + ANN_COORD(1) // one more coordinate hit + ANN_FLOP(4) // increment floating ops + + t = *(qq++) - *(pp++); // compute length and adv coordinate + // exceeds dist to k-th smallest? + if( (dist = ANN_SUM(dist, ANN_POW(t))) > min_dist) { + break; + } + } + + if (d >= ANNkdDim && // among the k best? + (ANN_ALLOW_SELF_MATCH || dist!=0)) { // and no self-match problem + // add it to the list + ANNkdPointMK->insert(dist, bkt[i]); + min_dist = ANNkdPointMK->max_key(); + } + } + ANN_LEAF(1) // one more leaf node visited + ANN_PTS(n_pts) // increment points visited + ANNptsVisited += n_pts; // increment number of points visited +} + + + +//////////////////////////////////////////////// +// range search +// //////////////////////////////////////////// + +void ANNkd_tree::range_search(const ANNorthRect& region, + std::vector<size_t>& point_indices) +{ + + // get bounding box of the root + ANNorthRect bnd_box = ANNorthRect(dim, bnd_box_lo, bnd_box_hi); + root->range_search(region, dim, pts, bnd_box, point_indices); +} + +void ANNkd_split::range_search(const ANNorthRect& region, + int ANNkdDim, + ANNpointArray ANNkdPts, + ANNorthRect& bnd_box, + std::vector<size_t>& point_indices) +{ + // check if the subtree is empty + if (0 == actual_num_points) return; + + // process left child + ANNcoord old_bnd_box_val = bnd_box.hi[cut_dim]; + bnd_box.hi[cut_dim] = cut_val; + if (region.contains(ANNkdDim, bnd_box)) { + child[ANN_LO]->range_search_add(point_indices); + } else if (region.intersects(ANNkdDim, bnd_box)) { + child[ANN_LO]->range_search(region, ANNkdDim, ANNkdPts, bnd_box, point_indices); + } + // restore bounding box + bnd_box.hi[cut_dim] = old_bnd_box_val; + // process right child + old_bnd_box_val = bnd_box.lo[cut_dim]; + bnd_box.lo[cut_dim] = cut_val; + if (region.contains(ANNkdDim, bnd_box)) { + child[ANN_HI]->range_search_add(point_indices); + } else if (region.intersects(ANNkdDim, bnd_box)) { + child[ANN_HI]->range_search(region, ANNkdDim, ANNkdPts, bnd_box, point_indices); + } + // restore bounding box + bnd_box.lo[cut_dim] = old_bnd_box_val; +} + +void ANNkd_leaf::range_search(const ANNorthRect& region, + int ANNkdDim, + ANNpointArray ANNkdPts, + ANNorthRect&, // nameless parameter to suppress + // warnings and allow recursion + // in splitting node + std::vector<size_t>& point_indices) +{ + for (int i = 0; i < n_pts; i++) { // check points in bucket + if (region.inside(ANNkdDim, ANNkdPts[bkt[i]]) == ANNtrue) { + //std::cout << "adding point, i = " << i; + //std::cout << ", x = " << ANNkdPts[bkt[i]][0]; + //std::cout << ", y = " << ANNkdPts[bkt[i]][1] << std::endl; + point_indices.push_back(bkt[i]); + } + } +} + +void ANNkd_split::range_search_add(std::vector<size_t>& point_indices) +{ + if ( 0 == actual_num_points ) + return; + child[ANN_LO]->range_search_add(point_indices); + child[ANN_HI]->range_search_add(point_indices); +} + +void ANNkd_leaf::range_search_add(std::vector<size_t>& point_indices) +{ + if ( 0 == actual_num_points ) + return; + for (int i = 0; i < n_pts; i++) { // add all points in a bucket + //std::cout << "adding point without checking, i = " << i <<", bkt[i] = " << bkt[i] << std::endl; + point_indices.push_back(bkt[i]); + } +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_split.cpp b/geom_bottleneck/bottleneck/src/ann/kd_split.cpp new file mode 100644 index 0000000..7979aaa --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_split.cpp @@ -0,0 +1,632 @@ +//---------------------------------------------------------------------- +// File: kd_split.cpp +// Programmer: Sunil Arya and David Mount +// Description: Methods for splitting kd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +//---------------------------------------------------------------------- + +#include "kd_tree.h" // kd-tree definitions +#include "kd_util.h" // kd-tree utilities +#include "kd_split.h" // splitting functions + +namespace geom_bt { +//---------------------------------------------------------------------- +// Constants +//---------------------------------------------------------------------- + +const double ERR = 0.001; // a small value +const double FS_ASPECT_RATIO = 3.0; // maximum allowed aspect ratio + // in fair split. Must be >= 2. + +//---------------------------------------------------------------------- +// NOTE: Virtually all point indexing is done through an index (i.e. +// permutation) array pidx. Consequently, a reference to the d-th +// coordinate of the i-th point is pa[pidx[i]][d]. The macro PA(i,d) +// is a shorthand for this. +//---------------------------------------------------------------------- + // standard 2-d indirect indexing +#define PA(i,d) (pa[pidx[(i)]][(d)]) + // accessing a single point +#define PP(i) (pa[pidx[(i)]]) + + +//---------------------------------------------------------------------- +// kd_split - Bentley's standard splitting routine for kd-trees +// Find the dimension of the greatest spread, and split +// just before the median point along this dimension. +//---------------------------------------------------------------------- + +void kd_split( + ANNpointArray pa, // point array (permuted on return) + ANNidxArray pidx, // point indices + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + // find dimension of maximum spread + cut_dim = annMaxSpread(pa, pidx, n, dim); + n_lo = n/2; // median rank + // split about median + annMedianSplit(pa, pidx, n, cut_dim, cut_val, n_lo); +} + +//---------------------------------------------------------------------- +// midpt_split - midpoint splitting rule for box-decomposition trees +// +// This is the simplest splitting rule that guarantees boxes +// of bounded aspect ratio. It simply cuts the box with the +// longest side through its midpoint. If there are ties, it +// selects the dimension with the maximum point spread. +// +// WARNING: This routine (while simple) doesn't seem to work +// well in practice in high dimensions, because it tends to +// generate a large number of trivial and/or unbalanced splits. +// Either kd_split(), sl_midpt_split(), or fair_split() are +// recommended, instead. +//---------------------------------------------------------------------- + +void midpt_split( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + int d; + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + } + } + ANNcoord max_spread = -1; // find long side with most spread + for (d = 0; d < dim; d++) { + // is it among longest? + if (double(bnds.hi[d] - bnds.lo[d]) >= (1-ERR)*max_length) { + // compute its spread + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // is it max so far? + max_spread = spr; + cut_dim = d; + } + } + } + // split along cut_dim at midpoint + cut_val = (bnds.lo[cut_dim] + bnds.hi[cut_dim]) / 2; + // permute points accordingly + int br1, br2; + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + //------------------------------------------------------------------ + // On return: pa[0..br1-1] < cut_val + // pa[br1..br2-1] == cut_val + // pa[br2..n-1] > cut_val + // + // We can set n_lo to any value in the range [br1..br2]. + // We choose split so that points are most evenly divided. + //------------------------------------------------------------------ + if (br1 > n/2) n_lo = br1; + else if (br2 < n/2) n_lo = br2; + else n_lo = n/2; +} + +//---------------------------------------------------------------------- +// sl_midpt_split - sliding midpoint splitting rule +// +// This is a modification of midpt_split, which has the nonsensical +// name "sliding midpoint". The idea is that we try to use the +// midpoint rule, by bisecting the longest side. If there are +// ties, the dimension with the maximum spread is selected. If, +// however, the midpoint split produces a trivial split (no points +// on one side of the splitting plane) then we slide the splitting +// (maintaining its orientation) until it produces a nontrivial +// split. For example, if the splitting plane is along the x-axis, +// and all the data points have x-coordinate less than the x-bisector, +// then the split is taken along the maximum x-coordinate of the +// data points. +// +// Intuitively, this rule cannot generate trivial splits, and +// hence avoids midpt_split's tendency to produce trees with +// a very large number of nodes. +// +//---------------------------------------------------------------------- + +void sl_midpt_split( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + int d; + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + } + } + ANNcoord max_spread = -1; // find long side with most spread + for (d = 0; d < dim; d++) { + // is it among longest? + if ((bnds.hi[d] - bnds.lo[d]) >= (1-ERR)*max_length) { + // compute its spread + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // is it max so far? + max_spread = spr; + cut_dim = d; + } + } + } + // ideal split at midpoint + ANNcoord ideal_cut_val = (bnds.lo[cut_dim] + bnds.hi[cut_dim])/2; + + ANNcoord min, max; + annMinMax(pa, pidx, n, cut_dim, min, max); // find min/max coordinates + + if (ideal_cut_val < min) // slide to min or max as needed + cut_val = min; + else if (ideal_cut_val > max) + cut_val = max; + else + cut_val = ideal_cut_val; + + // permute points accordingly + int br1, br2; + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + //------------------------------------------------------------------ + // On return: pa[0..br1-1] < cut_val + // pa[br1..br2-1] == cut_val + // pa[br2..n-1] > cut_val + // + // We can set n_lo to any value in the range [br1..br2] to satisfy + // the exit conditions of the procedure. + // + // if ideal_cut_val < min (implying br2 >= 1), + // then we select n_lo = 1 (so there is one point on left) and + // if ideal_cut_val > max (implying br1 <= n-1), + // then we select n_lo = n-1 (so there is one point on right). + // Otherwise, we select n_lo as close to n/2 as possible within + // [br1..br2]. + //------------------------------------------------------------------ + if (ideal_cut_val < min) n_lo = 1; + else if (ideal_cut_val > max) n_lo = n-1; + else if (br1 > n/2) n_lo = br1; + else if (br2 < n/2) n_lo = br2; + else n_lo = n/2; +} + +//---------------------------------------------------------------------- +// fair_split - fair-split splitting rule +// +// This is a compromise between the kd-tree splitting rule (which +// always splits data points at their median) and the midpoint +// splitting rule (which always splits a box through its center. +// The goal of this procedure is to achieve both nicely balanced +// splits, and boxes of bounded aspect ratio. +// +// A constant FS_ASPECT_RATIO is defined. Given a box, those sides +// which can be split so that the ratio of the longest to shortest +// side does not exceed ASPECT_RATIO are identified. Among these +// sides, we select the one in which the points have the largest +// spread. We then split the points in a manner which most evenly +// distributes the points on either side of the splitting plane, +// subject to maintaining the bound on the ratio of long to short +// sides. To determine that the aspect ratio will be preserved, +// we determine the longest side (other than this side), and +// determine how narrowly we can cut this side, without causing the +// aspect ratio bound to be exceeded (small_piece). +// +// This procedure is more robust than either kd_split or midpt_split, +// but is more complicated as well. When point distribution is +// extremely skewed, this degenerates to midpt_split (actually +// 1/3 point split), and when the points are most evenly distributed, +// this degenerates to kd-split. +//---------------------------------------------------------------------- + +void fair_split( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + int d; + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + cut_dim = 0; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + cut_dim = d; + } + } + + ANNcoord max_spread = 0; // find legal cut with max spread + cut_dim = 0; + for (d = 0; d < dim; d++) { + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + // is this side midpoint splitable + // without violating aspect ratio? + if (((double) max_length)*2.0/((double) length) <= FS_ASPECT_RATIO) { + // compute spread along this dim + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // best spread so far + max_spread = spr; + cut_dim = d; // this is dimension to cut + } + } + } + + max_length = 0; // find longest side other than cut_dim + for (d = 0; d < dim; d++) { + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (d != cut_dim && length > max_length) + max_length = length; + } + // consider most extreme splits + ANNcoord small_piece = max_length / FS_ASPECT_RATIO; + ANNcoord lo_cut = bnds.lo[cut_dim] + small_piece;// lowest legal cut + ANNcoord hi_cut = bnds.hi[cut_dim] - small_piece;// highest legal cut + + int br1, br2; + // is median below lo_cut ? + if (annSplitBalance(pa, pidx, n, cut_dim, lo_cut) >= 0) { + cut_val = lo_cut; // cut at lo_cut + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = br1; + } + // is median above hi_cut? + else if (annSplitBalance(pa, pidx, n, cut_dim, hi_cut) <= 0) { + cut_val = hi_cut; // cut at hi_cut + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = br2; + } + else { // median cut preserves asp ratio + n_lo = n/2; // split about median + annMedianSplit(pa, pidx, n, cut_dim, cut_val, n_lo); + } +} + +//---------------------------------------------------------------------- +// sl_fair_split - sliding fair split splitting rule +// +// Sliding fair split is a splitting rule that combines the +// strengths of both fair split with sliding midpoint split. +// Fair split tends to produce balanced splits when the points +// are roughly uniformly distributed, but it can produce many +// trivial splits when points are highly clustered. Sliding +// midpoint never produces trivial splits, and shrinks boxes +// nicely if points are highly clustered, but it may produce +// rather unbalanced splits when points are unclustered but not +// quite uniform. +// +// Sliding fair split is based on the theory that there are two +// types of splits that are "good": balanced splits that produce +// fat boxes, and unbalanced splits provided the cell with fewer +// points is fat. +// +// This splitting rule operates by first computing the longest +// side of the current bounding box. Then it asks which sides +// could be split (at the midpoint) and still satisfy the aspect +// ratio bound with respect to this side. Among these, it selects +// the side with the largest spread (as fair split would). It +// then considers the most extreme cuts that would be allowed by +// the aspect ratio bound. This is done by dividing the longest +// side of the box by the aspect ratio bound. If the median cut +// lies between these extreme cuts, then we use the median cut. +// If not, then consider the extreme cut that is closer to the +// median. If all the points lie to one side of this cut, then +// we slide the cut until it hits the first point. This may +// violate the aspect ratio bound, but will never generate empty +// cells. However the sibling of every such skinny cell is fat, +// and hence packing arguments still apply. +// +//---------------------------------------------------------------------- + +void sl_fair_split( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo) // num of points on low side (returned) +{ + int d; + ANNcoord min, max; // min/max coordinates + int br1, br2; // split break points + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + cut_dim = 0; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + cut_dim = d; + } + } + + ANNcoord max_spread = 0; // find legal cut with max spread + cut_dim = 0; + for (d = 0; d < dim; d++) { + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + // is this side midpoint splitable + // without violating aspect ratio? + if (((double) max_length)*2.0/((double) length) <= FS_ASPECT_RATIO) { + // compute spread along this dim + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // best spread so far + max_spread = spr; + cut_dim = d; // this is dimension to cut + } + } + } + + max_length = 0; // find longest side other than cut_dim + for (d = 0; d < dim; d++) { + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (d != cut_dim && length > max_length) + max_length = length; + } + // consider most extreme splits + ANNcoord small_piece = max_length / FS_ASPECT_RATIO; + ANNcoord lo_cut = bnds.lo[cut_dim] + small_piece;// lowest legal cut + ANNcoord hi_cut = bnds.hi[cut_dim] - small_piece;// highest legal cut + // find min and max along cut_dim + annMinMax(pa, pidx, n, cut_dim, min, max); + // is median below lo_cut? + if (annSplitBalance(pa, pidx, n, cut_dim, lo_cut) >= 0) { + if (max > lo_cut) { // are any points above lo_cut? + cut_val = lo_cut; // cut at lo_cut + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = br1; // balance if there are ties + } + else { // all points below lo_cut + cut_val = max; // cut at max value + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = n-1; + } + } + // is median above hi_cut? + else if (annSplitBalance(pa, pidx, n, cut_dim, hi_cut) <= 0) { + if (min < hi_cut) { // are any points below hi_cut? + cut_val = hi_cut; // cut at hi_cut + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = br2; // balance if there are ties + } + else { // all points above hi_cut + cut_val = min; // cut at min value + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + n_lo = 1; + } + } + else { // median cut is good enough + n_lo = n/2; // split about median + annMedianSplit(pa, pidx, n, cut_dim, cut_val, n_lo); + } +} + + +///////////////////////////////////////////////////////////////////////////////// +// for kd-trees with deletion +// +//---------------------------------------------------------------------- +// kd_split - Bentley's standard splitting routine for kd-trees +// Find the dimension of the greatest spread, and split +// just before the median point along this dimension. +//---------------------------------------------------------------------- + +void kd_split_wd( + ANNpointArray pa, // point array (permuted on return) + ANNidxArray pidx, // point indices + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo, // num of points on low side (returned) + int &cut_pt_idx) // index of cutting point (returned) +{ + // find dimension of maximum spread + cut_dim = annMaxSpread(pa, pidx, n, dim); + n_lo = n/2; // median rank + // split about median + annMedianSplit(pa, pidx, n, cut_dim, cut_val, n_lo); + cut_pt_idx = n_lo; + cut_val = PA(cut_pt_idx, cut_dim); +} + +//---------------------------------------------------------------------- +// midpt_split - midpoint splitting rule for box-decomposition trees +// +// This is the simplest splitting rule that guarantees boxes +// of bounded aspect ratio. It simply cuts the box with the +// longest side through its midpoint. If there are ties, it +// selects the dimension with the maximum point spread. +// +// WARNING: This routine (while simple) doesn't seem to work +// well in practice in high dimensions, because it tends to +// generate a large number of trivial and/or unbalanced splits. +// Either kd_split(), sl_midpt_split(), or fair_split() are +// recommended, instead. +//---------------------------------------------------------------------- + +void midpt_split_wd( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo, // num of points on low side (returned) + int &cut_pt_idx) // index of cutting point (returned) +{ + int d; + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + } + } + ANNcoord max_spread = -1; // find long side with most spread + for (d = 0; d < dim; d++) { + // is it among longest? + if (double(bnds.hi[d] - bnds.lo[d]) >= (1-ERR)*max_length) { + // compute its spread + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // is it max so far? + max_spread = spr; + cut_dim = d; + } + } + } + // split along cut_dim at midpoint + cut_val = (bnds.lo[cut_dim] + bnds.hi[cut_dim]) / 2; + // permute points accordingly + int br1, br2; + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + //------------------------------------------------------------------ + // On return: pa[0..br1-1] < cut_val + // pa[br1..br2-1] == cut_val + // pa[br2..n-1] > cut_val + // + // We can set n_lo to any value in the range [br1..br2]. + // We choose split so that points are most evenly divided. + //------------------------------------------------------------------ + if (br1 > n/2) n_lo = br1; + else if (br2 < n/2) n_lo = br2; + else n_lo = n/2; + + cut_pt_idx = n_lo; + cut_val = PA(cut_pt_idx, cut_dim); + +} + +//---------------------------------------------------------------------- +// sl_midpt_split - sliding midpoint splitting rule +// +// This is a modification of midpt_split, which has the nonsensical +// name "sliding midpoint". The idea is that we try to use the +// midpoint rule, by bisecting the longest side. If there are +// ties, the dimension with the maximum spread is selected. If, +// however, the midpoint split produces a trivial split (no points +// on one side of the splitting plane) then we slide the splitting +// (maintaining its orientation) until it produces a nontrivial +// split. For example, if the splitting plane is along the x-axis, +// and all the data points have x-coordinate less than the x-bisector, +// then the split is taken along the maximum x-coordinate of the +// data points. +// +// Intuitively, this rule cannot generate trivial splits, and +// hence avoids midpt_split's tendency to produce trees with +// a very large number of nodes. +// +//---------------------------------------------------------------------- + +void sl_midpt_split_wd( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices (permuted on return) + const ANNorthRect &bnds, // bounding rectangle for cell + int n, // number of points + int dim, // dimension of space + int &cut_dim, // cutting dimension (returned) + ANNcoord &cut_val, // cutting value (returned) + int &n_lo, // num of points on low side (returned) + int &cut_pt_idx) // index of cutting point (returned) +{ + int d; + + ANNcoord max_length = bnds.hi[0] - bnds.lo[0]; + for (d = 1; d < dim; d++) { // find length of longest box side + ANNcoord length = bnds.hi[d] - bnds.lo[d]; + if (length > max_length) { + max_length = length; + } + } + ANNcoord max_spread = -1; // find long side with most spread + for (d = 0; d < dim; d++) { + // is it among longest? + if ((bnds.hi[d] - bnds.lo[d]) >= (1-ERR)*max_length) { + // compute its spread + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spread) { // is it max so far? + max_spread = spr; + cut_dim = d; + } + } + } + // ideal split at midpoint + ANNcoord ideal_cut_val = (bnds.lo[cut_dim] + bnds.hi[cut_dim])/2; + + ANNcoord min, max; + annMinMax(pa, pidx, n, cut_dim, min, max); // find min/max coordinates + + if (ideal_cut_val < min) // slide to min or max as needed + cut_val = min; + else if (ideal_cut_val > max) + cut_val = max; + else + cut_val = ideal_cut_val; + + // permute points accordingly + int br1, br2; + annPlaneSplit(pa, pidx, n, cut_dim, cut_val, br1, br2); + //------------------------------------------------------------------ + // On return: pa[0..br1-1] < cut_val + // pa[br1..br2-1] == cut_val + // pa[br2..n-1] > cut_val + // + // We can set n_lo to any value in the range [br1..br2] to satisfy + // the exit conditions of the procedure. + // + // if ideal_cut_val < min (implying br2 >= 1), + // then we select n_lo = 1 (so there is one point on left) and + // if ideal_cut_val > max (implying br1 <= n-1), + // then we select n_lo = n-1 (so there is one point on right). + // Otherwise, we select n_lo as close to n/2 as possible within + // [br1..br2]. + //------------------------------------------------------------------ + if (ideal_cut_val < min) n_lo = 1; + else if (ideal_cut_val > max) n_lo = n-1; + else if (br1 > n/2) n_lo = br1; + else if (br2 < n/2) n_lo = br2; + else n_lo = n/2; +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_tree.cpp b/geom_bottleneck/bottleneck/src/ann/kd_tree.cpp new file mode 100644 index 0000000..ad3a82d --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_tree.cpp @@ -0,0 +1,560 @@ +//---------------------------------------------------------------------- +// File: kd_tree.cpp +// Programmer: Sunil Arya and David Mount +// Description: Basic methods for kd-trees. +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +// Revision 1.0 04/01/05 +// Increased aspect ratio bound (ANN_AR_TOOBIG) from 100 to 1000. +// Fixed leaf counts to count trivial leaves. +// Added optional pa, pi arguments to Skeleton kd_tree constructor +// for use in load constructor. +// Added annClose() to eliminate KD_TRIVIAL memory leak. +// -------------------------------------------------------------------- +// 2015 - modified by A. Nigmetov to support deletion of points +//---------------------------------------------------------------------- + +#ifdef _WIN32 +#include <ciso646> // make VS more conformal +#endif + +#include "kd_tree.h" // kd-tree declarations +#include "kd_split.h" // kd-tree splitting rules +#include "kd_util.h" // kd-tree utilities +#include <ANN/ANNperf.h> // performance evaluation + +namespace geom_bt { +//---------------------------------------------------------------------- +// Global data +// +// For some splitting rules, especially with small bucket sizes, +// it is possible to generate a large number of empty leaf nodes. +// To save storage we allocate a single trivial leaf node which +// contains no points. For messy coding reasons it is convenient +// to have it reference a trivial point index. +// +// KD_TRIVIAL is allocated when the first kd-tree is created. It +// must *never* deallocated (since it may be shared by more than +// one tree). +//---------------------------------------------------------------------- +static int IDX_TRIVIAL[] = {0}; // trivial point index +ANNkd_leaf *KD_TRIVIAL = NULL; // trivial leaf node + +//---------------------------------------------------------------------- +// Printing the kd-tree +// These routines print a kd-tree in reverse inorder (high then +// root then low). (This is so that if you look at the output +// from the right side it appear from left to right in standard +// inorder.) When outputting leaves we output only the point +// indices rather than the point coordinates. There is an option +// to print the point coordinates separately. +// +// The tree printing routine calls the printing routines on the +// individual nodes of the tree, passing in the level or depth +// in the tree. The level in the tree is used to print indentation +// for readability. +//---------------------------------------------------------------------- + +void ANNkd_split::print( // print splitting node + int level, // depth of node in tree + ostream &out) // output stream +{ + child[ANN_HI]->print(level+1, out); // print high child + out << " "; + for (int i = 0; i < level; i++) // print indentation + out << ".."; + out << "Split cd=" << cut_dim << " cv=" << cut_val; + out << " lbnd=" << cd_bnds[ANN_LO]; + out << " hbnd=" << cd_bnds[ANN_HI]; + out << " np=" << actual_num_points; + out << "\n"; + child[ANN_LO]->print(level+1, out); // print low child +} + +void ANNkd_leaf::print( // print leaf node + int level, // depth of node in tree + ostream &out) // output stream +{ + + out << " "; + for (int i = 0; i < level; i++) // print indentation + out << ".."; + + if (this == KD_TRIVIAL) { // canonical trivial leaf node + out << "Leaf (trivial)\n"; + } + else{ + out << "Leaf n=" << n_pts << " <"; + for (int j = 0; j < n_pts; j++) { + out << bkt[j]; + if (j < n_pts-1) out << ","; + } + out << ">\n"; + } +} + +void ANNkd_tree::Print( // print entire tree + ANNbool with_pts, // print points as well? + ostream &out) // output stream +{ + out << "ANN Version " << ANNversion << "\n"; + if (with_pts) { // print point coordinates + out << " Points:\n"; + for (int i = 0; i < n_pts; i++) { + out << "\t" << i << ": "; + annPrintPt(pts[i], dim, out); + out << "\n"; + } + } + if (root == NULL) // empty tree? + out << " Null tree.\n"; + else { + root->print(0, out); // invoke printing at root + } +} + +//---------------------------------------------------------------------- +// kd_tree statistics (for performance evaluation) +// This routine compute various statistics information for +// a kd-tree. It is used by the implementors for performance +// evaluation of the data structure. +//---------------------------------------------------------------------- + +#define MAX(a,b) ((a) > (b) ? (a) : (b)) + +void ANNkdStats::merge(const ANNkdStats &st) // merge stats from child +{ + n_lf += st.n_lf; n_tl += st.n_tl; + n_spl += st.n_spl; n_shr += st.n_shr; + depth = MAX(depth, st.depth); + sum_ar += st.sum_ar; +} + +//---------------------------------------------------------------------- +// Update statistics for nodes +//---------------------------------------------------------------------- + +const double ANN_AR_TOOBIG = 1000; // too big an aspect ratio + +void ANNkd_leaf::getStats( // get subtree statistics + int dim, // dimension of space + ANNkdStats &st, // stats (modified) + ANNorthRect &bnd_box) // bounding box +{ + st.reset(); + st.n_lf = 1; // count this leaf + if (this == KD_TRIVIAL) st.n_tl = 1; // count trivial leaf + double ar = annAspectRatio(dim, bnd_box); // aspect ratio of leaf + // incr sum (ignore outliers) + st.sum_ar += float(ar < ANN_AR_TOOBIG ? ar : ANN_AR_TOOBIG); +} + +void ANNkd_split::getStats( // get subtree statistics + int dim, // dimension of space + ANNkdStats &st, // stats (modified) + ANNorthRect &bnd_box) // bounding box +{ + ANNkdStats ch_stats; // stats for children + // get stats for low child + ANNcoord hv = bnd_box.hi[cut_dim]; // save box bounds + bnd_box.hi[cut_dim] = cut_val; // upper bound for low child + ch_stats.reset(); // reset + child[ANN_LO]->getStats(dim, ch_stats, bnd_box); + st.merge(ch_stats); // merge them + bnd_box.hi[cut_dim] = hv; // restore bound + // get stats for high child + ANNcoord lv = bnd_box.lo[cut_dim]; // save box bounds + bnd_box.lo[cut_dim] = cut_val; // lower bound for high child + ch_stats.reset(); // reset + child[ANN_HI]->getStats(dim, ch_stats, bnd_box); + st.merge(ch_stats); // merge them + bnd_box.lo[cut_dim] = lv; // restore bound + + st.depth++; // increment depth + st.n_spl++; // increment number of splits +} + +//---------------------------------------------------------------------- +// getStats +// Collects a number of statistics related to kd_tree or +// bd_tree. +//---------------------------------------------------------------------- + +void ANNkd_tree::getStats( // get tree statistics + ANNkdStats &st) // stats (modified) +{ + st.reset(dim, n_pts, bkt_size); // reset stats + // create bounding box + ANNorthRect bnd_box(dim, bnd_box_lo, bnd_box_hi); + if (root != NULL) { // if nonempty tree + root->getStats(dim, st, bnd_box); // get statistics + st.avg_ar = st.sum_ar / st.n_lf; // average leaf asp ratio + } +} + +//---------------------------------------------------------------------- +// kd_tree destructor +// The destructor just frees the various elements that were +// allocated in the construction process. +//---------------------------------------------------------------------- + +ANNkd_tree::~ANNkd_tree() // tree destructor +{ + if (root != NULL and root != KD_TRIVIAL) delete root; + if (pidx != NULL) delete [] pidx; + if (bnd_box_lo != NULL) annDeallocPt(bnd_box_lo); + if (bnd_box_hi != NULL) annDeallocPt(bnd_box_hi); +} + +//---------------------------------------------------------------------- +// This is called with all use of ANN is finished. It eliminates the +// minor memory leak caused by the allocation of KD_TRIVIAL. +//---------------------------------------------------------------------- +void annClose() // close use of ANN +{ + if (KD_TRIVIAL != NULL) { + delete KD_TRIVIAL; + KD_TRIVIAL = NULL; + } +} + +//---------------------------------------------------------------------- +// kd_tree constructors +// There is a skeleton kd-tree constructor which sets up a +// trivial empty tree. The last optional argument allows +// the routine to be passed a point index array which is +// assumed to be of the proper size (n). Otherwise, one is +// allocated and initialized to the identity. Warning: In +// either case the destructor will deallocate this array. +// +// As a kludge, we need to allocate KD_TRIVIAL if one has not +// already been allocated. (This is because I'm too dumb to +// figure out how to cause a pointer to be allocated at load +// time.) +//---------------------------------------------------------------------- + +void ANNkd_tree::SkeletonTree( // construct skeleton tree + int n, // number of points + int dd, // dimension + int bs, // bucket size + ANNpointArray pa, // point array + ANNidxArray pi) // point indices +{ + dim = dd; // initialize basic elements + n_pts = n; + bkt_size = bs; + pts = pa; // initialize points array + + root = NULL; // no associated tree yet + + if (pi == NULL) { // point indices provided? + pidx = new ANNidx[n]; // no, allocate space for point indices + for (int i = 0; i < n; i++) { + pidx[i] = i; // initially identity + } + } + else { + pidx = pi; // yes, use them + } + + bnd_box_lo = bnd_box_hi = NULL; // bounding box is nonexistent + if (KD_TRIVIAL == NULL) // no trivial leaf node yet? + KD_TRIVIAL = new ANNkd_leaf(0, IDX_TRIVIAL); // allocate it + + // for deletion + pointToLeafVec.clear(); + pointToLeafVec.reserve(n_pts); + for(int k = 0; k < n_pts; ++k) { + pointToLeafVec.push_back(NULL); + } +} + +ANNkd_tree::ANNkd_tree( // basic constructor + int n, // number of points + int dd, // dimension + int bs) // bucket size +{ SkeletonTree(n, dd, bs); } // construct skeleton tree + + + +//---------------------------------------------------------------------- +// rkd_tree - recursive procedure to build a kd-tree +// +// Builds a kd-tree for points in pa as indexed through the +// array pidx[0..n-1] (typically a subarray of the array used in +// the top-level call). This routine permutes the array pidx, +// but does not alter pa[]. +// +// The construction is based on a standard algorithm for constructing +// the kd-tree (see Friedman, Bentley, and Finkel, ``An algorithm for +// finding best matches in logarithmic expected time,'' ACM Transactions +// on Mathematical Software, 3(3):209-226, 1977). The procedure +// operates by a simple divide-and-conquer strategy, which determines +// an appropriate orthogonal cutting plane (see below), and splits +// the points. When the number of points falls below the bucket size, +// we simply store the points in a leaf node's bucket. +// +// One of the arguments is a pointer to a splitting routine, +// whose prototype is: +// +// void split( +// ANNpointArray pa, // complete point array +// ANNidxArray pidx, // point array (permuted on return) +// ANNorthRect &bnds, // bounds of current cell +// int n, // number of points +// int dim, // dimension of space +// int &cut_dim, // cutting dimension +// ANNcoord &cut_val, // cutting value +// int &n_lo) // no. of points on low side of cut +// +// This procedure selects a cutting dimension and cutting value, +// partitions pa about these values, and returns the number of +// points on the low side of the cut. +//---------------------------------------------------------------------- + +ANNkd_ptr rkd_tree( // recursive construction of kd-tree + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + int bsp, // bucket space + ANNorthRect &bnd_box, // bounding box for current node + ANNkd_splitter splitter, // splitting routine + vector<ANNkd_leaf*>* ppointToLeafVec) +{ + if (n <= bsp) { // n small, make a leaf node + if (n == 0) // empty leaf node + return KD_TRIVIAL; // return (canonical) empty leaf + else { // construct the node and return + ANNkd_leaf* res = new ANNkd_leaf(n, pidx); + if ( 1 == bsp) { + (*ppointToLeafVec)[*pidx] = res; + } + return res; + } + } + else { // n large, make a splitting node + int cd; // cutting dimension + ANNcoord cv; // cutting value + int n_lo; // number on low side of cut + ANNkd_node *lo, *hi; // low and high children + + // invoke splitting procedure + (*splitter)(pa, pidx, bnd_box, n, dim, cd, cv, n_lo); + + ANNcoord lv = bnd_box.lo[cd]; // save bounds for cutting dimension + ANNcoord hv = bnd_box.hi[cd]; + + bnd_box.hi[cd] = cv; // modify bounds for left subtree + lo = rkd_tree( // build left subtree + pa, pidx, n_lo, // ...from pidx[0..n_lo-1] + dim, bsp, bnd_box, splitter, ppointToLeafVec); + bnd_box.hi[cd] = hv; // restore bounds + + bnd_box.lo[cd] = cv; // modify bounds for right subtree + hi = rkd_tree( // build right subtree + pa, pidx + n_lo, n-n_lo,// ...from pidx[n_lo..n-1] + dim, bsp, bnd_box, splitter, ppointToLeafVec); + bnd_box.lo[cd] = lv; // restore bounds + + // create the splitting node + ANNkd_split *ptr = new ANNkd_split(cd, cv, lv, hv, lo, hi); + if (lo != KD_TRIVIAL) + lo->setParent(ptr); + if (hi != KD_TRIVIAL) + hi->setParent(ptr); + ptr->setNumPoints(lo->getNumPoints() + hi->getNumPoints()); + + return ptr; // return pointer to this node + } +} + +// for kd-trees with deletion +/* +ANNkd_ptr rkd_tree_wd( // recursive construction of kd-tree + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices to store in subtree + int n, // number of points + int dim, // dimension of space + int bsp, // bucket space + ANNorthRect &bnd_box, // bounding box for current node + ANNkd_splitter_wd splitter) // splitting routine +{ + ANNidx cut_pt_idx; + if (n <= bsp) { // n small, make a leaf node + if (n == 0) // empty leaf node + return KD_TRIVIAL; // return (canonical) empty leaf + else // construct the node and return + return new ANNkd_leaf(n, pidx); + } + else { // n large, make a splitting node + int cd; // cutting dimension + ANNcoord cv; // cutting value + int n_lo; // number on low side of cut + ANNkd_node *lo, *hi; // low and high children + + // invoke splitting procedure + (*splitter)(pa, pidx, bnd_box, n, dim, cd, cv, n_lo, cut_pt_idx); + + ANNcoord lv = bnd_box.lo[cd]; // save bounds for cutting dimension + ANNcoord hv = bnd_box.hi[cd]; + + bnd_box.hi[cd] = cv; // modify bounds for left subtree + lo = rkd_tree_wd( // build left subtree + pa, pidx, n_lo, // ...from pidx[0..n_lo-1] + dim, bsp, bnd_box, splitter); + bnd_box.hi[cd] = hv; // restore bounds + + bnd_box.lo[cd] = cv; // modify bounds for right subtree + hi = rkd_tree_wd( // build right subtree + pa, pidx + n_lo, n-n_lo,// ...from pidx[n_lo..n-1] + dim, bsp, bnd_box, splitter); + bnd_box.lo[cd] = lv; // restore bounds + + // create the splitting node + ANNkd_split *ptr = new ANNkd_split(cd, cv, lv, hv, lo, hi, cut_pt_idx); + + return ptr; // return pointer to this node + } +} +*/ + +//---------------------------------------------------------------------- +// kd-tree constructor +// This is the main constructor for kd-trees given a set of points. +// It first builds a skeleton tree, then computes the bounding box +// of the data points, and then invokes rkd_tree() to actually +// build the tree, passing it the appropriate splitting routine. +//---------------------------------------------------------------------- + +ANNkd_tree::ANNkd_tree( // construct from point array + ANNpointArray pa, // point array (with at least n pts) + int n, // number of points + int dd, // dimension + int bs, // bucket size + ANNsplitRule split) // splitting method +{ + SkeletonTree(n, dd, bs); // set up the basic stuff + pts = pa; // where the points are + actual_num_points = n; + if (n == 0) return; // no points--no sweat + + ANNorthRect bnd_box(dd); // bounding box for points + annEnclRect(pa, pidx, n, dd, bnd_box);// construct bounding rectangle + // copy to tree structure + bnd_box_lo = annCopyPt(dd, bnd_box.lo); + bnd_box_hi = annCopyPt(dd, bnd_box.hi); + + switch (split) { // build by rule + case ANN_KD_STD: // standard kd-splitting rule + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, kd_split, &pointToLeafVec); + break; + case ANN_KD_MIDPT: // midpoint split + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, midpt_split, &pointToLeafVec); + break; + case ANN_KD_FAIR: // fair split + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, fair_split, &pointToLeafVec); + break; + case ANN_KD_SUGGEST: // best (in our opinion) + case ANN_KD_SL_MIDPT: // sliding midpoint split + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, sl_midpt_split, &pointToLeafVec); + break; + case ANN_KD_SL_FAIR: // sliding fair split + root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, sl_fair_split, &pointToLeafVec); + break; + // for kd-trees with deletion + /* + //case ANN_KD_SUGGEST: + case ANN_KD_STD_WD: + root = rkd_tree_wd(pa, pidx, n, dd, bs, bnd_box, kd_split_wd); + break; + case ANN_KD_MIDPT_WD: + root = rkd_tree_wd(pa, pidx, n, dd, bs, bnd_box, kd_split_wd); + break; + case ANN_KD_SL_MIDPT_WD: + root = rkd_tree_wd(pa, pidx, n, dd, bs, bnd_box, kd_split_wd); + break; + */ + default: + annError("Illegal splitting method", ANNabort); + } +} + + +// deletion code +// +// +// +// +// +void ANNkd_tree::delete_point(const int point_idx) +{ + // range check + assert(0 <= point_idx and point_idx < n_pts); + assert(actual_num_points > 0); + // if this is the first deletion, + // initialize isDeleted vector + if (isDeleted.empty()) { + isDeleted.reserve(n_pts); + for(size_t k = 0; k < n_pts; ++k) { + isDeleted.push_back(false); + } + } + // points shouldn't be deleted twice + assert(!isDeleted[point_idx]); + assert(root != NULL); + ANNkd_leaf* leafWithPoint = pointToLeafVec.at(point_idx); + assert(leafWithPoint != NULL); + // if leafWithPoint != root, + // its parent will delete the leaf + pointToLeafVec.at(point_idx)->delete_point(point_idx, leafWithPoint != root); + if (leafWithPoint == root) { + // we had only one point, + // so the tree must delete it + root = KD_TRIVIAL; + delete leafWithPoint; + } + isDeleted[point_idx] = true; + actual_num_points--; +} + +void ANNkd_leaf::delete_point(const int point_idx, const bool killYourself) +{ + assert(n_pts == 1); + assert(bkt[0] == point_idx); + ANNkd_split* myPar = parent; + while(myPar != NULL) { + myPar->decNumPoints(); + myPar = myPar->getParent(); + } + if (parent != NULL) + parent->delete_leaf(this); + if (killYourself) + delete this; +} + +void ANNkd_split::delete_leaf(ANNkd_leaf* childToDelete) +{ + assert(child[ANN_LO] == childToDelete or child[ANN_HI] == childToDelete); + if (child[ANN_LO] == childToDelete) + child[ANN_LO] = KD_TRIVIAL; + else + child[ANN_HI] = KD_TRIVIAL; +} +} diff --git a/geom_bottleneck/bottleneck/src/ann/kd_util.cpp b/geom_bottleneck/bottleneck/src/ann/kd_util.cpp new file mode 100644 index 0000000..02b35c4 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/ann/kd_util.cpp @@ -0,0 +1,441 @@ +//---------------------------------------------------------------------- +// File: kd_util.cpp +// Programmer: Sunil Arya and David Mount +// Description: Common utilities for kd-trees +// Last modified: 01/04/05 (Version 1.0) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +//---------------------------------------------------------------------- + +#include "kd_util.h" // kd-utility declarations + +#include <ANN/ANNperf.h> // performance evaluation + +namespace geom_bt { +//---------------------------------------------------------------------- +// The following routines are utility functions for manipulating +// points sets, used in determining splitting planes for kd-tree +// construction. +//---------------------------------------------------------------------- + +//---------------------------------------------------------------------- +// NOTE: Virtually all point indexing is done through an index (i.e. +// permutation) array pidx. Consequently, a reference to the d-th +// coordinate of the i-th point is pa[pidx[i]][d]. The macro PA(i,d) +// is a shorthand for this. +//---------------------------------------------------------------------- + // standard 2-d indirect indexing +#define PA(i,d) (pa[pidx[(i)]][(d)]) + // accessing a single point +#define PP(i) (pa[pidx[(i)]]) + +//---------------------------------------------------------------------- +// annAspectRatio +// Compute the aspect ratio (ratio of longest to shortest side) +// of a rectangle. +//---------------------------------------------------------------------- + +double annAspectRatio( + int dim, // dimension + const ANNorthRect &bnd_box) // bounding cube +{ + ANNcoord length = bnd_box.hi[0] - bnd_box.lo[0]; + ANNcoord min_length = length; // min side length + ANNcoord max_length = length; // max side length + for (int d = 0; d < dim; d++) { + length = bnd_box.hi[d] - bnd_box.lo[d]; + if (length < min_length) min_length = length; + if (length > max_length) max_length = length; + } + return max_length/min_length; +} + +//---------------------------------------------------------------------- +// annEnclRect, annEnclCube +// These utilities compute the smallest rectangle and cube enclosing +// a set of points, respectively. +//---------------------------------------------------------------------- + +void annEnclRect( + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int dim, // dimension + ANNorthRect &bnds) // bounding cube (returned) +{ + for (int d = 0; d < dim; d++) { // find smallest enclosing rectangle + ANNcoord lo_bnd = PA(0,d); // lower bound on dimension d + ANNcoord hi_bnd = PA(0,d); // upper bound on dimension d + for (int i = 0; i < n; i++) { + if (PA(i,d) < lo_bnd) lo_bnd = PA(i,d); + else if (PA(i,d) > hi_bnd) hi_bnd = PA(i,d); + } + bnds.lo[d] = lo_bnd; + bnds.hi[d] = hi_bnd; + } +} + +void annEnclCube( // compute smallest enclosing cube + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int dim, // dimension + ANNorthRect &bnds) // bounding cube (returned) +{ + int d; + // compute smallest enclosing rect + annEnclRect(pa, pidx, n, dim, bnds); + + ANNcoord max_len = 0; // max length of any side + for (d = 0; d < dim; d++) { // determine max side length + ANNcoord len = bnds.hi[d] - bnds.lo[d]; + if (len > max_len) { // update max_len if longest + max_len = len; + } + } + for (d = 0; d < dim; d++) { // grow sides to match max + ANNcoord len = bnds.hi[d] - bnds.lo[d]; + ANNcoord half_diff = (max_len - len) / 2; + bnds.lo[d] -= half_diff; + bnds.hi[d] += half_diff; + } +} + +//---------------------------------------------------------------------- +// annBoxDistance - utility routine which computes distance from point to +// box (Note: most distances to boxes are computed using incremental +// distance updates, not this function.) +//---------------------------------------------------------------------- + +ANNdist annBoxDistance( // compute distance from point to box + const ANNpoint q, // the point + const ANNpoint lo, // low point of box + const ANNpoint hi, // high point of box + int dim) // dimension of space +{ + register ANNdist dist = 0.0; // sum of squared distances + register ANNdist t; + + for (register int d = 0; d < dim; d++) { + if (q[d] < lo[d]) { // q is left of box + t = ANNdist(lo[d]) - ANNdist(q[d]); + dist = ANN_SUM(dist, ANN_POW(t)); + } + else if (q[d] > hi[d]) { // q is right of box + t = ANNdist(q[d]) - ANNdist(hi[d]); + dist = ANN_SUM(dist, ANN_POW(t)); + } + } + ANN_FLOP(4*dim) // increment floating op count + + return dist; +} + +//---------------------------------------------------------------------- +// annSpread - find spread along given dimension +// annMinMax - find min and max coordinates along given dimension +// annMaxSpread - find dimension of max spread +//---------------------------------------------------------------------- + +ANNcoord annSpread( // compute point spread along dimension + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int d) // dimension to check +{ + ANNcoord min = PA(0,d); // compute max and min coords + ANNcoord max = PA(0,d); + for (int i = 1; i < n; i++) { + ANNcoord c = PA(i,d); + if (c < min) min = c; + else if (c > max) max = c; + } + return (max - min); // total spread is difference +} + +void annMinMax( // compute min and max coordinates along dim + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int d, // dimension to check + ANNcoord &min, // minimum value (returned) + ANNcoord &max) // maximum value (returned) +{ + min = PA(0,d); // compute max and min coords + max = PA(0,d); + for (int i = 1; i < n; i++) { + ANNcoord c = PA(i,d); + if (c < min) min = c; + else if (c > max) max = c; + } +} + +int annMaxSpread( // compute dimension of max spread + ANNpointArray pa, // point array + ANNidxArray pidx, // point indices + int n, // number of points + int dim) // dimension of space +{ + int max_dim = 0; // dimension of max spread + ANNcoord max_spr = 0; // amount of max spread + + if (n == 0) return max_dim; // no points, who cares? + + for (int d = 0; d < dim; d++) { // compute spread along each dim + ANNcoord spr = annSpread(pa, pidx, n, d); + if (spr > max_spr) { // bigger than current max + max_spr = spr; + max_dim = d; + } + } + return max_dim; +} + +//---------------------------------------------------------------------- +// annMedianSplit - split point array about its median +// Splits a subarray of points pa[0..n] about an element of given +// rank (median: n_lo = n/2) with respect to dimension d. It places +// the element of rank n_lo-1 correctly (because our splitting rule +// takes the mean of these two). On exit, the array is permuted so +// that: +// +// pa[0..n_lo-2][d] <= pa[n_lo-1][d] <= pa[n_lo][d] <= pa[n_lo+1..n-1][d]. +// +// The mean of pa[n_lo-1][d] and pa[n_lo][d] is returned as the +// splitting value. +// +// All indexing is done indirectly through the index array pidx. +// +// This function uses the well known selection algorithm due to +// C.A.R. Hoare. +//---------------------------------------------------------------------- + + // swap two points in pa array +#define PASWAP(a,b) { int tmp = pidx[a]; pidx[a] = pidx[b]; pidx[b] = tmp; } + +void annMedianSplit( + ANNpointArray pa, // points to split + ANNidxArray pidx, // point indices + int n, // number of points + int d, // dimension along which to split + ANNcoord &cv, // cutting value + int n_lo) // split into n_lo and n-n_lo +{ + int l = 0; // left end of current subarray + int r = n-1; // right end of current subarray + while (l < r) { + register int i = (r+l)/2; // select middle as pivot + register int k; + + if (PA(i,d) > PA(r,d)) // make sure last > pivot + PASWAP(i,r) + PASWAP(l,i); // move pivot to first position + + ANNcoord c = PA(l,d); // pivot value + i = l; + k = r; + for(;;) { // pivot about c + while (PA(++i,d) < c) ; + while (PA(--k,d) > c) ; + if (i < k) PASWAP(i,k) else break; + } + PASWAP(l,k); // pivot winds up in location k + + if (k > n_lo) r = k-1; // recurse on proper subarray + else if (k < n_lo) l = k+1; + else break; // got the median exactly + } + if (n_lo > 0) { // search for next smaller item + ANNcoord c = PA(0,d); // candidate for max + int k = 0; // candidate's index + for (int i = 1; i < n_lo; i++) { + if (PA(i,d) > c) { + c = PA(i,d); + k = i; + } + } + PASWAP(n_lo-1, k); // max among pa[0..n_lo-1] to pa[n_lo-1] + } + // cut value is midpoint value + cv = (PA(n_lo-1,d) + PA(n_lo,d))/2.0; +} + +//---------------------------------------------------------------------- +// annPlaneSplit - split point array about a cutting plane +// Split the points in an array about a given plane along a +// given cutting dimension. On exit, br1 and br2 are set so +// that: +// +// pa[ 0 ..br1-1] < cv +// pa[br1..br2-1] == cv +// pa[br2.. n -1] > cv +// +// All indexing is done indirectly through the index array pidx. +// +//---------------------------------------------------------------------- + +void annPlaneSplit( // split points by a plane + ANNpointArray pa, // points to split + ANNidxArray pidx, // point indices + int n, // number of points + int d, // dimension along which to split + ANNcoord cv, // cutting value + int &br1, // first break (values < cv) + int &br2) // second break (values == cv) +{ + int l = 0; + int r = n-1; + for(;;) { // partition pa[0..n-1] about cv + while (l < n && PA(l,d) < cv) l++; + while (r >= 0 && PA(r,d) >= cv) r--; + if (l > r) break; + PASWAP(l,r); + l++; r--; + } + br1 = l; // now: pa[0..br1-1] < cv <= pa[br1..n-1] + r = n-1; + for(;;) { // partition pa[br1..n-1] about cv + while (l < n && PA(l,d) <= cv) l++; + while (r >= br1 && PA(r,d) > cv) r--; + if (l > r) break; + PASWAP(l,r); + l++; r--; + } + br2 = l; // now: pa[br1..br2-1] == cv < pa[br2..n-1] +} + + +//---------------------------------------------------------------------- +// annBoxSplit - split point array about a orthogonal rectangle +// Split the points in an array about a given orthogonal +// rectangle. On exit, n_in is set to the number of points +// that are inside (or on the boundary of) the rectangle. +// +// All indexing is done indirectly through the index array pidx. +// +//---------------------------------------------------------------------- + +void annBoxSplit( // split points by a box + ANNpointArray pa, // points to split + ANNidxArray pidx, // point indices + int n, // number of points + int dim, // dimension of space + ANNorthRect &box, // the box + int &n_in) // number of points inside (returned) +{ + int l = 0; + int r = n-1; + for(;;) { // partition pa[0..n-1] about box + while (l < n && box.inside(dim, PP(l))) l++; + while (r >= 0 && !box.inside(dim, PP(r))) r--; + if (l > r) break; + PASWAP(l,r); + l++; r--; + } + n_in = l; // now: pa[0..n_in-1] inside and rest outside +} + +//---------------------------------------------------------------------- +// annSplitBalance - compute balance factor for a given plane split +// Balance factor is defined as the number of points lying +// below the splitting value minus n/2 (median). Thus, a +// median split has balance 0, left of this is negative and +// right of this is positive. (The points are unchanged.) +//---------------------------------------------------------------------- + +int annSplitBalance( // determine balance factor of a split + ANNpointArray pa, // points to split + ANNidxArray pidx, // point indices + int n, // number of points + int d, // dimension along which to split + ANNcoord cv) // cutting value +{ + int n_lo = 0; + for(int i = 0; i < n; i++) { // count number less than cv + if (PA(i,d) < cv) n_lo++; + } + return n_lo - n/2; +} + +//---------------------------------------------------------------------- +// annBox2Bnds - convert bounding box to list of bounds +// Given two boxes, an inner box enclosed within a bounding +// box, this routine determines all the sides for which the +// inner box is strictly contained with the bounding box, +// and adds an appropriate entry to a list of bounds. Then +// we allocate storage for the final list of bounds, and return +// the resulting list and its size. +//---------------------------------------------------------------------- + +void annBox2Bnds( // convert inner box to bounds + const ANNorthRect &inner_box, // inner box + const ANNorthRect &bnd_box, // enclosing box + int dim, // dimension of space + int &n_bnds, // number of bounds (returned) + ANNorthHSArray &bnds) // bounds array (returned) +{ + int i; + n_bnds = 0; // count number of bounds + for (i = 0; i < dim; i++) { + if (inner_box.lo[i] > bnd_box.lo[i]) // low bound is inside + n_bnds++; + if (inner_box.hi[i] < bnd_box.hi[i]) // high bound is inside + n_bnds++; + } + + bnds = new ANNorthHalfSpace[n_bnds]; // allocate appropriate size + + int j = 0; + for (i = 0; i < dim; i++) { // fill the array + if (inner_box.lo[i] > bnd_box.lo[i]) { + bnds[j].cd = i; + bnds[j].cv = inner_box.lo[i]; + bnds[j].sd = +1; + j++; + } + if (inner_box.hi[i] < bnd_box.hi[i]) { + bnds[j].cd = i; + bnds[j].cv = inner_box.hi[i]; + bnds[j].sd = -1; + j++; + } + } +} + +//---------------------------------------------------------------------- +// annBnds2Box - convert list of bounds to bounding box +// Given an enclosing box and a list of bounds, this routine +// computes the corresponding inner box. It is assumed that +// the box points have been allocated already. +//---------------------------------------------------------------------- + +void annBnds2Box( + const ANNorthRect &bnd_box, // enclosing box + int dim, // dimension of space + int n_bnds, // number of bounds + ANNorthHSArray bnds, // bounds array + ANNorthRect &inner_box) // inner box (returned) +{ + annAssignRect(dim, inner_box, bnd_box); // copy bounding box to inner + + for (int i = 0; i < n_bnds; i++) { + bnds[i].project(inner_box.lo); // project each endpoint + bnds[i].project(inner_box.hi); + } +} +} diff --git a/geom_bottleneck/bottleneck/src/basic_defs.cpp b/geom_bottleneck/bottleneck/src/basic_defs.cpp new file mode 100644 index 0000000..e09b119 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/basic_defs.cpp @@ -0,0 +1,230 @@ +/* + Copyrigth 2015, D. Morozov, M. Kerber, A. Nigmetov + + This file is part of GeomBottleneck. + + GeomBottleneck is free software: you can redistribute it and/or modify + it under the terms of the Lesser GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + GeomBottleneck is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + Lesser GNU General Public License for more details. + + You should have received a copy of the Lesser GNU General Public License + along with GeomBottleneck. If not, see <http://www.gnu.org/licenses/>. + +*/ + +#include <algorithm> +#include <cfloat> +#include "basic_defs_bt.h" + +namespace geom_bt { + +// Point + +bool Point::operator==(const Point& other) const +{ + return ((this->x == other.x) and (this->y == other.y)); +} + +bool Point::operator!=(const Point& other) const +{ + return !(*this == other); +} + +std::ostream& operator<<(std::ostream& output, const Point p) +{ + output << "(" << p.x << ", " << p.y << ")"; + return output; +} + +std::ostream& operator<<(std::ostream& output, const PointSet& ps) +{ + output << "{ "; + for(auto& p : ps) { + output << p << ", "; + } + output << "\b\b }"; + return output; +} + +double sqrDist(const Point& a, const Point& b) +{ + return (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y); +} + +double dist(const Point& a, const Point& b) +{ + return sqrt(sqrDist(a, b)); +} + +// DiagramPoint + +// compute l-inf distance between two diagram points +double distLInf(const DiagramPoint& a, const DiagramPoint& b) +{ + if ( DiagramPoint::DIAG == a.type && + DiagramPoint::DIAG == b.type ) { + // distance between points on the diagonal is 0 + return 0.0; + } + // otherwise distance is a usual l-inf distance + return std::max(fabs(a.getRealX() - b.getRealX()), fabs(a.getRealY() - b.getRealY())); +} + +bool DiagramPoint::operator==(const DiagramPoint& other) const +{ + assert(this->id >= MinValidId); + assert(other.id >= MinValidId); + bool areEqual{ this->id == other.id }; + assert(!areEqual or ((this->x == other.x) and (this->y == other.y) and (this->type == other.type))); + return areEqual; +} + +bool DiagramPoint::operator!=(const DiagramPoint& other) const +{ + return !(*this == other); +} + +std::ostream& operator<<(std::ostream& output, const DiagramPoint p) +{ + if ( p.type == DiagramPoint::DIAG ) { + output << "(" << p.x << ", " << p.y << ", " << 0.5 * (p.x + p.y) << ", " << p.id << " DIAG )"; + } else { + output << "(" << p.x << ", " << p.y << ", " << p.id << " NORMAL)"; + } + return output; +} + +std::ostream& operator<<(std::ostream& output, const DiagramPointSet& ps) +{ + output << "{ "; + for(auto pit = ps.cbegin(); pit != ps.cend(); ++pit) { + output << *pit << ", "; + } + output << "\b\b }"; + return output; +} + +DiagramPoint::DiagramPoint(double xx, double yy, Type ttype, IdType uid) : + x(xx), + y(yy), + type(ttype), + id(uid) +{ + //if ( xx < 0 ) + //throw "Negative x coordinate"; + //if ( yy < 0) + //throw "Negative y coordinate"; + //if ( yy < xx ) + //throw "Point is below the diagonal"; + if ( yy == xx and ttype != DiagramPoint::DIAG) + throw "Point on the main diagonal must have DIAG type"; + +} + +void DiagramPointSet::insert(const DiagramPoint p) +{ + points.insert(p); + if (p.id > maxId) { + maxId = p.id + 1; + } +} + +// erase should be called only for the element of the set +void DiagramPointSet::erase(const DiagramPoint& p, bool doCheck) +{ + auto it = points.find(p); + if (it != points.end()) { + points.erase(it); + } else { + assert(!doCheck); + } +} + +void DiagramPointSet::reserve(const size_t newSize) +{ + points.reserve(newSize); +} + + +void DiagramPointSet::erase(const std::unordered_set<DiagramPoint, DiagramPointHash>::const_iterator it) +{ + points.erase(it); +} + +void DiagramPointSet::clear() +{ + points.clear(); +} + +size_t DiagramPointSet::size() const +{ + return points.size(); +} + +bool DiagramPointSet::empty() const +{ + return points.empty(); +} + +bool DiagramPointSet::hasElement(const DiagramPoint& p) const +{ + return points.find(p) != points.end(); +} + + +void DiagramPointSet::removeDiagonalPoints() +{ + if (isLinked) { + auto ptIter = points.begin(); + while(ptIter != points.end()) { + if (ptIter->isDiagonal()) { + ptIter = points.erase(ptIter); + } else { + ptIter++; + } + } + isLinked = false; + } +} + + +// preprocess diagrams A and B by adding projections onto diagonal of points of +// A to B and vice versa. NB: ids of points will be changed! +void addProjections(DiagramPointSet& A, DiagramPointSet& B) +{ + + IdType uniqueId {MinValidId + 1}; + DiagramPointSet newA, newB; + + // copy normal points from A to newA + // add projections to newB + for(auto& pA : A) { + if (pA.isNormal()) { + DiagramPoint dpA {pA.getRealX(), pA.getRealY(), DiagramPoint::NORMAL, uniqueId++}; + DiagramPoint dpB {0.5*(pA.getRealX() +pA.getRealY()), 0.5 *(pA.getRealX() +pA.getRealY()), DiagramPoint::DIAG, uniqueId++}; + newA.insert(dpA); + newB.insert(dpB); + } + } + + for(auto& pB : B) { + if (pB.isNormal()) { + DiagramPoint dpB {pB.getRealX(), pB.getRealY(), DiagramPoint::NORMAL, uniqueId++}; + DiagramPoint dpA {0.5*(pB.getRealX() +pB.getRealY()), 0.5 *(pB.getRealX() +pB.getRealY()), DiagramPoint::DIAG, uniqueId++}; + newB.insert(dpB); + newA.insert(dpA); + } + } + + A = newA; + B = newB; + A.isLinked = true; + B.isLinked = true; +} +} diff --git a/geom_bottleneck/bottleneck/src/bottleneck.cpp b/geom_bottleneck/bottleneck/src/bottleneck.cpp new file mode 100644 index 0000000..a5009c5 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/bottleneck.cpp @@ -0,0 +1,555 @@ +/* + Copyrigth 2015, D. Morozov, M. Kerber, A. Nigmetov + + This file is part of GeomBottleneck. + + GeomBottleneck is free software: you can redistribute it and/or modify + it under the terms of the Lesser GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + GeomBottleneck is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + Lesser GNU General Public License for more details. + + You should have received a copy of the Lesser GNU General Public License + along with GeomBottleneck. If not, see <http://www.gnu.org/licenses/>. + +*/ + + +#include <iomanip> +#include <sstream> +#include <string> +#include <cctype> + +#include "bottleneck.h" +//#include "test_dist_calc.h" + +namespace geom_bt { + +// return the interval (distMin, distMax) such that: +// a) actual bottleneck distance between A and B is contained in the interval +// b) if the interval is not (0,0), then (distMax - distMin) / distMin < epsilon +std::pair<double, double> bottleneckDistApproxInterval(DiagramPointSet& A, DiagramPointSet& B, const double epsilon) +{ + // empty diagrams are not considered as error + if (A.empty() and B.empty()) + return std::make_pair(0.0, 0.0); + + // link diagrams A and B by adding projections + addProjections(A, B); + + // TODO: think about that! + // we need one threshold for checking if the distance is 0, + // another one for the oracle! + constexpr double epsThreshold { 1.0e-10 }; + std::pair<double, double> result { 0.0, 0.0 }; + bool useRangeSearch { true }; + // construct an oracle + BoundMatchOracle oracle(A, B, epsThreshold, useRangeSearch); + // check for distance = 0 + if (oracle.isMatchLess(2*epsThreshold)) { + return result; + } + // get a 3-approximation of maximal distance between A and B + // as a starting value for probe distance + double distProbe { getFurthestDistance3Approx(A, B) }; + // aliases for result components + double& distMin {result.first}; + double& distMax {result.second}; + + if ( oracle.isMatchLess(distProbe) ) { + // distProbe is an upper bound, + // find lower bound with binary search + do { + distMax = distProbe; + distProbe /= 2.0; + } while (oracle.isMatchLess(distProbe)); + distMin = distProbe; + } else { + // distProbe is a lower bound, + // find upper bound with exponential search + do { + distMin = distProbe; + distProbe *= 2.0; + } while (!oracle.isMatchLess(distProbe)); + distMax = distProbe; + } + // bounds are found, perform binary search + //std::cout << "Bounds found, distMin = " << distMin << ", distMax = " << distMax << ", ratio = " << ( distMax - distMin ) / distMin << std::endl ; + distProbe = ( distMin + distMax ) / 2.0; + while ( ( distMax - distMin ) / distMin >= epsilon ) { + if (oracle.isMatchLess(distProbe)) { + distMax = distProbe; + } else { + distMin = distProbe; + } + distProbe = ( distMin + distMax ) / 2.0; + } + return result; +} + +// get approximate distance, +// see bottleneckDistApproxInterval +double bottleneckDistApprox(DiagramPointSet& A, DiagramPointSet& B, const double epsilon) +{ + auto interval = bottleneckDistApproxInterval(A, B, epsilon); + return interval.second; +} + + +double bottleneckDistExactFromSortedPwDist(DiagramPointSet&A, DiagramPointSet& B, std::vector<double>& pairwiseDist) +{ + //for(size_t k = 0; k < pairwiseDist.size(); ++k) { + //std::cout << "pairwiseDist[" << k << "] = " << std::setprecision(15) << pairwiseDist[k] << std::endl; + //} + // trivial case: we have only one candidate + if (pairwiseDist.size() == 1) + return pairwiseDist[0]; + + bool useRangeSearch = true; + double distEpsilon = std::numeric_limits<double>::max(); + for(size_t k = 0; k < pairwiseDist.size() - 2; ++k) { + auto diff = pairwiseDist[k+1]- pairwiseDist[k]; + if ( diff > 1.0e-14 and diff < distEpsilon ) { + distEpsilon = diff; + } + } + distEpsilon /= 3.0; + + BoundMatchOracle oracle(A, B, distEpsilon, useRangeSearch); + // binary search + size_t iterNum {0}; + size_t idxMin {0}, idxMax {pairwiseDist.size() - 1}; + size_t idxMid; + while(idxMax > idxMin) { + idxMid = static_cast<size_t>(floor(idxMin + idxMax) / 2.0); + //std::cout << "while begin: min = " << idxMin << ", idxMax = " << idxMax << ", idxMid = " << idxMid << ", testing d = " << std::setprecision(15) << pairwiseDist[idxMid] << std::endl; + iterNum++; + // not A[imid] < dist <=> A[imid] >= dist <=> A[imid[ >= dist + eps + if (oracle.isMatchLess(pairwiseDist[idxMid] + distEpsilon / 2.0)) { + //std::cout << "isMatchLess = true" << std::endl; + idxMax = idxMid; + } else { + //std::cout << "isMatchLess = false " << std::endl; + idxMin = idxMid + 1; + } + //std::cout << "while end: idxMin = " << idxMin << ", idxMax = " << idxMax << ", idxMid = " << idxMid << std::endl; + } + idxMid = static_cast<size_t>(floor(idxMin + idxMax) / 2.0); + return pairwiseDist[idxMid]; +} + + +double bottleneckDistExact(DiagramPointSet& A, DiagramPointSet& B) +{ + constexpr double epsilon = 0.001; + auto interval = bottleneckDistApproxInterval(A, B, epsilon); + const double delta = 0.5 * (interval.second - interval.first); + const double approxDist = 0.5 * ( interval.first + interval.second); + const double minDist = interval.first; + const double maxDist = interval.second; + //std::cerr << std::setprecision(15) << "minDist = " << minDist << ", maxDist = " << maxDist << std::endl; + if ( delta == 0 ) { + return interval.first; + } + // copy points from A to a vector + // todo: get rid of this? + std::vector<DiagramPoint> pointsA; + pointsA.reserve(A.size()); + for(const auto& ptA : A) { + pointsA.push_back(ptA); + } + + //std::vector<double> killDist; + //for(auto ptA : A) { + //for(auto ptB : B) { + //if ( distLInf(ptA, ptB) > minDist and distLInf(ptA, ptB) < maxDist) { + //killDist.push_back(distLInf(ptA, ptB)); + //std::cout << ptA << ", " << ptB << std::endl; + //} + //} + //} + //std::sort(killDist.begin(), killDist.end()); + //for(auto d : killDist) { + //std::cout << d << std::endl; + //} + //std::cout << "*************" << std::endl; + + // in this vector we store the distances between the points + // that are candidates to realize + std::vector<double> pairwiseDist; + { + // vector to store centers of vertical stripes + // two for each point in A and the id of the corresponding point + std::vector<std::pair<double, DiagramPoint>> xCentersVec; + xCentersVec.reserve(2 * pointsA.size()); + for(auto ptA : pointsA) { + xCentersVec.push_back(std::make_pair(ptA.getRealX() - approxDist, ptA)); + xCentersVec.push_back(std::make_pair(ptA.getRealX() + approxDist, ptA)); + } + // lambda to compare pairs <coordinate, id> w.r.t coordinate + auto compLambda = [](std::pair<double, DiagramPoint> a, std::pair<double, DiagramPoint> b) + { return a.first < b.first; }; + + std::sort(xCentersVec.begin(), xCentersVec.end(), compLambda); + //std::cout << "xCentersVec.size = " << xCentersVec.size() << std::endl; + //for(auto p = xCentersVec.begin(); p!= xCentersVec.end(); ++p) { + //if (p->second.id == 200) { + //std::cout << "index of 200: " << p - xCentersVec.begin() << std::endl; + //} + //} + //std::vector<DiagramPoint> + // todo: sort points in B, reduce search range in lower and upper bounds + for(auto ptB : B) { + // iterator to the first stripe such that ptB lies to the left + // from its right boundary (x_B <= x_j + \delta iff x_j >= x_B - \delta + auto itStart = std::lower_bound(xCentersVec.begin(), + xCentersVec.end(), + std::make_pair(ptB.getRealX() - delta, ptB), + compLambda); + //if (ptB.id == 236) { + //std::cout << itStart - xCentersVec.begin() << std::endl; + //} + + for(auto iterA = itStart; iterA < xCentersVec.end(); ++iterA) { + //if (ptB.id == 236) { + //std::cout << "consider " << iterA->second << std::endl; + //} + if ( ptB.getRealX() < iterA->first - delta) { + // from that moment x_B >= x_j - delta + // is violated: x_B no longer lies to right from the left + // boundary of current stripe + //if (ptB.id == 236) { + //std::cout << "break" << std::endl; + //} + break; + } + // we're here => ptB lies in vertical stripe, + // check if distance fits into the interval we've found + double pwDist = distLInf(iterA->second, ptB); + //if (ptB.id == 236) { + //std::cout << pwDist << std::endl; + //} + //std::cout << 1000*minDist << " <= " << 1000*pwDist << " <= " << 1000*maxDist << std::endl; + if (pwDist >= minDist and pwDist <= maxDist) { + pairwiseDist.push_back(pwDist); + } + } + } + } + + { + // for y + // vector to store centers of vertical stripes + // two for each point in A and the id of the corresponding point + std::vector<std::pair<double, DiagramPoint>> yCentersVec; + yCentersVec.reserve(2 * pointsA.size()); + for(auto ptA : pointsA) { + yCentersVec.push_back(std::make_pair(ptA.getRealY() - approxDist, ptA)); + yCentersVec.push_back(std::make_pair(ptA.getRealY() + approxDist, ptA)); + } + // lambda to compare pairs <coordinate, id> w.r.t coordinate + auto compLambda = [](std::pair<double, DiagramPoint> a, std::pair<double, DiagramPoint> b) + { return a.first < b.first; }; + + std::sort(yCentersVec.begin(), yCentersVec.end(), compLambda); + + // std::cout << "Sorted vector of y-centers:" << std::endl; + //for(auto coordPtPair : yCentersVec) { + //std::cout << coordPtPair.first << ", id = " << coordPtPair.second.id << std::endl; + //} + /*std::cout << "End of sorted vector of y-centers:" << std::endl;*/ + + //std::vector<DiagramPoint> + // todo: sort points in B, reduce search range in lower and upper bounds + for(auto ptB : B) { + auto itStart = std::lower_bound(yCentersVec.begin(), + yCentersVec.end(), + std::make_pair(ptB.getRealY() - delta, ptB), + compLambda); + + //if (ptB.id == 316) { + //std::cout << itStart - yCentersVec.begin() << " " << distLInf(itStart->second, ptB) << std::endl; + //std::cout << "maxDist = " << maxDist << std::endl; + //std::cout << "minDist = " << minDist << std::endl; + //double pwDistDebug = distLInf(itStart->second, ptB); + //std::cout << ( pwDistDebug >= minDist and pwDistDebug <= maxDist) << std::endl; + //} + + for(auto iterA = itStart; iterA < yCentersVec.end(); ++iterA) { + if ( ptB.getRealY() < iterA->first - delta) { + break; + } + double pwDist = distLInf(iterA->second, ptB); + //std::cout << 1000*minDist << " <= " << 1000*pwDist << " <= " << 1000*maxDist << std::endl; + if (pwDist >= minDist and pwDist <= maxDist) { + //if (ptB.id == 316) { + //std::cout << "adding " << pwDist << std::endl; + //} + pairwiseDist.push_back(pwDist); + } + } + } + } + + //std::cerr << "pairwiseDist.size = " << pairwiseDist.size() << " out of " << A.size() * A.size() << std::endl; + std::sort(pairwiseDist.begin(), pairwiseDist.end()); + //for(auto ddd : pairwiseDist) { + //std::cerr << std::setprecision(15) << ddd << std::endl; + //} + + return bottleneckDistExactFromSortedPwDist(A, B, pairwiseDist); +} + +double bottleneckDistSlow(DiagramPointSet& A, DiagramPointSet& B) +{ + // use range search when building the layer graph + bool useRangeSearch { true }; + // find maximum of min. distances for each point, + // use this value as lower bound for bottleneck distance + bool useHeurMinIdx { true }; + + // find matching in a greedy manner to + // get an upper bound for a bottleneck distance + bool useHeurGreedyMatching { false }; + + // use successive multiplication of idxMin with 2 to get idxMax + bool goUpToFindIdxMax { false }; + // + goUpToFindIdxMax = goUpToFindIdxMax and !useHeurGreedyMatching; + + if (!useHeurGreedyMatching) { + long int N = 3 * (A.size() / 2 ) * (B.size() / 2); + std::vector<double> pairwiseDist; + pairwiseDist.reserve(N); + double maxMinDist {0.0}; + for(auto& p_A : A) { + double minDist { std::numeric_limits<double>::max() }; + for(auto& p_B : B) { + if (p_A.type != DiagramPoint::DIAG or p_B.type != DiagramPoint::DIAG) { + double d = distLInf(p_A, p_B); + pairwiseDist.push_back(d); + if (useHeurMinIdx and p_A.type != DiagramPoint::DIAG) { + if (d < minDist) + minDist = d; + } + } + } + if (useHeurMinIdx and DiagramPoint::DIAG != p_A.type and minDist > maxMinDist) { + maxMinDist = minDist; + } + } + std::sort(pairwiseDist.begin(), pairwiseDist.end()); + + double distEpsilon = std::numeric_limits<double>::max(); + for(size_t k = 0; k < pairwiseDist.size() - 2; ++k) { + auto diff = pairwiseDist[k+1]- pairwiseDist[k]; + if ( diff > 1.0e-10 and diff < distEpsilon ) { + distEpsilon = diff; + } + } + distEpsilon /= 3.0; + + BoundMatchOracle oracle(A, B, distEpsilon, useRangeSearch); + // binary search + size_t iterNum {0}; + size_t idxMin {0}, idxMax {pairwiseDist.size() - 1}; + if (useHeurMinIdx) { + auto maxMinIter = std::equal_range(pairwiseDist.begin(), pairwiseDist.end(), maxMinDist); + assert(maxMinIter.first != pairwiseDist.end()); + idxMin = maxMinIter.first - pairwiseDist.begin(); + //std::cout << "maxMinDist = " << maxMinDist << ", idxMin = " << idxMin << ", d = " << pairwiseDist[idxMin] << std::endl; + } + + if (goUpToFindIdxMax) { + if ( pairwiseDist.size() == 1) { + return pairwiseDist[0]; + } + + idxMax = std::max<size_t>(idxMin, 1); + while (!oracle.isMatchLess(pairwiseDist[idxMax])) { + //std::cout << "entered while" << std::endl; + idxMin = idxMax; + if (2*idxMax > pairwiseDist.size() -1) { + idxMax = pairwiseDist.size() - 1; + break; + } else { + idxMax *= 2; + } + } + //std::cout << "size = " << pairwiseDist.size() << ", idxMax = " << idxMax << ", pw[max] = " << pairwiseDist[idxMax] << std::endl; + } + + size_t idxMid { (idxMin + idxMax) / 2 }; + while(idxMax > idxMin) { + iterNum++; + if (oracle.isMatchLess(pairwiseDist[idxMid])) { + idxMax = idxMid; + } else { + if (idxMax - idxMin == 1) + idxMin++; + else + idxMin = idxMid; + } + idxMid = (idxMin + idxMax) / 2; + } + return pairwiseDist[idxMid]; + } else { + // with greeedy matching + long int N = A.size() * B.size(); + std::vector<DistVerticesPair> pairwiseDist; + pairwiseDist.reserve(N); + double maxMinDist {0.0}; + size_t idxA{0}, idxB{0}; + for(auto p_A : A) { + double minDist { std::numeric_limits<double>::max() }; + idxB = 0; + for(auto p_B : B) { + double d = distLInf(p_A, p_B); + pairwiseDist.push_back( std::make_pair(d, std::make_pair(idxA, idxB) ) ); + if (useHeurMinIdx and p_A.type != DiagramPoint::DIAG) { + if (d < minDist) + minDist = d; + } + idxB++; + } + if (useHeurMinIdx and DiagramPoint::DIAG != p_A.type and minDist > maxMinDist) { + maxMinDist = minDist; + } + idxA++; + } + + auto compLambda = [](DistVerticesPair a, DistVerticesPair b) + { return a.first < b.first;}; + + std::sort(pairwiseDist.begin(), + pairwiseDist.end(), + compLambda); + + double distEpsilon = std::numeric_limits<double>::max(); + for(size_t k = 0; k < pairwiseDist.size() - 2; ++k) { + auto diff = pairwiseDist[k+1].first - pairwiseDist[k].first; + if ( diff > 1.0e-10 and diff < distEpsilon ) { + distEpsilon = diff; + } + } + distEpsilon /= 3.0; + + BoundMatchOracle oracle(A, B, distEpsilon, useRangeSearch); + + // construct greedy matching + size_t numVert { A.size() }; + size_t numMatched { 0 }; + std::unordered_set<size_t> aTobMatched, bToaMatched; + aTobMatched.reserve(numVert); + bToaMatched.reserve(numVert); + size_t distVecIdx {0}; + while( numMatched < numVert) { + auto vertPair = pairwiseDist[distVecIdx++].second; + //std::cout << "distVecIdx = " << distVecIdx << ", matched: " << numMatched << " out of " << numVert << std::endl; + //std::cout << "vertex A idx = " << vertPair.first << ", B idx: " << vertPair.second << " out of " << numVert << std::endl; + if ( aTobMatched.count(vertPair.first) == 0 and + bToaMatched.count(vertPair.second) == 0 ) { + aTobMatched.insert(vertPair.first); + bToaMatched.insert(vertPair.second); + numMatched++; + } + } + size_t idxMax = distVecIdx-1; + //std::cout << "idxMax = " << idxMax << ", size = " << pairwiseDist.size() << std::endl; + // binary search + size_t iterNum {0}; + size_t idxMin {0}; + if (useHeurMinIdx) { + auto maxMinIter = std::equal_range(pairwiseDist.begin(), + pairwiseDist.end(), + std::make_pair(maxMinDist, std::make_pair(0,0)), + compLambda); + assert(maxMinIter.first != pairwiseDist.end()); + idxMin = maxMinIter.first - pairwiseDist.begin(); + //std::cout << "maxMinDist = " << maxMinDist << ", idxMin = " << idxMin << ", d = " << pairwiseDist[idxMin].first << std::endl; + } + size_t idxMid { (idxMin + idxMax) / 2 }; + while(idxMax > idxMin) { + iterNum++; + if (oracle.isMatchLess(pairwiseDist[idxMid].first)) { + idxMax = idxMid; + } else { + if (idxMax - idxMin == 1) + idxMin++; + else + idxMin = idxMid; + } + idxMid = (idxMin + idxMax) / 2; + } + return pairwiseDist[idxMid].first; + } + // stats + /* + // count number of edges + // pairwiseDist is sorted, add edges of the same length + int edgeNumber {idxMid}; + while(pairwiseDist[edgeNumber + 1] == pairwiseDist[edgeNumber]) + edgeNumber++; + // add edges between diagonal points + edgeNumber += N / 3; + // output stats + std::cout << idxMid << "\t" << N; + std::cout << "\t" << iterNum; + std::cout << "\t" << A.size() + B.size(); + std::cout << "\t" << edgeNumber << "\t"; + std::cout << (double)(edgeNumber) / (double)(A.size() + B.size()) << std::endl; + */ +} + +bool readDiagramPointSet(const std::string& fname, std::vector<std::pair<double, double>>& result) +{ + return readDiagramPointSet(fname.c_str(), result); +} + +bool readDiagramPointSet(const char* fname, std::vector<std::pair<double, double>>& result) +{ + size_t lineNumber { 0 }; + result.clear(); + std::ifstream f(fname); + if (!f.good()) { + std::cerr << "Cannot open file " << fname << std::endl; + return false; + } + std::string line; + while(std::getline(f, line)) { + lineNumber++; + // process comments: remove everything after hash + auto hashPos = line.find_first_of("#", 0); + if( std::string::npos != hashPos) { + line = std::string(line.begin(), line.begin() + hashPos); + } + if (line.empty()) { + continue; + } + // trim whitespaces + auto whiteSpaceFront = std::find_if_not(line.begin(),line.end(),isspace); + auto whiteSpaceBack = std::find_if_not(line.rbegin(),line.rend(),isspace).base(); + if (whiteSpaceBack <= whiteSpaceFront) { + // line consists of spaces only - move to the next line + continue; + } + line = std::string(whiteSpaceFront,whiteSpaceBack); + double x, y; + std::istringstream iss(line); + if (not(iss >> x >> y)) { + std::cerr << "Error in file " << fname << ", line number " << lineNumber << ": cannot parse \"" << line << "\"" << std::endl; + return false; + } + result.push_back(std::make_pair(x,y)); + } + f.close(); + return true; +} + + + +} diff --git a/geom_bottleneck/bottleneck/src/bound_match.cpp b/geom_bottleneck/bottleneck/src/bound_match.cpp new file mode 100644 index 0000000..06d3b67 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/bound_match.cpp @@ -0,0 +1,529 @@ +/* +Copyrigth 2015, D. Morozov, M. Kerber, A. Nigmetov + +This file is part of GeomBottleneck. + +GeomBottleneck is free software: you can redistribute it and/or modify +it under the terms of the Lesser GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +GeomBottleneck is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +Lesser GNU General Public License for more details. + +You should have received a copy of the Lesser GNU General Public License +along with GeomBottleneck. If not, see <http://www.gnu.org/licenses/>. + +*/ + +#include <iostream> +#include <assert.h> +#include "bound_match.h" + +namespace geom_bt { +/*static void printDebug(//bool isDebug, std::string s)*/ +//{ +//#ifdef DEBUG_BOUND_MATCH + //if (isDebug) { + //std::cout << s << std::endl; + //} +//#endif +//} + +//static void printDebug(//bool isDebug, std::string s, const Matching& m) +//{ +//#ifdef DEBUG_BOUND_MATCH + //if (isDebug) { + //std::cout << s << std::endl; + //std::cout << m << std::endl; + //} +//#endif +//} + +//static void printDebug(//bool isDebug, std::string s, const DiagramPoint& p) +//{ +//#ifdef DEBUG_BOUND_MATCH + //if (isDebug) { + //std::cout << s << p << std::endl; + //} +//#endif +//} + +//static void printDebug(//bool isDebug, std::string s, const double r) +//{ +//#ifdef DEBUG_BOUND_MATCH + //if (isDebug) { + //std::cout << s << r << std::endl; + //} +//#endif +//} + +//static void printDebug(//bool isDebug, std::string s, const Path p) +//{ +//#ifdef DEBUG_BOUND_MATCH + //if (isDebug) { + //std::cout << s; + //for(auto pt : p) { + //std::cout << pt << "; "; + //} + //std::cout << std::endl; + //} +//#endif +//} + +//static void printDebug(//bool isDebug, std::string s, const DiagramPointSet& dpSet) +//{ +//#ifdef DEBUG_BOUND_MATCH + //if (isDebug) { + //std::cout << s << dpSet << std::endl; + //} +//#endif +/*}*/ + +std::ostream& operator<<(std::ostream& output, const Matching& m) +{ + output << "Matching: " << m.AToB.size() << " pairs ("; + if (!m.isPerfect()) { + output << "not"; + } + output << " perfect)" << std::endl; + for(auto atob : m.AToB) { + output << atob.first << " <-> " << atob.second << " distance: " << distLInf(atob.first, atob.second) << std::endl; + } + return output; +} + +void Matching::sanityCheck() const +{ +#ifdef DEBUG_MATCHING + assert( AToB.size() == BToA.size() ); + for(auto aToBPair : AToB) { + auto bToAPair = BToA.find(aToBPair.second); + assert(bToAPair != BToA.end()); + if (aToBPair.first != bToAPair->second) { + std::cerr << "failed assertion, in aToB " << aToBPair.first; + std::cerr << ", in bToA " << bToAPair->second << std::endl; + } + assert( aToBPair.first == bToAPair->second); + } +#endif +} + +bool Matching::isPerfect() const +{ + //sanityCheck(); + return AToB.size() == A.size(); +} + +void Matching::matchVertices(const DiagramPoint& pA, const DiagramPoint& pB) +{ + assert(A.hasElement(pA)); + assert(B.hasElement(pB)); + AToB.erase(pA); + AToB.insert( {{ pA, pB }} ); + BToA.erase(pB); + BToA.insert( {{ pB, pA }} ); +} + +bool Matching::getMatchedVertex(const DiagramPoint& p, DiagramPoint& result) const +{ + sanityCheck(); + auto inA = AToB.find(p); + if (inA != AToB.end()) { + result = (*inA).second; + return true; + } else { + auto inB = BToA.find(p); + if (inB != BToA.end()) { + result = (*inB).second; + return true; + } + } + return false; +} + + +void Matching::checkAugPath(const Path& augPath) const +{ + assert(augPath.size() % 2 == 0); + for(size_t idx = 0; idx < augPath.size(); ++idx) { + bool mustBeExposed { idx == 0 or idx == augPath.size() - 1 }; + if (isExposed(augPath[idx]) != mustBeExposed) { + std::cerr << "mustBeExposed = " << mustBeExposed << ", idx = " << idx << ", point " << augPath[idx] << std::endl; + } + assert( isExposed(augPath[idx]) == mustBeExposed ); + DiagramPoint matchedVertex {0.0, 0.0, DiagramPoint::DIAG, 1}; + if ( idx % 2 == 0 ) { + assert( A.hasElement(augPath[idx])); + if (!mustBeExposed) { + getMatchedVertex(augPath[idx], matchedVertex); + assert(matchedVertex == augPath[idx - 1]); + } + } else { + assert( B.hasElement(augPath[idx])); + if (!mustBeExposed) { + getMatchedVertex(augPath[idx], matchedVertex); + assert(matchedVertex == augPath[idx + 1]); + } + } + } +} + +// use augmenting path to increase +// the size of the matching +void Matching::increase(const Path& augPath) +{ + //bool isDebug {false}; + sanityCheck(); + // check that augPath is an augmenting path + checkAugPath(augPath); + for(size_t idx = 0; idx < augPath.size() - 1; idx += 2) { + matchVertices( augPath[idx], augPath[idx + 1]); + } + //printDebug(isDebug, "", *this); + sanityCheck(); +} + +DiagramPointSet Matching::getExposedVertices(bool forA) const +{ + sanityCheck(); + DiagramPointSet result; + const DiagramPointSet* setToSearch { forA ? &A : &B }; + const std::unordered_map<DiagramPoint, DiagramPoint, DiagramPointHash>* mapToSearch { forA ? &AToB : &BToA }; + for(auto it = setToSearch->cbegin(); it != setToSearch->cend(); ++it) { + if (mapToSearch->find((*it)) == mapToSearch->cend()) { + result.insert((*it)); + } + } + return result; +} + +void Matching::getAllAdjacentVertices(const DiagramPointSet& setIn, + DiagramPointSet& setOut, + bool forA) const +{ + sanityCheck(); + //bool isDebug {false}; + setOut.clear(); + const std::unordered_map<DiagramPoint, DiagramPoint, DiagramPointHash>* m; + m = ( forA ) ? &BToA : &AToB; + for(auto pit = setIn.cbegin(); pit != setIn.cend(); ++pit) { + auto findRes = m->find(*pit); + if (findRes != m->cend()) { + setOut.insert((*findRes).second); + } + } + //printDebug(isDebug, "got all adjacent vertices for ", setIn); + //printDebug(isDebug, "the result is: ", setOut); + sanityCheck(); +} + +bool Matching::isExposed(const DiagramPoint& p) const +{ + return ( AToB.find(p) == AToB.end() ) && ( BToA.find(p) == BToA.end() ); +} + + +BoundMatchOracle::BoundMatchOracle(DiagramPointSet psA, DiagramPointSet psB, + double dEps, bool useRS) : + A(psA), B(psB), M(A, B), distEpsilon(dEps), useRangeSearch(useRS), prevQueryValue(0.0) +{ + neighbOracle = new NeighbOracle(psB, 0, distEpsilon); +} + +bool BoundMatchOracle::isMatchLess(double r) +{ + return buildMatchingForThreshold(r); +} + + +void BoundMatchOracle::removeFromLayer(const DiagramPoint& p, const int layerIdx) { + //bool isDebug {false}; + //printDebug(isDebug, "entered removeFromLayer, layerIdx == " + std::to_string(layerIdx) + ", p = ", p); + layerGraph[layerIdx].erase(p); + if (layerOracles[layerIdx]) { + layerOracles[layerIdx]->deletePoint(p); + } +} + +// return true, if there exists an augmenting path from startVertex +// in this case the path is returned in result. +// startVertex must be an exposed vertex from L_1 (layer[0]) +bool BoundMatchOracle::buildAugmentingPath(const DiagramPoint startVertex, Path& result) +{ + //bool isDebug {false}; + //printDebug(isDebug, "Entered buildAugmentingPath, startVertex: ", startVertex); + DiagramPoint prevVertexA = startVertex; + result.clear(); + result.push_back(startVertex); + size_t evenLayerIdx {1}; + while ( evenLayerIdx < layerGraph.size() ) { + //for(size_t evenLayerIdx = 1; evenLayerIdx < layerGraph.size(); evenLayerIdx += 2) { + DiagramPoint nextVertexB{0.0, 0.0, DiagramPoint::DIAG, 1}; // next vertex from even layer + bool neighbFound = layerOracles[evenLayerIdx]->getNeighbour(prevVertexA, nextVertexB); + //printDebug(isDebug, "Searched neighbours for ", prevVertexA); + //printDebug(isDebug, "; the result is ", nextVertexB); + if (neighbFound) { + result.push_back(nextVertexB); + if ( layerGraph.size() == evenLayerIdx + 1) { + //printDebug(isDebug, "Last layer reached, stopping; the path: ", result); + break; + } else { + // nextVertexB must be matched with some vertex from the next odd + // layer + DiagramPoint nextVertexA {0.0, 0.0, DiagramPoint::DIAG, 1}; + if (!M.getMatchedVertex(nextVertexB, nextVertexA)) { + std::cerr << "Vertices in even layers must be matched! Unmatched: "; + std::cerr << nextVertexB << std::endl; + std::cerr << evenLayerIdx << "; " << layerGraph.size() << std::endl; + throw "Unmatched vertex in even layer"; + } else { + assert( ! (nextVertexA.getRealX() == 0 and nextVertexA.getRealY() == 0) ); + result.push_back(nextVertexA); + //printDebug(isDebug, "Matched vertex from the even layer added to the path, result: ", result); + prevVertexA = nextVertexA; + evenLayerIdx += 2; + continue; + } + } + } else { + // prevVertexA has no neighbours in the next layer, + // backtrack + if (evenLayerIdx == 1) { + // startVertex is not connected to any vertices + // in the next layer, augm. path doesn't exist + //printDebug(isDebug, "startVertex is not connected to any vertices in the next layer, augm. path doesn't exist"); + removeFromLayer(startVertex, 0); + return false; + } else { + assert(evenLayerIdx >= 3); + assert(result.size() % 2 == 1); + result.pop_back(); + DiagramPoint prevVertexB = result.back(); + result.pop_back(); + //printDebug(isDebug, "removing 2 previous vertices from layers, evenLayerIdx == ", evenLayerIdx); + removeFromLayer(prevVertexA, evenLayerIdx-1); + removeFromLayer(prevVertexB, evenLayerIdx-2); + // we should proceed from the previous odd layer + //printDebug(isDebug, "Here! res.size == ", result.size()); + assert(result.size() >= 1); + prevVertexA = result.back(); + evenLayerIdx -= 2; + continue; + } + } + } // finished iterating over all layers + // remove all vertices in the augmenting paths + // the corresponding layers + for(size_t layerIdx = 0; layerIdx < result.size(); ++layerIdx) { + removeFromLayer(result[layerIdx], layerIdx); + } + return true; +} + + +// remove all edges whose length is > newThreshold +void Matching::trimMatching(const double newThreshold) +{ + //bool isDebug { false }; + sanityCheck(); + for(auto aToBIter = AToB.begin(); aToBIter != AToB.end(); ) { + if ( distLInf(aToBIter->first, aToBIter->second) > newThreshold ) { + // remove edge from AToB and BToA + //printDebug(isDebug, "removing edge ", aToBIter->first); + //printDebug(isDebug, " <-> ", aToBIter->second); + BToA.erase(aToBIter->second); + aToBIter = AToB.erase(aToBIter); + } else { + aToBIter++; + } + } + sanityCheck(); +} + +bool BoundMatchOracle::buildMatchingForThreshold(const double r) +{ + //bool isDebug {false}; + //printDebug(isDebug,"Entered buildMatchingForThreshold, r = " + std::to_string(r)); + if (prevQueryValue > r) { + M.trimMatching(r); + } + prevQueryValue = r; + while(true) { + buildLayerGraph(r); + //printDebug(isDebug,"Layer graph built"); + if (augPathExist) { + std::vector<Path> augmentingPaths; + DiagramPointSet copyLG0; + for(DiagramPoint p : layerGraph[0]) { + copyLG0.insert(p); + } + for(DiagramPoint exposedVertex : copyLG0) { + Path augPath; + if (buildAugmentingPath(exposedVertex, augPath)) { + //printDebug(isDebug, "Augmenting path found", augPath); + augmentingPaths.push_back(augPath); + } + /* + else { + printDebug(isDebug,"augmenting paths must exist, but were not found!", M); + std::cerr << "augmenting paths must exist, but were not found!" << std::endl; + std::cout.flush(); + std::cerr.flush(); + printLayerGraph(); + //throw "Something went wrong-1"; + //return M.isPerfect(); + // analyze: finished or no paths exist + // can this actually happen? + } + */ + + } + if (augmentingPaths.empty()) { + //printDebug(isDebug,"augmenting paths must exist, but were not found!", M); + std::cerr << "augmenting paths must exist, but were not found!" << std::endl; + throw "bad epsilon?"; + } + // swap all augmenting paths with matching to increase it + //printDebug(isDebug,"before increase with augmenting paths:", M); + for(auto& augPath : augmentingPaths ) { + //printDebug(isDebug, "Increasing with augm. path ", augPath); + M.increase(augPath); + } + //printDebug(isDebug,"after increase with augmenting paths:", M); + } else { + //printDebug(isDebug,"no augmenting paths exist, matching returned is:", M); + return M.isPerfect(); + } + } +} + +void BoundMatchOracle::printLayerGraph(void) +{ +#ifdef DEBUG_BOUND_MATCH + for(auto& layer : layerGraph) { + std::cout << "{ "; + for(auto& p : layer) { + std::cout << p << "; "; + } + std::cout << "\b\b }" << std::endl; + } +#endif +} + +void BoundMatchOracle::buildLayerGraph(double r) +{ + //bool isDebug {false}; + //printDebug(isDebug,"Entered buildLayerGraph"); + layerGraph.clear(); + DiagramPointSet L1 = M.getExposedVertices(); + //printDebug(isDebug,"Got exposed vertices"); + layerGraph.push_back(L1); + neighbOracle->rebuild(B, r); + //printDebug(isDebug,"Oracle rebuilt"); + size_t k = 0; + DiagramPointSet layerNextEven; + DiagramPointSet layerNextOdd; + bool exposedVerticesFound {false}; + while(true) { + //printDebug(isDebug, "k = ", k); + layerNextEven.clear(); + for( auto p : layerGraph[k]) { + //printDebug(isDebug,"looking for neighbours for ", p); + bool neighbFound; + DiagramPoint neighbour {0.0, 0.0, DiagramPoint::DIAG, 1}; + if (useRangeSearch) { + std::vector<DiagramPoint> neighbVec; + neighbOracle->getAllNeighbours(p, neighbVec); + neighbFound = !neighbVec.empty(); + for(auto& neighbPt : neighbVec) { + layerNextEven.insert(neighbPt); + if (!exposedVerticesFound and M.isExposed(neighbPt)) + exposedVerticesFound = true; + } + } else { + while(true) { + neighbFound = neighbOracle->getNeighbour(p, neighbour); + if (neighbFound) { + //printDebug(isDebug,"neighbour found, ", neighbour); + layerNextEven.insert(neighbour); + neighbOracle->deletePoint(neighbour); + //printDebug(isDebug,"is exposed: " + std::to_string(M.isExposed(neighbour))); + if ((!exposedVerticesFound) && M.isExposed(neighbour)) { + exposedVerticesFound = true; + } + } else { + //printDebug(isDebug,"no neighbours found for r = ", r); + break; + } + } + } // without range search + } // all vertices from previous odd layer processed + //printDebug(isDebug,"Next even layer finished"); + if (layerNextEven.empty()) { + //printDebug(isDebug,"Next even layer is empty, augPathExist = false"); + augPathExist = false; + break; + } + if (exposedVerticesFound) { + //printDebug(isDebug,"Exposed vertices found in the even layer, aug. paths exist"); + //printDebug(isDebug,"Deleting all non-exposed from the last layer (we do not need them)."); + for(auto it = layerNextEven.cbegin(); it != layerNextEven.cend(); ) { + if ( ! M.isExposed(*it) ) { + layerNextEven.erase(it++); + } else { + ++it; + } + + } + layerGraph.push_back(layerNextEven); + augPathExist = true; + break; + } + layerGraph.push_back(layerNextEven); + M.getAllAdjacentVertices(layerNextEven, layerNextOdd); + //printDebug(isDebug,"Next odd layer finished"); + layerGraph.push_back(layerNextOdd); + k += 2; + } + buildLayerOracles(r); + //printDebug(isDebug,"layer oracles built, layer graph:"); + printLayerGraph(); +} + + + +BoundMatchOracle::~BoundMatchOracle() +{ + for(auto& oracle : layerOracles) { + delete oracle; + } + delete neighbOracle; +} + +// create geometric oracles for each even layer +// odd layers have NULL in layerOracles +void BoundMatchOracle::buildLayerOracles(double r) +{ + //bool isDebug {false}; + //printDebug(isDebug,"entered buildLayerOracles"); + // free previously constructed oracles + for(auto& oracle : layerOracles) { + delete oracle; + } + layerOracles.clear(); + //printDebug(isDebug,"previous oracles deleted"); + for(size_t layerIdx = 0; layerIdx < layerGraph.size(); ++layerIdx) { + if (layerIdx % 2 == 1) { + // even layer, build actual oracle + layerOracles.push_back(new NeighbOracle(layerGraph[layerIdx], r, distEpsilon)); + } else { + // odd layer + layerOracles.push_back(nullptr); + } + } + //printDebug(isDebug,"exiting buildLayerOracles"); +} +} diff --git a/geom_bottleneck/bottleneck/src/brute.cpp b/geom_bottleneck/bottleneck/src/brute.cpp new file mode 100644 index 0000000..200bc35 --- /dev/null +++ b/geom_bottleneck/bottleneck/src/brute.cpp @@ -0,0 +1,110 @@ +//---------------------------------------------------------------------- +// File: brute.cpp +// Programmer: Sunil Arya and David Mount +// Description: Brute-force nearest neighbors +// Last modified: 05/03/05 (Version 1.1) +//---------------------------------------------------------------------- +// Copyright (c) 1997-2005 University of Maryland and Sunil Arya and +// David Mount. All Rights Reserved. +// +// This software and related documentation is part of the Approximate +// Nearest Neighbor Library (ANN). This software is provided under +// the provisions of the Lesser GNU Public License (LGPL). See the +// file ../ReadMe.txt for further information. +// +// The University of Maryland (U.M.) and the authors make no +// representations about the suitability or fitness of this software for +// any purpose. It is provided "as is" without express or implied +// warranty. +//---------------------------------------------------------------------- +// History: +// Revision 0.1 03/04/98 +// Initial release +// Revision 1.1 05/03/05 +// Added fixed-radius kNN search +//---------------------------------------------------------------------- + +#include <ANN/ANNx.h> // all ANN includes +#include "pr_queue_k.h" // k element priority queue + +//---------------------------------------------------------------------- +// Brute-force search simply stores a pointer to the list of +// data points and searches linearly for the nearest neighbor. +// The k nearest neighbors are stored in a k-element priority +// queue (which is implemented in a pretty dumb way as well). +// +// If ANN_ALLOW_SELF_MATCH is ANNfalse then data points at distance +// zero are not considered. +// +// Note that the error bound eps is passed in, but it is ignored. +// These routines compute exact nearest neighbors (which is needed +// for validation purposes in ann_test.cpp). +//---------------------------------------------------------------------- + +ANNbruteForce::ANNbruteForce( // constructor from point array + ANNpointArray pa, // point array + int n, // number of points + int dd) // dimension +{ + dim = dd; n_pts = n; pts = pa; +} + +ANNbruteForce::~ANNbruteForce() { } // destructor (empty) + +void ANNbruteForce::annkSearch( // approx k near neighbor search + ANNpoint q, // query point + int k, // number of near neighbors to return + ANNidxArray nn_idx, // nearest neighbor indices (returned) + ANNdistArray dd, // dist to near neighbors (returned) + double eps) // error bound (ignored) +{ + ANNmin_k mk(k); // construct a k-limited priority queue + int i; + + if (k > n_pts) { // too many near neighbors? + annError("Requesting more near neighbors than data points", ANNabort); + } + // run every point through queue + for (i = 0; i < n_pts; i++) { + // compute distance to point + ANNdist sqDist = annDist(dim, pts[i], q); + if (ANN_ALLOW_SELF_MATCH || sqDist != 0) + mk.insert(sqDist, i); + } + for (i = 0; i < k; i++) { // extract the k closest points + dd[i] = mk.ith_smallest_key(i); + nn_idx[i] = mk.ith_smallest_info(i); + } +} + +int ANNbruteForce::annkFRSearch( // approx fixed-radius kNN search + ANNpoint q, // query point + ANNdist sqRad, // squared radius + int k, // number of near neighbors to return + ANNidxArray nn_idx, // nearest neighbor array (returned) + ANNdistArray dd, // dist to near neighbors (returned) + double eps) // error bound +{ + ANNmin_k mk(k); // construct a k-limited priority queue + int i; + int pts_in_range = 0; // number of points in query range + // run every point through queue + for (i = 0; i < n_pts; i++) { + // compute distance to point + ANNdist sqDist = annDist(dim, pts[i], q); + if (sqDist <= sqRad && // within radius bound + (ANN_ALLOW_SELF_MATCH || sqDist != 0)) { // ...and no self match + mk.insert(sqDist, i); + pts_in_range++; + } + } + for (i = 0; i < k; i++) { // extract the k closest points + if (dd != NULL) + dd[i] = mk.ith_smallest_key(i); + if (nn_idx != NULL) + nn_idx[i] = mk.ith_smallest_info(i); + } + + return pts_in_range; +} +-\n}\n diff --git a/geom_bottleneck/bottleneck/src/neighb_oracle.cpp b/geom_bottleneck/bottleneck/src/neighb_oracle.cpp new file mode 100644 index 0000000..356883f --- /dev/null +++ b/geom_bottleneck/bottleneck/src/neighb_oracle.cpp @@ -0,0 +1,278 @@ +/* + Copyrigth 2015, D. Morozov, M. Kerber, A. Nigmetov + + This file is part of GeomBottleneck. + + GeomBottleneck is free software: you can redistribute it and/or modify + it under the terms of the Lesser GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + GeomBottleneck is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + Lesser GNU General Public License for more details. + + You should have received a copy of the Lesser GNU General Public License + along with GeomBottleneck. If not, see <http://www.gnu.org/licenses/>. + +*/ + + +#include <algorithm> +#include "neighb_oracle.h" +#include "def_debug.h" + +namespace geom_bt { +/*static void printDebug(//bool isDebug, std::string s)*/ +//{ +//#ifdef DEBUG_NEIGHBOUR_ORACLE + //if (isDebug) { + //std::cout << s << std::endl; + //} +//#endif +//} + +//static void printDebug(//bool isDebug, std::string s, const DiagramPoint& p) +//{ +//#ifdef DEBUG_NEIGHBOUR_ORACLE + //if (isDebug) { + //std::cout << s << p << std::endl; + //} +//#endif +//} + +//static void printDebug(//bool isDebug, std::string s, const double r) +//{ +//#ifdef DEBUG_NEIGHBOUR_ORACLE + //if (isDebug) { + //std::cout << s << r << std::endl; + //} +//#endif +//} + +//static void printDebug(//bool isDebug, std::string s, const DiagramPointSet& dpSet) +//{ +//#ifdef DEBUG_NEIGHBOUR_ORACLE + //if (isDebug) { + //std::cout << s << dpSet << std::endl; + //} +//#endif +//} + + + +// simple oracle +NeighbOracleSimple::NeighbOracleSimple() +{ + r = 0.0; +} + +NeighbOracleSimple::NeighbOracleSimple(const DiagramPointSet& S, const double rr, const double dEps) +{ + r = rr; + distEpsilon = dEps; + pointSet = S; +} + +void NeighbOracleSimple::rebuild(const DiagramPointSet& S, const double rr) +{ + pointSet = S; + r = rr; +} + +void NeighbOracleSimple::deletePoint(const DiagramPoint& p) +{ + pointSet.erase(p); +} + +bool NeighbOracleSimple::getNeighbour(const DiagramPoint& q, DiagramPoint& result) const +{ + for(auto pit = pointSet.cbegin(); pit != pointSet.cend(); ++pit) { + if ( distLInf(*pit, q) <= r) { + result = *pit; + return true; + } + } + return false; +} + +void NeighbOracleSimple::getAllNeighbours(const DiagramPoint& q, std::vector<DiagramPoint>& result) +{ + result.clear(); + for(const auto& point : pointSet) { + if ( distLInf(point, q) <= r) { + result.push_back(point); + } + } + for(auto& pt : result) { + deletePoint(pt); + } +} + +// ANN oracle +// + +NeighbOracleAnn::NeighbOracleAnn(const DiagramPointSet& S, const double rr, const double dEps) +{ + assert(dEps >= 0); + distEpsilon = dEps; + // allocate space for query point + // and the output of nearest neighbour search + // this memory will be used in getNeighbour and freed in destructor + annQueryPoint = annAllocPt(annDim); + annIndices = new ANNidx[annK]; + annDistances = new ANNdist[annK]; + annPoints = nullptr; + lo = annAllocPt(annDim); + hi = annAllocPt(annDim); + // create kd tree + kdTree = nullptr; + rebuild(S, rr); +} + +void NeighbOracleAnn::rebuild(const DiagramPointSet& S, double rr) +{ + //bool isDebug { false }; + //printDebug(isDebug, "Entered rebuild, r = ", rr); + r = rr; + size_t annNumPoints = S.size(); + //printDebug(isDebug, "S = ", S); + if (annNumPoints > 0) { + //originalPointSet = S; + pointIdxLookup.clear(); + pointIdxLookup.reserve(S.size()); + allPoints.clear(); + allPoints.reserve(S.size()); + diagonalPoints.clear(); + diagonalPoints.reserve(S.size() / 2); + for(auto pit = S.cbegin(); pit != S.cend(); ++pit) { + allPoints.push_back(*pit); + if (pit->type == DiagramPoint::DIAG) { + diagonalPoints.insert(*pit); + } + } + if (annPoints) { + annDeallocPts(annPoints); + } + annPoints = annAllocPts(annNumPoints, annDim); + auto annPointsPtr = *annPoints; + size_t pointIdx = 0; + for(auto& dataPoint : allPoints) { + pointIdxLookup.insert( { dataPoint, pointIdx++ } ); + *annPointsPtr++ = dataPoint.getRealX(); + *annPointsPtr++ = dataPoint.getRealY(); + } + delete kdTree; + kdTree = new ANNkd_tree(annPoints, + annNumPoints, + annDim, + 1, // bucket size + ANN_KD_STD); + } +} + +void NeighbOracleAnn::deletePoint(const DiagramPoint& p) +{ + //bool isDebug { true }; + auto findRes = pointIdxLookup.find(p); + assert(findRes != pointIdxLookup.end()); + //printDebug(isDebug, "Deleting point ", p); + size_t pointIdx { (*findRes).second }; + //printDebug(isDebug, "pointIdx = ", pointIdx); + //originalPointSet.erase(p); + diagonalPoints.erase(p, false); + kdTree->delete_point(pointIdx); +#ifdef DEBUG_NEIGHBOUR_ORACLE + kdTree->Print(ANNtrue, std::cout); +#endif +} + +bool NeighbOracleAnn::getNeighbour(const DiagramPoint& q, DiagramPoint& result) const +{ + //bool isDebug { false }; + //printDebug(isDebug, "getNeighbour for q = ", q); + if (0 == kdTree->getActualNumPoints() ) { + //printDebug(isDebug, "annNumPoints = 0, not found "); + return false; + } + // distance between two diagonal points + // is 0 + if (DiagramPoint::DIAG == q.type) { + if (!diagonalPoints.empty()) { + result = *diagonalPoints.cbegin(); + //printDebug(isDebug, "Neighbour found in diagonal points, res = ", result); + return true; + } + } + // if no neighbour found among diagonal points, + // search in ANN kd_tree + annQueryPoint[0] = q.getRealX(); + annQueryPoint[1] = q.getRealY(); + //annIndices[0] = ANN_NULL_IDX; + kdTree->annkSearch(annQueryPoint, annK, annIndices, annDistances, annEpsilon); + //kdTree->annkFRSearch(annQueryPoint, r, annK, annIndices, annDistances, annEpsilon); + //std::cout << distEpsilon << " = distEpsilon " << std::endl; + if (annDistances[0] <= r + distEpsilon) { + //if (annIndices[0] != ANN_NULL_IDX) { + result = allPoints[annIndices[0]]; + //printDebug(isDebug, "Neighbour found with kd-tree, index = ", annIndices[0]); + //printDebug(isDebug, "result = ", result); + return true; + } + //printDebug(isDebug, "No neighbour found for r = ", r); + return false; +} + +void NeighbOracleAnn::getAllNeighbours(const DiagramPoint& q, std::vector<DiagramPoint>& result) +{ + //bool isDebug { true }; + //printDebug(isDebug, "Entered getAllNeighbours for q = ", q); + result.clear(); + // add diagonal points, if necessary + if ( DiagramPoint::DIAG == q.type) { + for( auto& diagPt : diagonalPoints ) { + result.push_back(diagPt); + } + } + // delete diagonal points we found + // to prevent finding them again + for(auto& pt : result) { + //printDebug(isDebug, "deleting DIAG point pt = ", pt); + deletePoint(pt); + } + size_t diagOffset = result.size(); + // create the query rectangle + // centered at q of radius r + lo[0] = q.getRealX() - r; + lo[1] = q.getRealY() - r; + hi[0] = q.getRealX() + r; + hi[1] = q.getRealY() + r; + ANNorthRect annRect { annDim, lo, hi }; + std::vector<size_t> pointIndicesOut; + // perorm range search on kd-tree + kdTree->range_search(annRect, pointIndicesOut); + // get actual points in result + for(auto& ptIdx : pointIndicesOut) { + result.push_back(allPoints[ptIdx]); + } + // delete all points we found + for(auto ptIt = result.begin() + diagOffset; ptIt != result.end(); ++ptIt) { + //printDebug(isDebug, "deleting point pt = ", *ptIt); + deletePoint(*ptIt); + } +} + +NeighbOracleAnn::~NeighbOracleAnn() +{ + delete [] annIndices; + delete [] annDistances; + delete kdTree; + annDeallocPt(annQueryPoint); + annDeallocPt(lo); + annDeallocPt(hi); + if (annPoints) { + annDeallocPts(annPoints); + } +} +} |