Statistics
| Branch: | Revision:

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 kd-tree fixed-radius kNN search
5
// Last modified:        05/03/05 (Version 1.1)
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 1.1  05/03/05
22
//                Initial release
23
//----------------------------------------------------------------------
24

    
25
#include "kd_fix_rad_search.h"                        // kd fixed-radius search decls
26

    
27
//----------------------------------------------------------------------
28
//        Approximate fixed-radius 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 kd-tree 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 k-th 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 fixed-radius 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 k-th 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 self-match 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
}