root / rgbdslam / gicp / ann_1.1.1 / src / kd_pr_search.cpp @ 9240aaa3
History  View  Annotate  Download (8.7 KB)
1 
//


2 
// File: kd_pr_search.cpp

3 
// Programmer: Sunil Arya and David Mount

4 
// Description: Priority search for kdtrees

5 
// Last modified: 01/04/05 (Version 1.0)

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 
//

24  
25 
#include "kd_pr_search.h" // kd priority search declarations 
26  
27 
//

28 
// Approximate nearest neighbor searching by priority search.

29 
// The kdtree is searched for an approximate nearest neighbor.

30 
// The point is returned through one of the arguments, and the

31 
// distance returned is the SQUARED distance to this point.

32 
//

33 
// The method used for searching the kdtree is called priority

34 
// search. (It is described in Arya and Mount, ``Algorithms for

35 
// fast vector quantization,'' Proc. of DCC '93: Data Compression

36 
// Conference}, eds. J. A. Storer and M. Cohn, IEEE Press, 1993,

37 
// 381390.)

38 
//

39 
// The cell of the kdtree containing the query point is located,

40 
// and cells are visited in increasing order of distance from the

41 
// query point. This is done by placing each subtree which has

42 
// NOT been visited in a priority queue, according to the closest

43 
// distance of the corresponding enclosing rectangle from the

44 
// query point. The search stops when the distance to the nearest

45 
// remaining rectangle exceeds the distance to the nearest point

46 
// seen by a factor of more than 1/(1+eps). (Implying that any

47 
// point found subsequently in the search cannot be closer by more

48 
// than this factor.)

49 
//

50 
// The main entry point is annkPriSearch() which sets things up and

51 
// then call the recursive routine ann_pri_search(). This is a

52 
// recursive routine which performs the processing for one node in

53 
// the kdtree. There are two versions of this virtual procedure,

54 
// one for splitting nodes and one for leaves. When a splitting node

55 
// is visited, we determine which child to continue the search on

56 
// (the closer one), and insert the other child into the priority

57 
// queue. When a leaf is visited, we compute the distances to the

58 
// points in the buckets, and update information on the closest

59 
// points.

60 
//

61 
// Some trickery is used to incrementally update the distance from

62 
// a kdtree rectangle to the query point. This comes about from

63 
// the fact that which each successive split, only one component

64 
// (along the dimension that is split) of the squared distance to

65 
// the child rectangle is different from the squared distance to

66 
// the parent rectangle.

67 
//

68  
69 
//

70 
// To keep argument lists short, a number of global variables

71 
// are maintained which are common to all the recursive calls.

72 
// These are given below.

73 
//

74  
75 
double ANNprEps; // the error bound 
76 
int ANNprDim; // dimension of space 
77 
ANNpoint ANNprQ; // query point

78 
double ANNprMaxErr; // max tolerable squared error 
79 
ANNpointArray ANNprPts; // the points

80 
ANNpr_queue *ANNprBoxPQ; // priority queue for boxes

81 
ANNmin_k *ANNprPointMK; // set of k closest points

82  
83 
//

84 
// annkPriSearch  priority search for k nearest neighbors

85 
//

86  
87 
void ANNkd_tree::annkPriSearch(

88 
ANNpoint q, // query point

89 
int k, // number of near neighbors to return 
90 
ANNidxArray nn_idx, // nearest neighbor indices (returned)

91 
ANNdistArray dd, // dist to near neighbors (returned)

92 
double eps) // error bound (ignored) 
93 
{ 
94 
// max tolerable squared error

95 
ANNprMaxErr = ANN_POW(1.0 + eps); 
96 
ANN_FLOP(2) // increment floating ops 
97  
98 
ANNprDim = dim; // copy arguments to static equivs

99 
ANNprQ = q; 
100 
ANNprPts = pts; 
101 
ANNptsVisited = 0; // initialize count of points visited 
102  
103 
ANNprPointMK = new ANNmin_k(k); // create set for closest k points 
104  
105 
// distance to root box

106 
ANNdist box_dist = annBoxDistance(q, 
107 
bnd_box_lo, bnd_box_hi, dim); 
108  
109 
ANNprBoxPQ = new ANNpr_queue(n_pts);// create priority queue for boxes 
110 
ANNprBoxPQ>insert(box_dist, root); // insert root in priority queue

111  
112 
while (ANNprBoxPQ>non_empty() &&

113 
(!(ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited))) {

114 
ANNkd_ptr np; // next box from prior queue

115  
116 
// extract closest box from queue

117 
ANNprBoxPQ>extr_min(box_dist, (void *&) np);

118  
119 
ANN_FLOP(2) // increment floating ops 
120 
if (box_dist*ANNprMaxErr >= ANNprPointMK>max_key())

121 
break;

122  
123 
np>ann_pri_search(box_dist); // search this subtree.

124 
} 
125  
126 
for (int i = 0; i < k; i++) { // extract the kth closest points 
127 
dd[i] = ANNprPointMK>ith_smallest_key(i); 
128 
nn_idx[i] = ANNprPointMK>ith_smallest_info(i); 
129 
} 
130  
131 
delete ANNprPointMK; // deallocate closest point set 
132 
delete ANNprBoxPQ; // deallocate priority queue 
133 
} 
134  
135 
//

