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


2 
// File: kd_fix_rad_search.cpp

3 
// Programmer: Sunil Arya and David Mount

4 
// Description: Standard kdtree fixedradius kNN search

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 1.1 05/03/05

22 
// Initial release

23 
//

24  
25 
#include "kd_fix_rad_search.h" // kd fixedradius search decls 
26  
27 
//

28 
// Approximate fixedradius k nearest neighbor search

29 
// The squared radius is provided, and this procedure finds the

30 
// k nearest neighbors within the radius, and returns the total

31 
// number of points lying within the radius.

32 
//

33 
// The method used for searching the kdtree is a variation of the

34 
// nearest neighbor search used in kd_search.cpp, except that the

35 
// radius of the search ball is known. We refer the reader to that

36 
// file for the explanation of the recursive search procedure.

37 
//

38  
39 
//

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

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

42 
// These are given below.

43 
//

44  
45 
int ANNkdFRDim; // dimension of space 
46 
ANNpoint ANNkdFRQ; // query point

47 
ANNdist ANNkdFRSqRad; // squared radius search bound

48 
double ANNkdFRMaxErr; // max tolerable squared error 
49 
ANNpointArray ANNkdFRPts; // the points

50 
ANNmin_k* ANNkdFRPointMK; // set of k closest points

51 
int ANNkdFRPtsVisited; // total points visited 
52 
int ANNkdFRPtsInRange; // number of points in the range 
53  
54 
//

55 
// annkFRSearch  fixed radius search for k nearest neighbors

56 
//

57  
58 
int ANNkd_tree::annkFRSearch(

59 
ANNpoint q, // the query point

60 
ANNdist sqRad, // squared radius search bound

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

63 
ANNdistArray dd, // the approximate nearest neighbor

64 
double eps) // the error bound 
65 
{ 
66 
ANNkdFRDim = dim; // copy arguments to static equivs

67 
ANNkdFRQ = q; 
68 
ANNkdFRSqRad = sqRad; 
69 
ANNkdFRPts = pts; 
70 
ANNkdFRPtsVisited = 0; // initialize count of points visited 
71 
ANNkdFRPtsInRange = 0; // ...and points in the range 
72  
73 
ANNkdFRMaxErr = ANN_POW(1.0 + eps); 
74 
ANN_FLOP(2) // increment floating op count 
75  
76 
ANNkdFRPointMK = new ANNmin_k(k); // create set for closest k points 
77 
// search starting at the root

78 
root>ann_FR_search(annBoxDistance(q, bnd_box_lo, bnd_box_hi, dim)); 
79  
80 
for (int i = 0; i < k; i++) { // extract the kth closest points 
81 
if (dd != NULL) 
82 
dd[i] = ANNkdFRPointMK>ith_smallest_key(i); 
83 
if (nn_idx != NULL) 
84 
nn_idx[i] = ANNkdFRPointMK>ith_smallest_info(i); 
85 
} 
86  
87 
delete ANNkdFRPointMK; // deallocate closest point set 
88 
return ANNkdFRPtsInRange; // return final point count 
89 
} 
90  
91 
//

92 
// kd_split::ann_FR_search  search a splitting node

93 
// Note: This routine is similar in structure to the standard kNN

94 
// search. It visits the subtree that is closer to the query point

95 
// first. For fixedradius search, there is no benefit in visiting

96 
// one subtree before the other, but we maintain the same basic

97 
// code structure for the sake of uniformity.

98 
//

99  
100 
void ANNkd_split::ann_FR_search(ANNdist box_dist)

101 
{ 
102 
// check dist calc term condition

103 
if (ANNmaxPtsVisited != 0 && ANNkdFRPtsVisited > ANNmaxPtsVisited) return; 
104  
105 
// distance to cutting plane

106 
ANNcoord cut_diff = ANNkdFRQ[cut_dim]  cut_val; 
107  
108 
if (cut_diff < 0) { // left of cutting plane 
109 
child[ANN_LO]>ann_FR_search(box_dist);// visit closer child first

110  
111 
ANNcoord box_diff = cd_bnds[ANN_LO]  ANNkdFRQ[cut_dim]; 
112 
if (box_diff < 0) // within bounds  ignore 
113 
box_diff = 0;

114 
// distance to further box

115 
box_dist = (ANNdist) ANN_SUM(box_dist, 
116 
ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); 
117  
118 
// visit further child if in range

119 
if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad)

120 
child[ANN_HI]>ann_FR_search(box_dist); 
121  
122 
} 
123 
else { // right of cutting plane 
124 
child[ANN_HI]>ann_FR_search(box_dist);// visit closer child first

125  
126 
ANNcoord box_diff = ANNkdFRQ[cut_dim]  cd_bnds[ANN_HI]; 
127 
if (box_diff < 0) // within bounds  ignore 
128 
box_diff = 0;

129 
// distance to further box

130 
box_dist = (ANNdist) ANN_SUM(box_dist, 
131 
ANN_DIFF(ANN_POW(box_diff), ANN_POW(cut_diff))); 
132  
133 
// visit further child if close enough

134 
if (box_dist * ANNkdFRMaxErr <= ANNkdFRSqRad)

135 
child[ANN_LO]>ann_FR_search(box_dist); 
136  
137 
} 
138 
ANN_FLOP(13) // increment floating ops 
139 
ANN_SPL(1) // one more splitting node visited 
140 
} 
141  
142 
//

143 
// kd_leaf::ann_FR_search  search points in a leaf node

144 
// Note: The unreadability of this code is the result of

145 
// some fine tuning to replace indexing by pointer operations.

146 
//

147  
148 
void ANNkd_leaf::ann_FR_search(ANNdist box_dist)

149 
{ 
150 
register ANNdist dist; // distance to data point 
151 
register ANNcoord* pp; // data coordinate pointer 
152 
register ANNcoord* qq; // query coordinate pointer 
153 
register ANNcoord t;

154 
register int d; 
155  
156 
for (int i = 0; i < n_pts; i++) { // check points in bucket 
157  
158 
pp = ANNkdFRPts[bkt[i]]; // first coord of next data point

159 
qq = ANNkdFRQ; // first coord of query point

160 
dist = 0;

161  
162 
for(d = 0; d < ANNkdFRDim; d++) { 
163 
ANN_COORD(1) // one more coordinate hit 
164 
ANN_FLOP(5) // increment floating ops 
165  
166 
t = *(qq++)  *(pp++); // compute length and adv coordinate

167 
// exceeds dist to kth smallest?

168 
if( (dist = ANN_SUM(dist, ANN_POW(t))) > ANNkdFRSqRad) {

169 
break;

170 
} 
171 
} 
172  
173 
if (d >= ANNkdFRDim && // among the k best? 
174 
(ANN_ALLOW_SELF_MATCH  dist!=0)) { // and no selfmatch problem 
175 
// add it to the list

176 
ANNkdFRPointMK>insert(dist, bkt[i]); 
177 
ANNkdFRPtsInRange++; // increment point count

178 
} 
179 
} 
180 
ANN_LEAF(1) // one more leaf node visited 
181 
ANN_PTS(n_pts) // increment points visited

182 
ANNkdFRPtsVisited += n_pts; // increment number of points visited

183 
} 