Statistics
| Branch: | Revision:

root / rgbdslam / gicp / ann_1.1.1 / src / kd_tree.cpp @ 9240aaa3

History | View | Annotate | Download (14.9 KB)

1
//----------------------------------------------------------------------
2
// File:                        kd_tree.cpp
3
// Programmer:                Sunil Arya and David Mount
4
// Description:                Basic methods 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
//        Revision 1.0  04/01/05
24
//                Increased aspect ratio bound (ANN_AR_TOOBIG) from 100 to 1000.
25
//                Fixed leaf counts to count trivial leaves.
26
//                Added optional pa, pi arguments to Skeleton kd_tree constructor
27
//                        for use in load constructor.
28
//                Added annClose() to eliminate KD_TRIVIAL memory leak.
29
//----------------------------------------------------------------------
30

    
31
#include "kd_tree.h"                                        // kd-tree declarations
32
#include "kd_split.h"                                        // kd-tree splitting rules
33
#include "kd_util.h"                                        // kd-tree utilities
34
#include <ANN/ANNperf.h>                                // performance evaluation
35

    
36
//----------------------------------------------------------------------
37
//        Global data
38
//
39
//        For some splitting rules, especially with small bucket sizes,
40
//        it is possible to generate a large number of empty leaf nodes.
41
//        To save storage we allocate a single trivial leaf node which
42
//        contains no points.  For messy coding reasons it is convenient
43
//        to have it reference a trivial point index.
44
//
45
//        KD_TRIVIAL is allocated when the first kd-tree is created.  It
46
//        must *never* deallocated (since it may be shared by more than
47
//        one tree).
48
//----------------------------------------------------------------------
49
static int                                IDX_TRIVIAL[] = {0};        // trivial point index
50
ANNkd_leaf                                *KD_TRIVIAL = NULL;                // trivial leaf node
51

    
52
//----------------------------------------------------------------------
53
//        Printing the kd-tree 
54
//                These routines print a kd-tree in reverse inorder (high then
55
//                root then low).  (This is so that if you look at the output
56
//                from the right side it appear from left to right in standard
57
//                inorder.)  When outputting leaves we output only the point
58
//                indices rather than the point coordinates. There is an option
59
//                to print the point coordinates separately.
60
//
61
//                The tree printing routine calls the printing routines on the
62
//                individual nodes of the tree, passing in the level or depth
63
//                in the tree.  The level in the tree is used to print indentation
64
//                for readability.
65
//----------------------------------------------------------------------
66

    
67
void ANNkd_split::print(                                // print splitting node
68
                int level,                                                // depth of node in tree
69
                ostream &out)                                        // output stream
70
{
71
        child[ANN_HI]->print(level+1, out);        // print high child
72
        out << "    ";
73
        for (int i = 0; i < level; i++)                // print indentation
74
                out << "..";
75
        out << "Split cd=" << cut_dim << " cv=" << cut_val;
76
        out << " lbnd=" << cd_bnds[ANN_LO];
77
        out << " hbnd=" << cd_bnds[ANN_HI];
78
        out << "\n";
79
        child[ANN_LO]->print(level+1, out);        // print low child
80
}
81

    
82
void ANNkd_leaf::print(                                        // print leaf node
83
                int level,                                                // depth of node in tree
84
                ostream &out)                                        // output stream
85
{
86

    
87
        out << "    ";
88
        for (int i = 0; i < level; i++)                // print indentation
89
                out << "..";
90

    
91
        if (this == KD_TRIVIAL) {                        // canonical trivial leaf node
92
                out << "Leaf (trivial)\n";
93
        }
94
        else{
95
                out << "Leaf n=" << n_pts << " <";
96
                for (int j = 0; j < n_pts; j++) {
97
                        out << bkt[j];
98
                        if (j < n_pts-1) out << ",";
99
                }
100
                out << ">\n";
101
        }
102
}
103

    
104
void ANNkd_tree::Print(                                        // print entire tree
105
                ANNbool with_pts,                                // print points as well?
106
                ostream &out)                                        // output stream
