root / rgbdslam / gicp / ann_1.1.1 / src / kd_tree.h @ 9240aaa3
History  View  Annotate  Download (7.8 KB)
1 
//


2 
// File: kd_tree.h

3 
// Programmer: Sunil Arya and David Mount

4 
// Description: Declarations for standard kdtree routines

5 
// Last modified: 05/03/05 (Version 1.1)

6 
//

7 
// Copyright (c) 19972005 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 kdtree node

36 
//

37 
// Nodes in kdtrees 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 kdtree 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; // fixedradius 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 kdtree to access us

63 
}; 
64  
65 
//

66 
// kdsplitting 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 kdtrees 
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 kdtree node

84 
// Leaf nodes of the kdtree 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 kdtree. 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 kdtree

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); // fixedradius 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 kdtree 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 
// kdtree splitting node.

133 
// Splitting nodes contain a cutting dimension and a cutting value.

134 
// These indicate the axisparellel 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 kdtree

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); // fixedradius search 
182 
}; 
183  
184 
//

185 
// External entry points

186 
//

187  
188 
ANNkd_ptr rkd_tree( // recursive construction of kdtree

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
