Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (15.8 KB)

1
//----------------------------------------------------------------------
2
// File:                        bd_tree.cpp
3
// Programmer:                David Mount
4
// Description:                Basic methods for bd-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 l.0  04/01/05
24
//                Fixed centroid shrink threshold condition to depend on the
25
//                        dimension.
26
//                Moved dump routine to kd_dump.cpp.
27
//----------------------------------------------------------------------
28

    
29
#include "bd_tree.h"                                        // bd-tree declarations
30
#include "kd_util.h"                                        // kd-tree utilities
31
#include "kd_split.h"                                        // kd-tree splitting rules
32

    
33
#include <ANN/ANNperf.h>                                // performance evaluation
34

    
35
//----------------------------------------------------------------------
36
//        Printing a bd-tree 
37
//                These routines print a bd-tree.   See the analogous procedure
38
//                in kd_tree.cpp for more information.
39
//----------------------------------------------------------------------
40

    
41
void ANNbd_shrink::print(                                // print shrinking node
42
                int level,                                                // depth of node in tree
43
                ostream &out)                                        // output stream
44
{
45
        child[ANN_OUT]->print(level+1, out);                // print out-child
46

    
47
        out << "    ";
48
        for (int i = 0; i < level; i++)                                // print indentation
49
                out << "..";
50
        out << "Shrink";
51
        for (int j = 0; j < n_bnds; j++) {                        // print sides, 2 per line
52
                if (j % 2 == 0) {
53
                        out << "\n";                                                // newline and indentation
54
                        for (int i = 0; i < level+2; i++) out << "  ";
55
                }
56
                out << "  ([" << bnds[j].cd << "]"
57
                         << (bnds[j].sd > 0 ? ">=" : "< ")
58
                         << bnds[j].cv << ")";
59
        }
60
        out << "\n";
61

    
62
        child[ANN_IN]->print(level+1, out);                        // print in-child
63
}
64

    
65
//----------------------------------------------------------------------
66
//        kd_tree statistics utility (for performance evaluation)
67
//                This routine computes various statistics information for
68
//                shrinking nodes.  See file kd_tree.cpp for more information.
69
//----------------------------------------------------------------------
70

    
71
void ANNbd_shrink::getStats(                                        // get subtree statistics
72
        int                                        dim,                                        // dimension of space
73
        ANNkdStats                        &st,                                        // stats (modified)
74
        ANNorthRect                        &bnd_box)                                // bounding box
75
{
76
        ANNkdStats ch_stats;                                                // stats for children
77
        ANNorthRect inner_box(dim);                                        // inner box of shrink
78

    
79
        annBnds2Box(bnd_box,                                                // enclosing box
80
                                dim,                                                        // dimension
81
                                n_bnds,                                                        // number of bounds
82
                                bnds,                                                        // bounds array
83
                                inner_box);                                                // inner box (modified)
84
                                                                                                // get stats for inner child
85
        ch_stats.reset();                                                        // reset
86
        child[ANN_IN]->getStats(dim, ch_stats, inner_box);
87
        st.merge(ch_stats);                                                        // merge them
88
                                                                                                // get stats for outer child
89
        ch_stats.reset();                                                        // reset
90
        child[ANN_OUT]->getStats(dim, ch_stats, bnd_box);
91
        st.merge(ch_stats);                                                        // merge them
92

    
93
        st.depth++;                                                                        // increment depth
94
        st.n_shr++;                                                                        // increment number of shrinks
95
}
96

    
97
//----------------------------------------------------------------------
98
// bd-tree constructor
99
//                This is the main constructor for bd-trees given a set of points.
100
//                It first builds a skeleton kd-tree as a basis, then computes the
101
//                bounding box of the data points, and then invokes rbd_tree() to
102
//                actually build the tree, passing it the appropriate splitting
103
//                and shrinking information.
104
//----------------------------------------------------------------------
105

    
106
ANNkd_ptr rbd_tree(                                                // recursive construction of bd-tree
107
        ANNpointArray                pa,                                // point array
108
        ANNidxArray                        pidx,                        // point indices to store in subtree
109
        int                                        n,                                // number of points
110
        int                                        dim,                        // dimension of space
111
        int                                        bsp,                        // bucket space
112
        ANNorthRect                        &bnd_box,                // bounding box for current node
113
        ANNkd_splitter                splitter,                // splitting routine
114
        ANNshrinkRule                shrink);                // shrinking rule