107
{
108
        out << "ANN Version " << ANNversion << "\n";
109
        if (with_pts) {                                                // print point coordinates
110
                out << "    Points:\n";
111
                for (int i = 0; i < n_pts; i++) {
112
                        out << "\t" << i << ": ";
113
                        annPrintPt(pts[i], dim, out);
114
                        out << "\n";
115
                }
116
        }
117
        if (root == NULL)                                        // empty tree?
118
                out << "    Null tree.\n";
119
        else {
120
                root->print(0, out);                        // invoke printing at root
121
        }
122
}
123

    
124
//----------------------------------------------------------------------
125
//        kd_tree statistics (for performance evaluation)
126
//                This routine compute various statistics information for
127
//                a kd-tree.  It is used by the implementors for performance
128
//                evaluation of the data structure.
129
//----------------------------------------------------------------------
130

    
131
#define MAX(a,b)                ((a) > (b) ? (a) : (b))
132

    
133
void ANNkdStats::merge(const ANNkdStats &st)        // merge stats from child 
134
{
135
        n_lf += st.n_lf;                        n_tl += st.n_tl;
136
        n_spl += st.n_spl;                        n_shr += st.n_shr;
137
        depth = MAX(depth, st.depth);
138
        sum_ar += st.sum_ar;
139
}
140

    
141
//----------------------------------------------------------------------
142
//        Update statistics for nodes
143
//----------------------------------------------------------------------
144

    
145
const double ANN_AR_TOOBIG = 1000;                                // too big an aspect ratio
146

    
147
void ANNkd_leaf::getStats(                                                // get subtree statistics
148
        int                                        dim,                                        // dimension of space
149
        ANNkdStats                        &st,                                        // stats (modified)
150
        ANNorthRect                        &bnd_box)                                // bounding box
151
{
152
        st.reset();
153
        st.n_lf = 1;                                                                // count this leaf
154
        if (this == KD_TRIVIAL) st.n_tl = 1;                // count trivial leaf
155
        double ar = annAspectRatio(dim, bnd_box);        // aspect ratio of leaf
156
                                                                                                // incr sum (ignore outliers)
157
        st.sum_ar += float(ar < ANN_AR_TOOBIG ? ar : ANN_AR_TOOBIG);
158
}
159

    
160
void ANNkd_split::getStats(                                                // get subtree statistics
161
        int                                        dim,                                        // dimension of space
162
        ANNkdStats                        &st,                                        // stats (modified)
163
        ANNorthRect                        &bnd_box)                                // bounding box
164
{
165
        ANNkdStats ch_stats;                                                // stats for children
166
                                                                                                // get stats for low child
167
        ANNcoord hv = bnd_box.hi[cut_dim];                        // save box bounds
168
        bnd_box.hi[cut_dim] = cut_val;                                // upper bound for low child
169
        ch_stats.reset();                                                        // reset
170
        child[ANN_LO]->getStats(dim, ch_stats, bnd_box);
171
        st.merge(ch_stats);                                                        // merge them
172
        bnd_box.hi[cut_dim] = hv;                                        // restore bound
173
                                                                                                // get stats for high child
174
        ANNcoord lv = bnd_box.lo[cut_dim];                        // save box bounds
175
        bnd_box.lo[cut_dim] = cut_val;                                // lower bound for high child
176
        ch_stats.reset();                                                        // reset
177
        child[ANN_HI]->getStats(dim, ch_stats, bnd_box);
178
        st.merge(ch_stats);                                                        // merge them
179
        bnd_box.lo[cut_dim] = lv;                                        // restore bound
180

    
181
        st.depth++;                                                                        // increment depth
182
        st.n_spl++;                                                                        // increment number of splits
183
}
184

    
185
//----------------------------------------------------------------------
186
//        getStats
187
//                Collects a number of statistics related to kd_tree or
188
//                bd_tree.
189
//----------------------------------------------------------------------
190

    
191
void ANNkd_tree::getStats(                                                // get tree statistics
192
        ANNkdStats                        &st)                                        // stats (modified)
