16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2008, Xavier Delacour, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 2008-05-13, Xavier Delacour <xavier.delacour@gmail.com> 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#ifndef __cv_kdtree_h__ 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#define __cv_kdtree_h__ 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <vector> 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <algorithm> 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <limits> 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <iostream> 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "assert.h" 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "math.h" 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 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 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#undef __deref 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#undef __valuetype 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntemplate < class __valuetype, class __deref > 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennclass CvKDTree { 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennpublic: 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn typedef __deref deref_type; 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn typedef typename __deref::scalar_type scalar_type; 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn typedef typename __deref::accum_type accum_type; 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennprivate: 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn struct node { 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dim; // split dimension; >=0 for nodes, -1 for leaves 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __valuetype value; // if leaf, value of leaf 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int left, right; // node indices of left and right branches 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scalar_type boundary; // left if deref(value,dim)<=boundary, otherwise right 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn typedef std::vector < node > node_array; 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __deref deref; // requires operator() (__valuetype lhs,int dim) 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn node_array nodes; // node storage 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int point_dim; // dimension of points (the k in kd-tree) 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int root_node; // index of root node, -1 if empty tree 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // for given set of point indices, compute dimension of highest variance 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __instype, class __valuector > 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dimension_of_highest_variance(__instype * first, __instype * last, 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __valuector ctor) { 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(last - first > 0); 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accum_type maxvar = -std::numeric_limits < accum_type >::max(); 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxj = -1; 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int j = 0; j < point_dim; ++j) { 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accum_type mean = 0; 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (__instype * k = first; k < last; ++k) 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mean += deref(ctor(*k), j); 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mean /= last - first; 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accum_type var = 0; 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (__instype * k = first; k < last; ++k) { 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accum_type diff = accum_type(deref(ctor(*k), j)) - mean; 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var += diff * diff; 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn var /= last - first; 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(maxj != -1 || var >= maxvar); 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (var >= maxvar) { 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxvar = var; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxj = j; 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return maxj; 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // given point indices and dimension, find index of median; (almost) modifies [first,last) 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // such that points_in[first,median]<=point[median], points_in(median,last)>point[median]. 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // implemented as partial quicksort; expected linear perf. 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __instype, class __valuector > 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __instype * median_partition(__instype * first, __instype * last, 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dim, __valuector ctor) { 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(last - first > 0); 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __instype *k = first + (last - first) / 2; 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn median_partition(first, last, k, dim, ctor); 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return k; 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __instype, class __valuector > 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn struct median_pr { 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const __instype & pivot; 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dim; 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __deref deref; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __valuector ctor; 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn median_pr(const __instype & _pivot, int _dim, __deref _deref, __valuector _ctor) 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn : pivot(_pivot), dim(_dim), deref(_deref), ctor(_ctor) { 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool operator() (const __instype & lhs) const { 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return deref(ctor(lhs), dim) <= deref(ctor(pivot), dim); 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __instype, class __valuector > 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void median_partition(__instype * first, __instype * last, 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __instype * k, int dim, __valuector ctor) { 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int pivot = (last - first) / 2; 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::swap(first[pivot], last[-1]); 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __instype *middle = std::partition(first, last - 1, 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn median_pr < __instype, __valuector > 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (last[-1], dim, deref, ctor)); 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::swap(*middle, last[-1]); 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (middle < k) 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn median_partition(middle + 1, last, k, dim, ctor); 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if (middle > k) 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn median_partition(first, middle, k, dim, ctor); 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // insert given points into the tree; return created node 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __instype, class __valuector > 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int insert(__instype * first, __instype * last, __valuector ctor) { 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (first == last) 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return -1; 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else { 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dim = dimension_of_highest_variance(first, last, ctor); 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __instype *median = median_partition(first, last, dim, ctor); 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __instype *split = median; 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (; split != last && deref(ctor(*split), dim) == 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn deref(ctor(*median), dim); ++split); 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (split == last) { // leaf 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int nexti = -1; 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (--split; split >= first; --split) { 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i = nodes.size(); 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn node & n = *nodes.insert(nodes.end(), node()); 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n.dim = -1; 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n.value = ctor(*split); 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n.left = -1; 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n.right = nexti; 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nexti = i; 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return nexti; 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else { // node 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i = nodes.size(); 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // note that recursive insert may invalidate this ref 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn node & n = *nodes.insert(nodes.end(), node()); 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n.dim = dim; 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n.boundary = deref(ctor(*median), dim); 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int left = insert(first, split, ctor); 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nodes[i].left = left; 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int right = insert(split, last, ctor); 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nodes[i].right = right; 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return i; 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // run to leaf; linear search for p; 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // if found, remove paths to empty leaves on unwind 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool remove(int *i, const __valuetype & p) { 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (*i == -1) 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return false; 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn node & n = nodes[*i]; 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool r; 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (n.dim >= 0) { // node 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (deref(p, n.dim) <= n.boundary) // left 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r = remove(&n.left, p); 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else // right 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r = remove(&n.right, p); 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // if terminal, remove this node 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (n.left == -1 && n.right == -1) 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *i = -1; 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return r; 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else { // leaf 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (n.value == p) { 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *i = n.right; 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return remove(&n.right, p); 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennpublic: 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn struct identity_ctor { 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const __valuetype & operator() (const __valuetype & rhs) const { 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return rhs; 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize an empty tree 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvKDTree(__deref _deref = __deref()) 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn : deref(_deref), root_node(-1) { 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // given points, initialize a balanced tree 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvKDTree(__valuetype * first, __valuetype * last, int _point_dim, 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __deref _deref = __deref()) 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn : deref(_deref) { 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn set_data(first, last, _point_dim, identity_ctor()); 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // given points, initialize a balanced tree 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __instype, class __valuector > 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvKDTree(__instype * first, __instype * last, int _point_dim, 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __valuector ctor, __deref _deref = __deref()) 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn : deref(_deref) { 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn set_data(first, last, _point_dim, ctor); 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void set_deref(__deref _deref) { 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn deref = _deref; 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void set_data(__valuetype * first, __valuetype * last, int _point_dim) { 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn set_data(first, last, _point_dim, identity_ctor()); 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __instype, class __valuector > 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void set_data(__instype * first, __instype * last, int _point_dim, 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __valuector ctor) { 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point_dim = _point_dim; 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nodes.clear(); 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nodes.reserve(last - first); 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn root_node = insert(first, last, ctor); 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dims() const { 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return point_dim; 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // remove the given point 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool remove(const __valuetype & p) { 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return remove(&root_node, p); 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void print() const { 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn print(root_node); 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void print(int i, int indent = 0) const { 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (i == -1) 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int j = 0; j < indent; ++j) 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::cout << " "; 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const node & n = nodes[i]; 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (n.dim >= 0) { 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::cout << "node " << i << ", left " << nodes[i].left << ", right " << 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nodes[i].right << ", dim " << nodes[i].dim << ", boundary " << 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nodes[i].boundary << std::endl; 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn print(n.left, indent + 3); 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn print(n.right, indent + 3); 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::cout << "leaf " << i << ", value = " << nodes[i].value << std::endl; 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //////////////////////////////////////////////////////////////////////////////////////// 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // bbf search 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennpublic: 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn struct bbf_nn { // info on found neighbors (approx k nearest) 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const __valuetype *p; // nearest neighbor 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accum_type dist; // distance from d to query point 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bbf_nn(const __valuetype & _p, accum_type _dist) 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn : p(&_p), dist(_dist) { 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool operator<(const bbf_nn & rhs) const { 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return dist < rhs.dist; 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn typedef std::vector < bbf_nn > bbf_nn_pqueue; 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennprivate: 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn struct bbf_node { // info on branches not taken 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int node; // corresponding node 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accum_type dist; // minimum distance from bounds to query point 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bbf_node(int _node, accum_type _dist) 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn : node(_node), dist(_dist) { 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool operator<(const bbf_node & rhs) const { 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return dist > rhs.dist; 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn typedef std::vector < bbf_node > bbf_pqueue; 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mutable bbf_pqueue tmp_pq; 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // called for branches not taken, as bbf walks to leaf; 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // construct bbf_node given minimum distance to bounds of alternate branch 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void pq_alternate(int alt_n, bbf_pqueue & pq, scalar_type dist) const { 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (alt_n == -1) 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // add bbf_node for alternate branch in priority queue 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pq.push_back(bbf_node(alt_n, dist)); 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn push_heap(pq.begin(), pq.end()); 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // called by bbf to walk to leaf; 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // takes one step down the tree towards query point d 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __desctype > 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int bbf_branch(int i, const __desctype * d, bbf_pqueue & pq) const { 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const node & n = nodes[i]; 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // push bbf_node with bounds of alternate branch, then branch 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (d[n.dim] <= n.boundary) { // left 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pq_alternate(n.right, pq, n.boundary - d[n.dim]); 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return n.left; 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else { // right 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pq_alternate(n.left, pq, d[n.dim] - n.boundary); 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return n.right; 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // compute euclidean distance between two points 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __desctype > 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accum_type distance(const __desctype * d, const __valuetype & p) const { 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accum_type dist = 0; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (int j = 0; j < point_dim; ++j) { 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn accum_type diff = accum_type(d[j]) - accum_type(deref(p, j)); 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist += diff * diff; 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } return (accum_type) sqrt(dist); 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // called per candidate nearest neighbor; constructs new bbf_nn for 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // candidate and adds it to priority queue of all candidates; if 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // queue len exceeds k, drops the point furthest from query point d. 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __desctype > 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void bbf_new_nn(bbf_nn_pqueue & nn_pq, int k, 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const __desctype * d, const __valuetype & p) const { 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bbf_nn nn(p, distance(d, p)); 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if ((int) nn_pq.size() < k) { 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nn_pq.push_back(nn); 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn push_heap(nn_pq.begin(), nn_pq.end()); 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else if (nn_pq[0].dist > nn.dist) { 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pop_heap(nn_pq.begin(), nn_pq.end()); 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nn_pq.end()[-1] = nn; 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn push_heap(nn_pq.begin(), nn_pq.end()); 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(nn_pq.size() < 2 || nn_pq[0].dist >= nn_pq[1].dist); 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennpublic: 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // finds (with high probability) the k nearest neighbors of d, 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // searching at most emax leaves/bins. 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // ret_nn_pq is an array containing the (at most) k nearest neighbors 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // (see bbf_nn structure def above). 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn template < class __desctype > 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int find_nn_bbf(const __desctype * d, 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int k, int emax, 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bbf_nn_pqueue & ret_nn_pq) const { 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(k > 0); 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ret_nn_pq.clear(); 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (root_node == -1) 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return 0; 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // add root_node to bbf_node priority queue; 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // iterate while queue non-empty and emax>0 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp_pq.clear(); 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp_pq.push_back(bbf_node(root_node, 0)); 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while (tmp_pq.size() && emax > 0) { 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // from node nearest query point d, run to leaf 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pop_heap(tmp_pq.begin(), tmp_pq.end()); 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bbf_node bbf(tmp_pq.end()[-1]); 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp_pq.erase(tmp_pq.end() - 1); 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for (i = bbf.node; 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i != -1 && nodes[i].dim >= 0; 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i = bbf_branch(i, d, tmp_pq)); 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (i != -1) { 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // add points in leaf/bin to ret_nn_pq 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do { 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bbf_new_nn(ret_nn_pq, k, d, nodes[i].value); 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } while (-1 != (i = nodes[i].right)); 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn --emax; 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp_pq.clear(); 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return ret_nn_pq.size(); 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //////////////////////////////////////////////////////////////////////////////////////// 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // orthogonal range search 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennprivate: 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn void find_ortho_range(int i, scalar_type * bounds_min, 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scalar_type * bounds_max, 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::vector < __valuetype > &inbounds) const { 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (i == -1) 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const node & n = nodes[i]; 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (n.dim >= 0) { // node 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (bounds_min[n.dim] <= n.boundary) 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn find_ortho_range(n.left, bounds_min, bounds_max, inbounds); 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if (bounds_max[n.dim] > n.boundary) 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn find_ortho_range(n.right, bounds_min, bounds_max, inbounds); 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else { // leaf 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do { 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn inbounds.push_back(nodes[i].value); 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } while (-1 != (i = nodes[i].right)); 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennpublic: 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // return all points that lie within the given bounds; inbounds is cleared 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int find_ortho_range(scalar_type * bounds_min, 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn scalar_type * bounds_max, 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn std::vector < __valuetype > &inbounds) const { 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn inbounds.clear(); 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn find_ortho_range(root_node, bounds_min, bounds_max, inbounds); 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return inbounds.size(); 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn}; 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif // __cv_kdtree_h__ 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Local Variables: 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// mode:C++ 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// End: 462