115

    
116
ANNbd_tree::ANNbd_tree(                                        // construct from point array
117
        ANNpointArray                pa,                                // point array (with at least n pts)
118
        int                                        n,                                // number of points
119
        int                                        dd,                                // dimension
120
        int                                        bs,                                // bucket size
121
        ANNsplitRule                split,                        // splitting rule
122
        ANNshrinkRule                shrink)                        // shrinking rule
123
        : ANNkd_tree(n, dd, bs)                                // build skeleton base tree
124
{
125
        pts = pa;                                                        // where the points are
126
        if (n == 0) return;                                        // no points--no sweat
127

    
128
        ANNorthRect bnd_box(dd);                        // bounding box for points
129
                                                                                // construct bounding rectangle
130
        annEnclRect(pa, pidx, n, dd, bnd_box);
131
                                                                                // copy to tree structure
132
        bnd_box_lo = annCopyPt(dd, bnd_box.lo);
133
        bnd_box_hi = annCopyPt(dd, bnd_box.hi);
134

    
135
        switch (split) {                                        // build by rule
136
        case ANN_KD_STD:                                        // standard kd-splitting rule
137
                root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, kd_split, shrink);
138
                break;
139
        case ANN_KD_MIDPT:                                        // midpoint split
140
                root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, midpt_split, shrink);
141
                break;
142
        case ANN_KD_SUGGEST:                                // best (in our opinion)
143
        case ANN_KD_SL_MIDPT:                                // sliding midpoint split
144
                root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, sl_midpt_split, shrink);
145
                break;
146
        case ANN_KD_FAIR:                                        // fair split
147
                root = rbd_tree(pa, pidx, n, dd, bs, bnd_box, fair_split, shrink);
148
                break;
149
        case ANN_KD_SL_FAIR:                                // sliding fair split
150
                root = rbd_tree(pa, pidx, n, dd, bs,
151
                                                bnd_box, sl_fair_split, shrink);
152
                break;
153
        default:
154
                annError("Illegal splitting method", ANNabort);
155
        }
156
}
157

    
158
//----------------------------------------------------------------------
159
//        Shrinking rules
160
//----------------------------------------------------------------------
161

    
162
enum ANNdecomp {SPLIT, SHRINK};                        // decomposition methods
163

    
164
//----------------------------------------------------------------------
165
//        trySimpleShrink - Attempt a simple shrink
166
//
167
//                We compute the tight bounding box of the points, and compute
168
//                the 2*dim ``gaps'' between the sides of the tight box and the
169
//                bounding box.  If any of the gaps is large enough relative to
170
//                the longest side of the tight bounding box, then we shrink
171
//                all sides whose gaps are large enough.  (The reason for
172
//                comparing against the tight bounding box, is that after
173
//                shrinking the longest box size will decrease, and if we use
174
//                the standard bounding box, we may decide to shrink twice in
175
//                a row.  Since the tight box is fixed, we cannot shrink twice
176
//                consecutively.)
177
//----------------------------------------------------------------------
178
const float BD_GAP_THRESH = 0.5;                // gap threshold (must be < 1)
179
const int   BD_CT_THRESH  = 2;                        // min number of shrink sides
180

    
181
ANNdecomp trySimpleShrink(                                // try a simple shrink
182
        ANNpointArray                pa,                                // point array
183
        ANNidxArray                        pidx,                        // point indices to store in subtree
184
        int                                        n,                                // number of points
185
        int                                        dim,                        // dimension of space
186
        const ANNorthRect        &bnd_box,                // current bounding box
187
        ANNorthRect                        &inner_box)                // inner box if shrinking (returned)
188
{
189
        int i;
190
                                                                                                // compute tight bounding box
191
        annEnclRect(pa, pidx, n, dim, inner_box);
192

    
193
        ANNcoord max_length = 0;                                        // find longest box side
194
        for (i = 0; i < dim; i++) {
195
                ANNcoord length = inner_box.hi[i] - inner_box.lo[i];
196
                if (length > max_length) {
197
                        max_length = length;
198
                }
199
        }
200

    
201
        int shrink_ct = 0;                                                        // number of sides we shrunk
202
        for (i = 0; i < dim; i++) {                                        // select which sides to shrink
203
                                                                                                // gap between boxes
204
                ANNcoord gap_hi = bnd_box.hi[i] - inner_box.hi[i];
205
                                                                                                // big enough gap to shrink?
206
                if (gap_hi < max_length*BD_GAP_THRESH)
207
                        inner_box.hi[i] = bnd_box.hi[i];        // no - expand
208
                else shrink_ct++;                                                // yes - shrink this side
209

    
210
                                                                                                // repeat for high side
211
                ANNcoord gap_lo = inner_box.lo[i] - bnd_box.lo[i];
212
                if (gap_lo < max_length*BD_GAP_THRESH)
213
                        inner_box.lo[i] = bnd_box.lo[i];        // no - expand
214
                else shrink_ct++;                                                // yes - shrink this side
215
        }
216

    
217
        if (shrink_ct >= BD_CT_THRESH)                                // did we shrink enough sides?
218
                 return SHRINK;
219
        else return SPLIT;
220
}
221

    
222
//----------------------------------------------------------------------
223
//        tryCentroidShrink - Attempt a centroid shrink
224
//
225
//        We repeatedly apply the splitting rule, always to the larger subset
226
//        of points, until the number of points decreases by the constant
227
//        fraction BD_FRACTION.  If this takes more than dim*BD_MAX_SPLIT_FAC
228
//        splits for this to happen, then we shrink to the final inner box
229
//        Otherwise we split.
230
//----------------------------------------------------------------------
231

    
232
const float        BD_MAX_SPLIT_FAC = 0.5;                // maximum number of splits allowed
233
const float        BD_FRACTION = 0.5;                        // ...to reduce points by this fraction
234
                                                                                // ...This must be < 1.
235

    
236
ANNdecomp tryCentroidShrink(                        // try a centroid shrink
237
        ANNpointArray                pa,                                // point array
238
        ANNidxArray                        pidx,                        // point indices to store in subtree
239
        int                                        n,                                // number of points
240
        int                                        dim,                        // dimension of space
241
        const ANNorthRect        &bnd_box,                // current bounding box
242
        ANNkd_splitter                splitter,                // splitting procedure
243
        ANNorthRect                        &inner_box)                // inner box if shrinking (returned)
244
{
245
        int n_sub = n;                                                // number of points in subset
246
        int n_goal = (int) (n*BD_FRACTION); // number of point in goal
247
        int n_splits = 0;                                        // number of splits needed
248
                                                                                // initialize inner box to bounding box
249
        annAssignRect(dim, inner_box, bnd_box);
250

    
251
        while (n_sub > n_goal) {                        // keep splitting until goal reached
252
                int cd;                                                        // cut dim from splitter (ignored)
253
                ANNcoord cv;                                        // cut value from splitter (ignored)
254
                int n_lo;                                                // number of points on low side
255
                                                                                // invoke splitting procedure
256
                (*splitter)(pa, pidx, inner_box, n_sub, dim, cd, cv, n_lo);
257
                n_splits++;                                                // increment split count
258

    
259
                if (n_lo >= n_sub/2) {                        // most points on low side
260
                        inner_box.hi[cd] = cv;                // collapse high side
261
                        n_sub = n_lo;                                // recurse on lower points
262
                }
263
                else {                                                        // most points on high side
264
                        inner_box.lo[cd] = cv;                // collapse low side
265
                        pidx += n_lo;                                // recurse on higher points
266
                        n_sub -= n_lo;
267
                }
268
        }
269
    if (n_splits > dim*BD_MAX_SPLIT_FAC)// took too many splits
270
                return SHRINK;                                        // shrink to final subset
271
        else
272
                return SPLIT;
273
}
274

    
275
//----------------------------------------------------------------------
276
//        selectDecomp - select which decomposition to use
277
//----------------------------------------------------------------------
278

    
279
ANNdecomp selectDecomp(                        // select decomposition method
280
        ANNpointArray                pa,                                // point array
281
        ANNidxArray                        pidx,                        // point indices to store in subtree
282
        int                                        n,                                // number of points
283
        int                                        dim,                        // dimension of space
284
        const ANNorthRect        &bnd_box,                // current bounding box
285
        ANNkd_splitter                splitter,                // splitting procedure
286
        ANNshrinkRule                shrink,                        // shrinking rule
287
        ANNorthRect                        &inner_box)                // inner box if shrinking (returned)
288
{
289
        ANNdecomp decomp = SPLIT;                        // decomposition
290

    
291
        switch (shrink) {                                        // check shrinking rule
292
        case ANN_BD_NONE:                                        // no shrinking allowed
293
                decomp = SPLIT;
294
                break;
295
        case ANN_BD_SUGGEST:                                // author's suggestion
296
        case ANN_BD_SIMPLE:                                        // simple shrink
297
                decomp = trySimpleShrink(
298
                                pa, pidx,                                // points and indices
299
                                n, dim,                                        // number of points and dimension
300
                                bnd_box,                                // current bounding box
301
                                inner_box);                                // inner box if shrinking (returned)
302
                break;
303
        case ANN_BD_CENTROID:                                // centroid shrink
304
                decomp = tryCentroidShrink(
305
                                pa, pidx,                                // points and indices
306
                                n, dim,                                        // number of points and dimension
307
                                bnd_box,                                // current bounding box
308
                                splitter,                                // splitting procedure
309
                                inner_box);                                // inner box if shrinking (returned)
310
                break;
311
        default:
312
                annError("Illegal shrinking rule", ANNabort);
313
        }
314
        return decomp;
315
}
316

    
317
//----------------------------------------------------------------------
318
//        rbd_tree - recursive procedure to build a bd-tree
319
//
320
//                This is analogous to rkd_tree, but for bd-trees.  See the
321
//                procedure rkd_tree() in kd_split.cpp for more information.
322
//
323
//                If the number of points falls below the bucket size, then a
324
//                leaf node is created for the points.  Otherwise we invoke the
325
//                procedure selectDecomp() which determines whether we are to
326
//                split or shrink.  If splitting is chosen, then we essentially
327
//                do exactly as rkd_tree() would, and invoke the specified
328
//                splitting procedure to the points.  Otherwise, the selection
329
//                procedure returns a bounding box, from which we extract the
330
//                appropriate shrinking bounds, and create a shrinking node.
331
//                Finally the points are subdivided, and the procedure is
332
//                invoked recursively on the two subsets to form the children.
333
//----------------------------------------------------------------------
334

    
335
ANNkd_ptr rbd_tree(                                // recursive construction of bd-tree
336
        ANNpointArray                pa,                                // point array
337
        ANNidxArray                        pidx,                        // point indices to store in subtree
338
        int                                        n,                                // number of points
339
        int                                        dim,                        // dimension of space
340
        int                                        bsp,                        // bucket space
341
        ANNorthRect                        &bnd_box,                // bounding box for current node
342
        ANNkd_splitter                splitter,                // splitting routine
343
        ANNshrinkRule                shrink)                        // shrinking rule
344
{
345
        ANNdecomp decomp;                                        // decomposition method
346

    
347
        ANNorthRect inner_box(dim);                        // inner box (if shrinking)
348

    
349
        if (n <= bsp) {                                                // n small, make a leaf node
350
                if (n == 0)                                                // empty leaf node
351
                        return KD_TRIVIAL;                        // return (canonical) empty leaf
352
                else                                                        // construct the node and return
353
                        return new ANNkd_leaf(n, pidx); 
354
        }
355
        
356
        decomp = selectDecomp(                                // select decomposition method
357
                                pa, pidx,                                // points and indices
358
                                n, dim,                                        // number of points and dimension
359
                                bnd_box,                                // current bounding box
360
                                splitter, shrink,                // splitting/shrinking methods
361
                                inner_box);                                // inner box if shrinking (returned)
362
        
363
        if (decomp == SPLIT) {                                // split selected
364
                int cd;                                                        // cutting dimension
365
                ANNcoord cv;                                        // cutting value
366
                int n_lo;                                                // number on low side of cut
367
                                                                                // invoke splitting procedure
368
                (*splitter)(pa, pidx, bnd_box, n, dim, cd, cv, n_lo);
369

    
370
                ANNcoord lv = bnd_box.lo[cd];        // save bounds for cutting dimension
371
                ANNcoord hv = bnd_box.hi[cd];
372

    
373
                bnd_box.hi[cd] = cv;                        // modify bounds for left subtree
374
                ANNkd_ptr lo = rbd_tree(                // build left subtree
375
                                pa, pidx, n_lo,                        // ...from pidx[0..n_lo-1]
376
                                dim, bsp, bnd_box, splitter, shrink);
377
                bnd_box.hi[cd] = hv;                        // restore bounds
378

    
379
                bnd_box.lo[cd] = cv;                        // modify bounds for right subtree
380
                ANNkd_ptr hi = rbd_tree(                // build right subtree
381
                                pa, pidx + n_lo, n-n_lo,// ...from pidx[n_lo..n-1]
382
                                dim, bsp, bnd_box, splitter, shrink);
383
                bnd_box.lo[cd] = lv;                        // restore bounds
384
                                                                                // create the splitting node
385
                return new ANNkd_split(cd, cv, lv, hv, lo, hi);
386
        }
387
        else {                                                                // shrink selected
388
                int n_in;                                                // number of points in box
389
                int n_bnds;                                                // number of bounding sides
390

    
391
                annBoxSplit(                                        // split points around inner box
392
                                pa,                                                // points to split
393
                                pidx,                                        // point indices
394
                                n,                                                // number of points
395
                                dim,                                        // dimension
396
                                inner_box,                                // inner box
397
                                n_in);                                        // number of points inside (returned)
398

    
399
                ANNkd_ptr in = rbd_tree(                // build inner subtree pidx[0..n_in-1]
400
                                pa, pidx, n_in, dim, bsp, inner_box, splitter, shrink);
401
                ANNkd_ptr out = rbd_tree(                // build outer subtree pidx[n_in..n]
402
                                pa, pidx+n_in, n - n_in, dim, bsp, bnd_box, splitter, shrink);
403

    
404
                ANNorthHSArray bnds = NULL;                // bounds (alloc in Box2Bnds and
405
                                                                                // ...freed in bd_shrink destroyer)
406

    
407
                annBox2Bnds(                                        // convert inner box to bounds
408
                                inner_box,                                // inner box
409
                                bnd_box,                                // enclosing box
410
                                dim,                                        // dimension
411
                                n_bnds,                                        // number of bounds (returned)
412
                                bnds);                                        // bounds array (modified)
413

    
414
                                                                                // return shrinking node
415
                return new ANNbd_shrink(n_bnds, bnds, in, out);
416
        }
417
}