193
{
194
        st.reset(dim, n_pts, bkt_size);                                // reset stats
195
                                                                                                // create bounding box
196
        ANNorthRect bnd_box(dim, bnd_box_lo, bnd_box_hi);
197
        if (root != NULL) {                                                        // if nonempty tree
198
                root->getStats(dim, st, bnd_box);                // get statistics
199
                st.avg_ar = st.sum_ar / st.n_lf;                // average leaf asp ratio
200
        }
201
}
202

    
203
//----------------------------------------------------------------------
204
//        kd_tree destructor
205
//                The destructor just frees the various elements that were
206
//                allocated in the construction process.
207
//----------------------------------------------------------------------
208

    
209
ANNkd_tree::~ANNkd_tree()                                // tree destructor
210
{
211
        if (root != NULL) delete root;
212
        if (pidx != NULL) delete [] pidx;
213
        if (bnd_box_lo != NULL) annDeallocPt(bnd_box_lo);
214
        if (bnd_box_hi != NULL) annDeallocPt(bnd_box_hi);
215
}
216

    
217
//----------------------------------------------------------------------
218
//        This is called with all use of ANN is finished.  It eliminates the
219
//        minor memory leak caused by the allocation of KD_TRIVIAL.
220
//----------------------------------------------------------------------
221
void annClose()                                // close use of ANN
222
{
223
        if (KD_TRIVIAL != NULL) {
224
                delete KD_TRIVIAL;
225
                KD_TRIVIAL = NULL;
226
        }
227
}
228

    
229
//----------------------------------------------------------------------
230
//        kd_tree constructors
231
//                There is a skeleton kd-tree constructor which sets up a
232
//                trivial empty tree.         The last optional argument allows
233
//                the routine to be passed a point index array which is
234
//                assumed to be of the proper size (n).  Otherwise, one is
235
//                allocated and initialized to the identity.        Warning: In
236
//                either case the destructor will deallocate this array.
237
//
238
//                As a kludge, we need to allocate KD_TRIVIAL if one has not
239
//                already been allocated.         (This is because I'm too dumb to
240
//                figure out how to cause a pointer to be allocated at load
241
//                time.)
242
//----------------------------------------------------------------------
243

    
244
void ANNkd_tree::SkeletonTree(                        // construct skeleton tree
245
                int n,                                                        // number of points
246
                int dd,                                                        // dimension
247
                int bs,                                                        // bucket size
248
                ANNpointArray pa,                                // point array
249
                ANNidxArray pi)                                        // point indices
250
{
251
        dim = dd;                                                        // initialize basic elements
252
        n_pts = n;
253
        bkt_size = bs;
254
        pts = pa;                                                        // initialize points array
255

    
256
        root = NULL;                                                // no associated tree yet
257

    
258
        if (pi == NULL) {                                        // point indices provided?
259
                pidx = new ANNidx[n];                        // no, allocate space for point indices
260
                for (int i = 0; i < n; i++) {
261
                        pidx[i] = i;                                // initially identity
262
                }
263
        }
264
        else {
265
                pidx = pi;                                                // yes, use them
266
        }
267

    
268
        bnd_box_lo = bnd_box_hi = NULL;                // bounding box is nonexistent
269
        if (KD_TRIVIAL == NULL)                                // no trivial leaf node yet?
270
                KD_TRIVIAL = new ANNkd_leaf(0, IDX_TRIVIAL);        // allocate it
271
}
272

    
273
ANNkd_tree::ANNkd_tree(                                        // basic constructor
274
                int n,                                                        // number of points
275
                int dd,                                                        // dimension
276
                int bs)                                                        // bucket size
277
{  SkeletonTree(n, dd, bs);  }                        // construct skeleton tree
278

    
279
//----------------------------------------------------------------------
280
//        rkd_tree - recursive procedure to build a kd-tree
281
//
282
//                Builds a kd-tree for points in pa as indexed through the
283
//                array pidx[0..n-1] (typically a subarray of the array used in
284
//                the top-level call).  This routine permutes the array pidx,
285
//                but does not alter pa[].
286
//
287
//                The construction is based on a standard algorithm for constructing
288
//                the kd-tree (see Friedman, Bentley, and Finkel, ``An algorithm for
289
//                finding best matches in logarithmic expected time,'' ACM Transactions
290
//                on Mathematical Software, 3(3):209-226, 1977).  The procedure
291
//                operates by a simple divide-and-conquer strategy, which determines
292
//                an appropriate orthogonal cutting plane (see below), and splits
293
//                the points.  When the number of points falls below the bucket size,
294
//                we simply store the points in a leaf node's bucket.
295
//
296
//                One of the arguments is a pointer to a splitting routine,
297
//                whose prototype is:
298
//                
299
//                                void split(
300
//                                                ANNpointArray pa,  // complete point array
301
//                                                ANNidxArray pidx,  // point array (permuted on return)
302
//                                                ANNorthRect &bnds, // bounds of current cell
303
//                                                int n,                           // number of points
304
//                                                int dim,                   // dimension of space
305
//                                                int &cut_dim,           // cutting dimension
306
//                                                ANNcoord &cut_val, // cutting value
307
//                                                int &n_lo)                   // no. of points on low side of cut
308
//
309
//                This procedure selects a cutting dimension and cutting value,
310
//                partitions pa about these values, and returns the number of
311
//                points on the low side of the cut.
312
//----------------------------------------------------------------------
313

    
314
ANNkd_ptr rkd_tree(                                // recursive construction of kd-tree
315
        ANNpointArray                pa,                                // point array
316
        ANNidxArray                        pidx,                        // point indices to store in subtree
317
        int                                        n,                                // number of points
318
        int                                        dim,                        // dimension of space
319
        int                                        bsp,                        // bucket space
320
        ANNorthRect                        &bnd_box,                // bounding box for current node
321
        ANNkd_splitter                splitter)                // splitting routine