136 
// kd_split::ann_pri_search  search a splitting node

137 
//

138  
139 
void ANNkd_split::ann_pri_search(ANNdist box_dist)

140 
{ 
141 
ANNdist new_dist; // distance to child visited later

142 
// distance to cutting plane

143 
ANNcoord cut_diff = ANNprQ[cut_dim]  cut_val; 
144  
145 
if (cut_diff < 0) { // left of cutting plane 
146 
ANNcoord box_diff = cd_bnds[ANN_LO]  ANNprQ[cut_dim]; 
147 
if (box_diff < 0) // within bounds  ignore 
148 
box_diff = 0;

149 
// distance to further box

150 
new_dist = (ANNdist) ANN_SUM(box_dist, 
151 
ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); 
152  
153 
if (child[ANN_HI] != KD_TRIVIAL)// enqueue if not trivial 
154 
ANNprBoxPQ>insert(new_dist, child[ANN_HI]); 
155 
// continue with closer child

156 
child[ANN_LO]>ann_pri_search(box_dist); 
157 
} 
158 
else { // right of cutting plane 
159 
ANNcoord box_diff = ANNprQ[cut_dim]  cd_bnds[ANN_HI]; 
160 
if (box_diff < 0) // within bounds  ignore 
161 
box_diff = 0;

162 
// distance to further box

163 
new_dist = (ANNdist) ANN_SUM(box_dist, 
164 
ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); 
165  
166 
if (child[ANN_LO] != KD_TRIVIAL)// enqueue if not trivial 
167 
ANNprBoxPQ>insert(new_dist, child[ANN_LO]); 
168 
// continue with closer child

169 
child[ANN_HI]>ann_pri_search(box_dist); 
170 
} 
171 
ANN_SPL(1) // one more splitting node visited 
172 
ANN_FLOP(8) // increment floating ops 
173 
} 
174  
175 
//

176 
// kd_leaf::ann_pri_search  search points in a leaf node

177 
//

178 
// This is virtually identical to the ann_search for standard search.

179 
//

180  
181 
void ANNkd_leaf::ann_pri_search(ANNdist box_dist)

182 
{ 
183 
register ANNdist dist; // distance to data point 
184 
register ANNcoord* pp; // data coordinate pointer 
185 
register ANNcoord* qq; // query coordinate pointer 
186 
register ANNdist min_dist; // distance to kth closest point 
187 
register ANNcoord t;

188 
register int d; 
189  
190 
min_dist = ANNprPointMK>max_key(); // kth smallest distance so far

191  
192 
for (int i = 0; i < n_pts; i++) { // check points in bucket 
193  
194 
pp = ANNprPts[bkt[i]]; // first coord of next data point

195 
qq = ANNprQ; // first coord of query point

196 
dist = 0;

197  
198 
for(d = 0; d < ANNprDim; d++) { 
199 
ANN_COORD(1) // one more coordinate hit 
200 
ANN_FLOP(4) // increment floating ops 
201  
202 
t = *(qq++)  *(pp++); // compute length and adv coordinate

203 
// exceeds dist to kth smallest?

204 
if( (dist = ANN_SUM(dist, ANN_POW(t))) > min_dist) {

205 
break;

206 
} 
207 
} 
208  
209 
if (d >= ANNprDim && // among the k best? 
210 
(ANN_ALLOW_SELF_MATCH  dist!=0)) { // and no selfmatch problem 
211 
// add it to the list

212 
ANNprPointMK>insert(dist, bkt[i]); 
213 
min_dist = ANNprPointMK>max_key(); 
214 
} 
215 
} 
216 
ANN_LEAF(1) // one more leaf node visited 
217 
ANN_PTS(n_pts) // increment points visited

218 
ANNptsVisited += n_pts; // increment number of points visited

219 
} 