Statistics
| Branch: | Revision:

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
}