322
{
323
        if (n <= bsp) {                                                // n small, make a leaf node
324
                if (n == 0)                                                // empty leaf node
325
                        return KD_TRIVIAL;                        // return (canonical) empty leaf
326
                else                                                        // construct the node and return
327
                        return new ANNkd_leaf(n, pidx); 
328
        }
329
        else {                                                                // n large, make a splitting node
330
                int cd;                                                        // cutting dimension
331
                ANNcoord cv;                                        // cutting value
332
                int n_lo;                                                // number on low side of cut
333
                ANNkd_node *lo, *hi;                        // low and high children
334

    
335
                                                                                // invoke splitting procedure
336
                (*splitter)(pa, pidx, bnd_box, n, dim, cd, cv, n_lo);
337

    
338
                ANNcoord lv = bnd_box.lo[cd];        // save bounds for cutting dimension
339
                ANNcoord hv = bnd_box.hi[cd];
340

    
341
                bnd_box.hi[cd] = cv;                        // modify bounds for left subtree
342
                lo = rkd_tree(                                        // build left subtree
343
                                pa, pidx, n_lo,                        // ...from pidx[0..n_lo-1]
344
                                dim, bsp, bnd_box, splitter);
345
                bnd_box.hi[cd] = hv;                        // restore bounds
346

    
347
                bnd_box.lo[cd] = cv;                        // modify bounds for right subtree
348
                hi = rkd_tree(                                        // build right subtree
349
                                pa, pidx + n_lo, n-n_lo,// ...from pidx[n_lo..n-1]
350
                                dim, bsp, bnd_box, splitter);
351
                bnd_box.lo[cd] = lv;                        // restore bounds
352

    
353
                                                                                // create the splitting node
354
                ANNkd_split *ptr = new ANNkd_split(cd, cv, lv, hv, lo, hi);
355

    
356
                return ptr;                                                // return pointer to this node
357
        }
358
} 
359

    
360
//----------------------------------------------------------------------
361
// kd-tree constructor
362
//                This is the main constructor for kd-trees given a set of points.
363
//                It first builds a skeleton tree, then computes the bounding box
364
//                of the data points, and then invokes rkd_tree() to actually
365
//                build the tree, passing it the appropriate splitting routine.
366
//----------------------------------------------------------------------
367

    
368
ANNkd_tree::ANNkd_tree(                                        // construct from point array
369
        ANNpointArray                pa,                                // point array (with at least n pts)
370
        int                                        n,                                // number of points
371
        int                                        dd,                                // dimension
372
        int                                        bs,                                // bucket size
373
        ANNsplitRule                split)                        // splitting method
374
{
375
        SkeletonTree(n, dd, bs);                        // set up the basic stuff
376
        pts = pa;                                                        // where the points are
377
        if (n == 0) return;                                        // no points--no sweat
378

    
379
        ANNorthRect bnd_box(dd);                        // bounding box for points
380
        annEnclRect(pa, pidx, n, dd, bnd_box);// construct bounding rectangle
381
                                                                                // copy to tree structure
382
        bnd_box_lo = annCopyPt(dd, bnd_box.lo);
383
        bnd_box_hi = annCopyPt(dd, bnd_box.hi);
384

    
385
        switch (split) {                                        // build by rule
386
        case ANN_KD_STD:                                        // standard kd-splitting rule
387
                root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, kd_split);
388
                break;
389
        case ANN_KD_MIDPT:                                        // midpoint split
390
                root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, midpt_split);
391
                break;
392
        case ANN_KD_FAIR:                                        // fair split
393
                root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, fair_split);
394
                break;
395
        case ANN_KD_SUGGEST:                                // best (in our opinion)
396
        case ANN_KD_SL_MIDPT:                                // sliding midpoint split
397
                root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, sl_midpt_split);
398
                break;
399
        case ANN_KD_SL_FAIR:                                // sliding fair split
400
                root = rkd_tree(pa, pidx, n, dd, bs, bnd_box, sl_fair_split);
401
                break;
402
        default:
403
                annError("Illegal splitting method", ANNabort);
404
        }
405
}