X-Git-Url: https://vcs.maemo.org/git/?a=blobdiff_plain;f=src%2Fcv%2F_cvkdtree.hpp;fp=src%2Fcv%2F_cvkdtree.hpp;h=2afda210167a6014b57c09a016c4cbf502798351;hb=e4c14cdbdf2fe805e79cd96ded236f57e7b89060;hp=0000000000000000000000000000000000000000;hpb=454138ff8a20f6edb9b65a910101403d8b520643;p=opencv

diff --git a/src/cv/_cvkdtree.hpp b/src/cv/_cvkdtree.hpp
new file mode 100644
index 0000000..2afda21
--- /dev/null
+++ b/src/cv/_cvkdtree.hpp
@@ -0,0 +1,467 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                        Intel License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2008, Xavier Delacour, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of Intel Corporation may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+// 2008-05-13, Xavier Delacour <xavier.delacour@gmail.com>
+
+#ifndef __cv_kdtree_h__
+#define __cv_kdtree_h__
+
+#include "_cv.h"
+
+#include <vector>
+#include <algorithm>
+#include <limits>
+#include <iostream>
+#include "assert.h"
+#include "math.h"
+
+#if _MSC_VER >= 1400
+#pragma warning(disable: 4512) // suppress "assignment operator could not be generated"
+#endif
+
+// J.S. Beis and D.G. Lowe. Shape indexing using approximate nearest-neighbor search 
+// in highdimensional spaces. In Proc. IEEE Conf. Comp. Vision Patt. Recog., 
+// pages 1000--1006, 1997. http://citeseer.ist.psu.edu/beis97shape.html 
+#undef __deref
+#undef __valuetype
+
+template < class __valuetype, class __deref >
+class CvKDTree {
+public:
+  typedef __deref deref_type;
+  typedef typename __deref::scalar_type scalar_type;
+  typedef typename __deref::accum_type accum_type;
+
+private:
+  struct node {
+    int dim;			// split dimension; >=0 for nodes, -1 for leaves
+    __valuetype value;		// if leaf, value of leaf
+    int left, right;		// node indices of left and right branches
+    scalar_type boundary;	// left if deref(value,dim)<=boundary, otherwise right
+  };
+  typedef std::vector < node > node_array;
+
+  __deref deref;		// requires operator() (__valuetype lhs,int dim)
+
+  node_array nodes;		// node storage
+  int point_dim;		// dimension of points (the k in kd-tree)
+  int root_node;		// index of root node, -1 if empty tree
+
+  // for given set of point indices, compute dimension of highest variance
+  template < class __instype, class __valuector >
+  int dimension_of_highest_variance(__instype * first, __instype * last,
+				    __valuector ctor) {
+    assert(last - first > 0);
+
+    accum_type maxvar = -std::numeric_limits < accum_type >::max();
+    int maxj = -1;
+    for (int j = 0; j < point_dim; ++j) {
+      accum_type mean = 0;
+      for (__instype * k = first; k < last; ++k)
+	mean += deref(ctor(*k), j);
+      mean /= last - first;
+      accum_type var = 0;
+      for (__instype * k = first; k < last; ++k) {
+	accum_type diff = accum_type(deref(ctor(*k), j)) - mean;
+	var += diff * diff;
+      }
+      var /= last - first;
+
+      assert(maxj != -1 || var >= maxvar);
+
+      if (var >= maxvar) {
+	maxvar = var;
+	maxj = j;
+      }
+    }
+
+    return maxj;
+  }
+
+  // given point indices and dimension, find index of median; (almost) modifies [first,last) 
+  // such that points_in[first,median]<=point[median], points_in(median,last)>point[median].
+  // implemented as partial quicksort; expected linear perf.
+  template < class __instype, class __valuector >
+  __instype * median_partition(__instype * first, __instype * last,
+			       int dim, __valuector ctor) {
+    assert(last - first > 0);
+    __instype *k = first + (last - first) / 2;
+    median_partition(first, last, k, dim, ctor);
+    return k;
+  }
+
+  template < class __instype, class __valuector >
+  struct median_pr {
+    const __instype & pivot;
+    int dim;
+    __deref deref;
+    __valuector ctor;
+    median_pr(const __instype & _pivot, int _dim, __deref _deref, __valuector _ctor)
+      : pivot(_pivot), dim(_dim), deref(_deref), ctor(_ctor) {
+    }
+    bool operator() (const __instype & lhs) const {
+      return deref(ctor(lhs), dim) <= deref(ctor(pivot), dim);
+    }
+  };
+
+  template < class __instype, class __valuector >
+  void median_partition(__instype * first, __instype * last, 
+			__instype * k, int dim, __valuector ctor) {
+    int pivot = (int)((last - first) / 2);
+
+    std::swap(first[pivot], last[-1]);
+    __instype *middle = std::partition(first, last - 1,
+				       median_pr < __instype, __valuector > 
+				       (last[-1], dim, deref, ctor));
+    std::swap(*middle, last[-1]);
+
+    if (middle < k)
+      median_partition(middle + 1, last, k, dim, ctor);
+    else if (middle > k)
+      median_partition(first, middle, k, dim, ctor);
+  }
+
+  // insert given points into the tree; return created node
+  template < class __instype, class __valuector >
+  int insert(__instype * first, __instype * last, __valuector ctor) {
+    if (first == last)
+      return -1;
+    else {
+
+      int dim = dimension_of_highest_variance(first, last, ctor);
+      __instype *median = median_partition(first, last, dim, ctor);
+
+      __instype *split = median;
+      for (; split != last && deref(ctor(*split), dim) == 
+	     deref(ctor(*median), dim); ++split);
+
+      if (split == last) { // leaf
+	int nexti = -1;
+	for (--split; split >= first; --split) {
+	  int i = (int)nodes.size();
+	  node & n = *nodes.insert(nodes.end(), node());
+	  n.dim = -1;
+	  n.value = ctor(*split);
+	  n.left = -1;
+	  n.right = nexti;
+	  nexti = i;
+	}
+
+	return nexti;
+      } else { // node
+	int i = (int)nodes.size();
+	// note that recursive insert may invalidate this ref
+	node & n = *nodes.insert(nodes.end(), node());
+
+	n.dim = dim;
+	n.boundary = deref(ctor(*median), dim);
+
+	int left = insert(first, split, ctor);
+	nodes[i].left = left;
+	int right = insert(split, last, ctor);
+	nodes[i].right = right;
+
+	return i;
+      }
+    }
+  }
+
+  // run to leaf; linear search for p;
+  // if found, remove paths to empty leaves on unwind
+  bool remove(int *i, const __valuetype & p) {
+    if (*i == -1)
+      return false;
+    node & n = nodes[*i];
+    bool r;
+
+    if (n.dim >= 0) { // node
+      if (deref(p, n.dim) <= n.boundary) // left
+	r = remove(&n.left, p);
+      else // right
+	r = remove(&n.right, p);
+
+      // if terminal, remove this node
+      if (n.left == -1 && n.right == -1)
+	*i = -1;
+
+      return r;
+    } else { // leaf
+      if (n.value == p) {
+	*i = n.right;
+	return true;
+      } else
+	return remove(&n.right, p);
+    }
+  }
+
+public:
+  struct identity_ctor {
+    const __valuetype & operator() (const __valuetype & rhs) const {
+      return rhs;
+    }
+  };
+
+  // initialize an empty tree
+  CvKDTree(__deref _deref = __deref())
+    : deref(_deref), root_node(-1) {
+  }
+  // given points, initialize a balanced tree
+  CvKDTree(__valuetype * first, __valuetype * last, int _point_dim,
+	   __deref _deref = __deref())
+    : deref(_deref) {
+    set_data(first, last, _point_dim, identity_ctor());
+  }
+  // given points, initialize a balanced tree
+  template < class __instype, class __valuector >
+  CvKDTree(__instype * first, __instype * last, int _point_dim,
+	   __valuector ctor, __deref _deref = __deref())
+    : deref(_deref) {
+    set_data(first, last, _point_dim, ctor);
+  }
+
+  void set_deref(__deref _deref) {
+    deref = _deref;
+  }
+
+  void set_data(__valuetype * first, __valuetype * last, int _point_dim) {
+    set_data(first, last, _point_dim, identity_ctor());
+  }
+  template < class __instype, class __valuector >
+  void set_data(__instype * first, __instype * last, int _point_dim,
+		__valuector ctor) {
+    point_dim = _point_dim;
+    nodes.clear();
+    nodes.reserve(last - first);
+    root_node = insert(first, last, ctor);
+  }
+
+  int dims() const {
+    return point_dim;
+  }
+
+  // remove the given point
+  bool remove(const __valuetype & p) {
+    return remove(&root_node, p);
+  }
+
+  void print() const {
+    print(root_node);
+  }
+  void print(int i, int indent = 0) const {
+    if (i == -1)
+      return;
+    for (int j = 0; j < indent; ++j)
+      std::cout << " ";
+    const node & n = nodes[i];
+    if (n.dim >= 0) {
+      std::cout << "node " << i << ", left " << nodes[i].left << ", right " << 
+	nodes[i].right << ", dim " << nodes[i].dim << ", boundary " << 
+	nodes[i].boundary << std::endl;
+      print(n.left, indent + 3);
+      print(n.right, indent + 3);
+    } else
+      std::cout << "leaf " << i << ", value = " << nodes[i].value << std::endl;
+  }
+
+  ////////////////////////////////////////////////////////////////////////////////////////
+  // bbf search
+public:
+  struct bbf_nn {		// info on found neighbors (approx k nearest)
+    const __valuetype *p;	// nearest neighbor
+    accum_type dist;		// distance from d to query point
+    bbf_nn(const __valuetype & _p, accum_type _dist)
+      : p(&_p), dist(_dist) {
+    }
+    bool operator<(const bbf_nn & rhs) const {
+      return dist < rhs.dist;
+    }
+  };
+  typedef std::vector < bbf_nn > bbf_nn_pqueue;
+private:
+  struct bbf_node {		// info on branches not taken
+    int node;			// corresponding node
+    accum_type dist;		// minimum distance from bounds to query point
+    bbf_node(int _node, accum_type _dist)
+      : node(_node), dist(_dist) {
+    }
+    bool operator<(const bbf_node & rhs) const {
+      return dist > rhs.dist;
+    }
+  };
+  typedef std::vector < bbf_node > bbf_pqueue;
+  mutable bbf_pqueue tmp_pq;
+
+  // called for branches not taken, as bbf walks to leaf;
+  // construct bbf_node given minimum distance to bounds of alternate branch
+  void pq_alternate(int alt_n, bbf_pqueue & pq, scalar_type dist) const {
+    if (alt_n == -1)
+      return;
+
+    // add bbf_node for alternate branch in priority queue
+    pq.push_back(bbf_node(alt_n, dist));
+    push_heap(pq.begin(), pq.end());
+  }
+
+  // called by bbf to walk to leaf;
+  // takes one step down the tree towards query point d
+  template < class __desctype >
+  int bbf_branch(int i, const __desctype * d, bbf_pqueue & pq) const {
+    const node & n = nodes[i];
+    // push bbf_node with bounds of alternate branch, then branch
+    if (d[n.dim] <= n.boundary) {	// left
+      pq_alternate(n.right, pq, n.boundary - d[n.dim]);
+      return n.left;
+    } else {			// right
+      pq_alternate(n.left, pq, d[n.dim] - n.boundary);
+      return n.right;
+    }
+  }
+
+  // compute euclidean distance between two points
+  template < class __desctype >
+  accum_type distance(const __desctype * d, const __valuetype & p) const {
+    accum_type dist = 0;
+    for (int j = 0; j < point_dim; ++j) {
+      accum_type diff = accum_type(d[j]) - accum_type(deref(p, j));
+      dist += diff * diff;
+    } return (accum_type) sqrt(dist);
+  }
+
+  // called per candidate nearest neighbor; constructs new bbf_nn for
+  // candidate and adds it to priority queue of all candidates; if 
+  // queue len exceeds k, drops the point furthest from query point d.
+  template < class __desctype >
+  void bbf_new_nn(bbf_nn_pqueue & nn_pq, int k, 
+		  const __desctype * d, const __valuetype & p) const {
+    bbf_nn nn(p, distance(d, p));
+    if ((int) nn_pq.size() < k) {
+      nn_pq.push_back(nn);
+      push_heap(nn_pq.begin(), nn_pq.end());
+    } else if (nn_pq[0].dist > nn.dist) {
+      pop_heap(nn_pq.begin(), nn_pq.end());
+      nn_pq.end()[-1] = nn;
+      push_heap(nn_pq.begin(), nn_pq.end());
+    }
+    assert(nn_pq.size() < 2 || nn_pq[0].dist >= nn_pq[1].dist);
+  }
+
+public:
+  // finds (with high probability) the k nearest neighbors of d, 
+  // searching at most emax leaves/bins.
+  // ret_nn_pq is an array containing the (at most) k nearest neighbors 
+  // (see bbf_nn structure def above).
+  template < class __desctype >
+  int find_nn_bbf(const __desctype * d, 
+		  int k, int emax, 
+		  bbf_nn_pqueue & ret_nn_pq) const {
+    assert(k > 0);
+    ret_nn_pq.clear();
+
+    if (root_node == -1)
+      return 0;
+
+    // add root_node to bbf_node priority queue;
+    // iterate while queue non-empty and emax>0
+    tmp_pq.clear();
+    tmp_pq.push_back(bbf_node(root_node, 0));
+    while (tmp_pq.size() && emax > 0) {
+
+      // from node nearest query point d, run to leaf
+      pop_heap(tmp_pq.begin(), tmp_pq.end());
+      bbf_node bbf(tmp_pq.end()[-1]);
+      tmp_pq.erase(tmp_pq.end() - 1);
+
+      int i;
+      for (i = bbf.node;
+	   i != -1 && nodes[i].dim >= 0; 
+	   i = bbf_branch(i, d, tmp_pq));
+
+      if (i != -1) {
+
+	// add points in leaf/bin to ret_nn_pq
+	do {
+	  bbf_new_nn(ret_nn_pq, k, d, nodes[i].value);
+	} while (-1 != (i = nodes[i].right));
+
+	--emax;
+      }
+    }
+
+    tmp_pq.clear();
+    return (int)ret_nn_pq.size();
+  }
+
+  ////////////////////////////////////////////////////////////////////////////////////////
+  // orthogonal range search
+private:
+  void find_ortho_range(int i, scalar_type * bounds_min,
+			scalar_type * bounds_max,
+			std::vector < __valuetype > &inbounds) const {
+    if (i == -1)
+      return;
+    const node & n = nodes[i];
+    if (n.dim >= 0) { // node
+      if (bounds_min[n.dim] <= n.boundary)
+	find_ortho_range(n.left, bounds_min, bounds_max, inbounds);
+      if (bounds_max[n.dim] > n.boundary)
+	find_ortho_range(n.right, bounds_min, bounds_max, inbounds);
+    } else { // leaf
+      do {
+	inbounds.push_back(nodes[i].value);
+      } while (-1 != (i = nodes[i].right));
+    }
+  }
+public:
+  // return all points that lie within the given bounds; inbounds is cleared
+  int find_ortho_range(scalar_type * bounds_min,
+		       scalar_type * bounds_max,
+		       std::vector < __valuetype > &inbounds) const {
+    inbounds.clear();
+    find_ortho_range(root_node, bounds_min, bounds_max, inbounds);
+    return (int)inbounds.size();
+  }
+};
+
+#endif // __cv_kdtree_h__
+
+// Local Variables:
+// mode:C++
+// End: