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 kd-trees
|
5 |
// Last modified: 01/04/05 (Version 1.0)
|
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 |
//----------------------------------------------------------------------
|
24 |
|
25 |
#include "kd_pr_search.h" // kd priority search declarations |
26 |
|
27 |
//----------------------------------------------------------------------
|
28 |
// Approximate nearest neighbor searching by priority search.
|
29 |
// The kd-tree 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 kd-tree 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 |
// 381--390.)
|
38 |
//
|
39 |
// The cell of the kd-tree 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 kd-tree. 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 kd-tree 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 k-th 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 k-th closest point |
187 |
register ANNcoord t;
|
188 |
register int d; |
189 |
|
190 |
min_dist = ANNprPointMK->max_key(); // k-th 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 k-th 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 self-match 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 |
} |