kd_tree.h
Go to the documentation of this file.
1 //----------------------------------------------------------------------
2 // File: kd_tree.h
3 // Programmer: Sunil Arya and David Mount
4 // Description: Declarations for standard kd-tree routines
5 // Last modified: 05/03/05 (Version 1.1)
6 //----------------------------------------------------------------------
7 // Copyright (c) 1997-2005 University of Maryland and Sunil Arya and
8 // David Mount. All Rights Reserved.
9 //
10 // This software and related documentation is part of the Approximate
11 // Nearest Neighbor Library (ANN). This software is provided under
12 // the provisions of the Lesser GNU Public License (LGPL). See the
13 // file ../ReadMe.txt for further information.
14 //
15 // The University of Maryland (U.M.) and the authors make no
16 // representations about the suitability or fitness of this software for
17 // any purpose. It is provided "as is" without express or implied
18 // warranty.
19 //----------------------------------------------------------------------
20 // History:
21 // Revision 0.1 03/04/98
22 // Initial release
23 // Revision 1.1 05/03/05
24 // Added fixed radius kNN search
25 //----------------------------------------------------------------------
26 
27 #ifndef ANN_kd_tree_H
28 #define ANN_kd_tree_H
29 
30 #include <ANN/ANNx.h> // all ANN includes
31 
32 using namespace std; // make std:: available
33 
34 //----------------------------------------------------------------------
35 // Generic kd-tree node
36 //
37 // Nodes in kd-trees are of two types, splitting nodes which contain
38 // splitting information (a splitting hyperplane orthogonal to one
39 // of the coordinate axes) and leaf nodes which contain point
40 // information (an array of points stored in a bucket). This is
41 // handled by making a generic class kd_node, which is essentially an
42 // empty shell, and then deriving the leaf and splitting nodes from
43 // this.
44 //----------------------------------------------------------------------
45 
46 class ANNkd_node{ // generic kd-tree node (empty shell)
47 public:
48  virtual ~ANNkd_node() {} // virtual distroyer
49 
50  virtual void ann_search(ANNdist) = 0; // tree search
51  virtual void ann_pri_search(ANNdist) = 0; // priority search
52  virtual void ann_FR_search(ANNdist) = 0; // fixed-radius search
53 
54  virtual void getStats( // get tree statistics
55  int dim, // dimension of space
56  ANNkdStats &st, // statistics
57  ANNorthRect &bnd_box) = 0; // bounding box
58  // print node
59  virtual void print(int level, ostream &out) = 0;
60  virtual void dump(ostream &out) = 0; // dump node
61 
62  friend class ANNkd_tree; // allow kd-tree to access us
63 };
64 
65 //----------------------------------------------------------------------
66 // kd-splitting function:
67 // kd_splitter is a pointer to a splitting routine for preprocessing.
68 // Different splitting procedures result in different strategies
69 // for building the tree.
70 //----------------------------------------------------------------------
71 
72 typedef void (*ANNkd_splitter)( // splitting routine for kd-trees
73  ANNpointArray pa, // point array (unaltered)
74  ANNidxArray pidx, // point indices (permuted on return)
75  const ANNorthRect &bnds, // bounding rectangle for cell
76  int n, // number of points
77  int dim, // dimension of space
78  int &cut_dim, // cutting dimension (returned)
79  ANNcoord &cut_val, // cutting value (returned)
80  int &n_lo); // num of points on low side (returned)
81 
82 //----------------------------------------------------------------------
83 // Leaf kd-tree node
84 // Leaf nodes of the kd-tree store the set of points associated
85 // with this bucket, stored as an array of point indices. These
86 // are indices in the array points, which resides with the
87 // root of the kd-tree. We also store the number of points
88 // that reside in this bucket.
89 //----------------------------------------------------------------------
90 
91 class ANNkd_leaf: public ANNkd_node // leaf node for kd-tree
92 {
93  int n_pts; // no. points in bucket
94  ANNidxArray bkt; // bucket of points
95 public:
96  ANNkd_leaf( // constructor
97  int n, // number of points
98  ANNidxArray b) // bucket
99  {
100  n_pts = n; // number of points in bucket
101  bkt = b; // the bucket
102  }
103 
104  ~ANNkd_leaf() { } // destructor (none)
105 
106  virtual void getStats( // get tree statistics
107  int dim, // dimension of space
108  ANNkdStats &st, // statistics
109  ANNorthRect &bnd_box); // bounding box
110  virtual void print(int level, ostream &out);// print node
111  virtual void dump(ostream &out); // dump node
112 
113  virtual void ann_search(ANNdist); // standard search
114  virtual void ann_pri_search(ANNdist); // priority search
115  virtual void ann_FR_search(ANNdist); // fixed-radius search
116 };
117 
118 //----------------------------------------------------------------------
119 // KD_TRIVIAL is a special pointer to an empty leaf node. Since
120 // some splitting rules generate many (more than 50%) trivial
121 // leaves, we use this one shared node to save space.
122 //
123 // The pointer is initialized to NULL, but whenever a kd-tree is
124 // created, we allocate this node, if it has not already been
125 // allocated. This node is *never* deallocated, so it produces
126 // a small memory leak.
127 //----------------------------------------------------------------------
128 
129 extern ANNkd_leaf *KD_TRIVIAL; // trivial (empty) leaf node
130 
131 //----------------------------------------------------------------------
132 // kd-tree splitting node.
133 // Splitting nodes contain a cutting dimension and a cutting value.
134 // These indicate the axis-parellel plane which subdivide the
135 // box for this node. The extent of the bounding box along the
136 // cutting dimension is maintained (this is used to speed up point
137 // to box distance calculations) [we do not store the entire bounding
138 // box since this may be wasteful of space in high dimensions].
139 // We also store pointers to the 2 children.
140 //----------------------------------------------------------------------
141 
142 class ANNkd_split : public ANNkd_node // splitting node of a kd-tree
143 {
144  int cut_dim; // dim orthogonal to cutting plane
145  ANNcoord cut_val; // location of cutting plane
146  ANNcoord cd_bnds[2]; // lower and upper bounds of
147  // rectangle along cut_dim
148  ANNkd_ptr child[2]; // left and right children
149 public:
150  ANNkd_split( // constructor
151  int cd, // cutting dimension
152  ANNcoord cv, // cutting value
153  ANNcoord lv, ANNcoord hv, // low and high values
154  ANNkd_ptr lc=NULL, ANNkd_ptr hc=NULL) // children
155  {
156  cut_dim = cd; // cutting dimension
157  cut_val = cv; // cutting value
158  cd_bnds[ANN_LO] = lv; // lower bound for rectangle
159  cd_bnds[ANN_HI] = hv; // upper bound for rectangle
160  child[ANN_LO] = lc; // left child
161  child[ANN_HI] = hc; // right child
162  }
163 
164  ~ANNkd_split() // destructor
165  {
166  if (child[ANN_LO]!= NULL && child[ANN_LO]!= KD_TRIVIAL)
167  delete child[ANN_LO];
168  if (child[ANN_HI]!= NULL && child[ANN_HI]!= KD_TRIVIAL)
169  delete child[ANN_HI];
170  }
171 
172  virtual void getStats( // get tree statistics
173  int dim, // dimension of space
174  ANNkdStats &st, // statistics
175  ANNorthRect &bnd_box); // bounding box
176  virtual void print(int level, ostream &out);// print node
177  virtual void dump(ostream &out); // dump node
178 
179  virtual void ann_search(ANNdist); // standard search
180  virtual void ann_pri_search(ANNdist); // priority search
181  virtual void ann_FR_search(ANNdist); // fixed-radius search
182 };
183 
184 //----------------------------------------------------------------------
185 // External entry points
186 //----------------------------------------------------------------------
187 
188 ANNkd_ptr rkd_tree( // recursive construction of kd-tree
189  ANNpointArray pa, // point array (unaltered)
190  ANNidxArray pidx, // point indices to store in subtree
191  int n, // number of points
192  int dim, // dimension of space
193  int bsp, // bucket space
194  ANNorthRect &bnd_box, // bounding box for current node
195  ANNkd_splitter splitter); // splitting routine
196 
197 #endif
ANNkd_leaf::ANNkd_leaf
ANNkd_leaf(int n, ANNidxArray b)
Definition: kd_tree.h:96
ANNkd_leaf
Definition: kd_tree.h:91
ANNkd_split
Definition: kd_tree.h:142
ANNkd_leaf::bkt
ANNidxArray bkt
Definition: kd_tree.h:94
ANNkd_leaf::n_pts
int n_pts
Definition: kd_tree.h:93
ANNcoord
double ANNcoord
Definition: ANN.h:158
print
ROSCONSOLE_DECL void print(FilterBase *filter, void *logger, Level level, const char *file, int line, const char *function, const char *fmt,...) ROSCONSOLE_PRINTF_ATTRIBUTE(7
ANNkd_split::cut_dim
int cut_dim
Definition: kd_tree.h:144
ANNkd_node
Definition: kd_tree.h:46
dim
int dim
Definition: ann2fig.cpp:81
ANNpointArray
ANNpoint * ANNpointArray
Definition: ANN.h:376
ANNkd_tree::pidx
ANNidxArray pidx
Definition: ANN.h:711
ANNkd_split::ANNkd_split
ANNkd_split(int cd, ANNcoord cv, ANNcoord lv, ANNcoord hv, ANNkd_ptr lc=NULL, ANNkd_ptr hc=NULL)
Definition: kd_tree.h:150
ANNkd_splitter
void(* ANNkd_splitter)(ANNpointArray pa, ANNidxArray pidx, const ANNorthRect &bnds, int n, int dim, int &cut_dim, ANNcoord &cut_val, int &n_lo)
Definition: kd_tree.h:72
ANNidxArray
ANNidx * ANNidxArray
Definition: ANN.h:378
ANN_HI
Definition: ANNx.h:45
ANNkd_node::~ANNkd_node
virtual ~ANNkd_node()
Definition: kd_tree.h:48
ANNkd_split::cut_val
ANNcoord cut_val
Definition: kd_tree.h:145
rkd_tree
ANNkd_ptr rkd_tree(ANNpointArray pa, ANNidxArray pidx, int n, int dim, int bsp, ANNorthRect &bnd_box, ANNkd_splitter splitter)
Definition: kd_tree.cpp:314
ANN_LO
Definition: ANNx.h:45
ANNkd_split::~ANNkd_split
~ANNkd_split()
Definition: kd_tree.h:164
ANNdist
double ANNdist
Definition: ANN.h:159
ANNkd_leaf::~ANNkd_leaf
~ANNkd_leaf()
Definition: kd_tree.h:104
ANNkd_tree
Definition: ANN.h:705
KD_TRIVIAL
ANNkd_leaf * KD_TRIVIAL
Definition: kd_tree.cpp:50

addwa_local_planner
Author(s): Xie Fusheng
autogenerated on Mon Jun 10 2019 15:52:59