Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (14.7 KB)

1
//----------------------------------------------------------------------
2
// File:                        kd_util.cpp
3
// Programmer:                Sunil Arya and David Mount
4
// Description:                Common utilities 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_util.h"                                        // kd-utility declarations
26

    
27
#include <ANN/ANNperf.h>                                // performance evaluation
28

    
29
//----------------------------------------------------------------------
30
// The following routines are utility functions for manipulating
31
// points sets, used in determining splitting planes for kd-tree
32
// construction.
33
//----------------------------------------------------------------------
34

    
35
//----------------------------------------------------------------------
36
//        NOTE: Virtually all point indexing is done through an index (i.e.
37
//        permutation) array pidx.  Consequently, a reference to the d-th
38
//        coordinate of the i-th point is pa[pidx[i]][d].  The macro PA(i,d)
39
//        is a shorthand for this.
40
//----------------------------------------------------------------------
41
                                                                                // standard 2-d indirect indexing
42
#define PA(i,d)                        (pa[pidx[(i)]][(d)])
43
                                                                                // accessing a single point
44
#define PP(i)                        (pa[pidx[(i)]])
45

    
46
//----------------------------------------------------------------------
47
//        annAspectRatio
48
//                Compute the aspect ratio (ratio of longest to shortest side)
49
//                of a rectangle.
50
//----------------------------------------------------------------------
51

    
52
double annAspectRatio(
53
        int                                        dim,                        // dimension
54
        const ANNorthRect        &bnd_box)                // bounding cube
55
{
56
        ANNcoord length = bnd_box.hi[0] - bnd_box.lo[0];
57
        ANNcoord min_length = length;                                // min side length
58
        ANNcoord max_length = length;                                // max side length
59
        for (int d = 0; d < dim; d++) {
60
                length = bnd_box.hi[d] - bnd_box.lo[d];
61
                if (length < min_length) min_length = length;
62
                if (length > max_length) max_length = length;
63
        }
64
        return max_length/min_length;
65
}
66

    
67
//----------------------------------------------------------------------
68
//        annEnclRect, annEnclCube
69
//                These utilities compute the smallest rectangle and cube enclosing
70
//                a set of points, respectively.
71
//----------------------------------------------------------------------
72

    
73
void annEnclRect(
74
        ANNpointArray                pa,                                // point array
75
        ANNidxArray                        pidx,                        // point indices
76
        int                                        n,                                // number of points
77
        int                                        dim,                        // dimension
78
        ANNorthRect                        &bnds)                        // bounding cube (returned)
79
{
80
        for (int d = 0; d < dim; d++) {                // find smallest enclosing rectangle
81
                ANNcoord lo_bnd = PA(0,d);                // lower bound on dimension d
82
                ANNcoord hi_bnd = PA(0,d);                // upper bound on dimension d
83
                for (int i = 0; i < n; i++) {
84
                        if (PA(i,d) < lo_bnd) lo_bnd = PA(i,d);
85
                        else if (PA(i,d) > hi_bnd) hi_bnd = PA(i,d);
86
                }
87
                bnds.lo[d] = lo_bnd;
88
                bnds.hi[d] = hi_bnd;
89
        }
90
}
91

    
92
void annEnclCube(                                                // compute smallest enclosing cube
93
        ANNpointArray                pa,                                // point array
94
        ANNidxArray                        pidx,                        // point indices
95
        int                                        n,                                // number of points
96
        int                                        dim,                        // dimension
97
        ANNorthRect                        &bnds)                        // bounding cube (returned)
98
{
99
        int d;
100
                                                                                // compute smallest enclosing rect
101
        annEnclRect(pa, pidx, n, dim, bnds);
102

    
103
        ANNcoord max_len = 0;                                // max length of any side
104
        for (d = 0; d < dim; d++) {                        // determine max side length
105
                ANNcoord len = bnds.hi[d] - bnds.lo[d];
106
                if (len > max_len) {                        // update max_len if longest
107
                        max_len = len;
108
                }
109
        }
110
        for (d = 0; d < dim; d++) {                        // grow sides to match max
111
                ANNcoord len = bnds.hi[d] - bnds.lo[d];
112
                ANNcoord half_diff = (max_len - len) / 2;
113
                bnds.lo[d] -= half_diff;
114
                bnds.hi[d] += half_diff;
115
        }
116
}
117

    
118
//----------------------------------------------------------------------
119
//        annBoxDistance - utility routine which computes distance from point to
120
//                box (Note: most distances to boxes are computed using incremental
121
//                distance updates, not this function.)
122
//----------------------------------------------------------------------
123

    
124
ANNdist annBoxDistance(                        // compute distance from point to box
125
        const ANNpoint                q,                                // the point
126
        const ANNpoint                lo,                                // low point of box
127
        const ANNpoint                hi,                                // high point of box
128
        int                                        dim)                        // dimension of space
129
{
130
        register ANNdist dist = 0.0;                // sum of squared distances
131
        register ANNdist t;
132

    
133
        for (register int d = 0; d < dim; d++) {
134
                if (q[d] < lo[d]) {                                // q is left of box
135
                        t = ANNdist(lo[d]) - ANNdist(q[d]);
136
                        dist = ANN_SUM(dist, ANN_POW(t));
137
                }
138
                else if (q[d] > hi[d]) {                // q is right of box
139
                        t = ANNdist(q[d]) - ANNdist(hi[d]);
140
                        dist = ANN_SUM(dist, ANN_POW(t));
141
                }
142
        }
143
        ANN_FLOP(4*dim)                                                // increment floating op count
144

    
145
        return dist;
146
}
147

    
148
//----------------------------------------------------------------------
149
//        annSpread - find spread along given dimension
150
//        annMinMax - find min and max coordinates along given dimension
151
//        annMaxSpread - find dimension of max spread
152
//----------------------------------------------------------------------
153

    
154
ANNcoord annSpread(                                // compute point spread along dimension
155
        ANNpointArray                pa,                                // point array
156
        ANNidxArray                        pidx,                        // point indices
157
        int                                        n,                                // number of points
158
        int                                        d)                                // dimension to check
159
{
160
        ANNcoord min = PA(0,d);                                // compute max and min coords
161
        ANNcoord max = PA(0,d);
162
        for (int i = 1; i < n; i++) {
163
                ANNcoord c = PA(i,d);
164
                if (c < min) min = c;
165
                else if (c > max) max = c;
166
        }
167
        return (max - min);                                        // total spread is difference
168
}
169

    
170
void annMinMax(                                        // compute min and max coordinates along dim
171
        ANNpointArray                pa,                                // point array
172
        ANNidxArray                        pidx,                        // point indices
173
        int                                        n,                                // number of points
174
        int                                        d,                                // dimension to check
175
        ANNcoord                        &min,                        // minimum value (returned)
176
        ANNcoord                        &max)                        // maximum value (returned)
177
{
178
        min = PA(0,d);                                                // compute max and min coords
179
        max = PA(0,d);
180
        for (int i = 1; i < n; i++) {
181
                ANNcoord c = PA(i,d);
182
                if (c < min) min = c;
183
                else if (c > max) max = c;
184
        }
185
}
186

    
187
int annMaxSpread(                                                // compute dimension of max spread
188
        ANNpointArray                pa,                                // point array
189
        ANNidxArray                        pidx,                        // point indices
190
        int                                        n,                                // number of points
191
        int                                        dim)                        // dimension of space
192
{
193
        int max_dim = 0;                                        // dimension of max spread
194
        ANNcoord max_spr = 0;                                // amount of max spread
195

    
196
        if (n == 0) return max_dim;                        // no points, who cares?
197

    
198
        for (int d = 0; d < dim; d++) {                // compute spread along each dim
199
                ANNcoord spr = annSpread(pa, pidx, n, d);
200
                if (spr > max_spr) {                        // bigger than current max
201
                        max_spr = spr;
202
                        max_dim = d;
203
                }
204
        }
205
        return max_dim;
206
}
207

    
208
//----------------------------------------------------------------------
209
//        annMedianSplit - split point array about its median
210
//                Splits a subarray of points pa[0..n] about an element of given
211
//                rank (median: n_lo = n/2) with respect to dimension d.  It places
212
//                the element of rank n_lo-1 correctly (because our splitting rule
213
//                takes the mean of these two).  On exit, the array is permuted so
214
//                that:
215
//
216
//                pa[0..n_lo-2][d] <= pa[n_lo-1][d] <= pa[n_lo][d] <= pa[n_lo+1..n-1][d].
217
//
218
//                The mean of pa[n_lo-1][d] and pa[n_lo][d] is returned as the
219
//                splitting value.
220
//
221
//                All indexing is done indirectly through the index array pidx.
222
//
223
//                This function uses the well known selection algorithm due to
224
//                C.A.R. Hoare.
225
//----------------------------------------------------------------------
226

    
227
                                                                                // swap two points in pa array
228
#define PASWAP(a,b) { int tmp = pidx[a]; pidx[a] = pidx[b]; pidx[b] = tmp; }
229

    
230
void annMedianSplit(
231
        ANNpointArray                pa,                                // points to split
232
        ANNidxArray                        pidx,                        // point indices
233
        int                                        n,                                // number of points
234
        int                                        d,                                // dimension along which to split
235
        ANNcoord                        &cv,                        // cutting value
236
        int                                        n_lo)                        // split into n_lo and n-n_lo
237
{
238
        int l = 0;                                                        // left end of current subarray
239
        int r = n-1;                                                // right end of current subarray
240
        while (l < r) {
241
                register int i = (r+l)/2;                // select middle as pivot
242
                register int k;
243

    
244
                if (PA(i,d) > PA(r,d))                        // make sure last > pivot
245
                        PASWAP(i,r)
246
                PASWAP(l,i);                                        // move pivot to first position
247

    
248
                ANNcoord c = PA(l,d);                        // pivot value
249
                i = l;
250
                k = r;
251
                for(;;) {                                                // pivot about c
252
                        while (PA(++i,d) < c) ;
253
                        while (PA(--k,d) > c) ;
254
                        if (i < k) PASWAP(i,k) else break;
255
                }
256
                PASWAP(l,k);                                        // pivot winds up in location k
257

    
258
                if (k > n_lo)           r = k-1;                // recurse on proper subarray
259
                else if (k < n_lo) l = k+1;
260
                else break;                                                // got the median exactly
261
        }
262
        if (n_lo > 0) {                                                // search for next smaller item
263
                ANNcoord c = PA(0,d);                        // candidate for max
264
                int k = 0;                                                // candidate's index
265
                for (int i = 1; i < n_lo; i++) {
266
                        if (PA(i,d) > c) {
267
                                c = PA(i,d);
268
                                k = i;
269
                        }
270
                }
271
                PASWAP(n_lo-1, k);                                // max among pa[0..n_lo-1] to pa[n_lo-1]
272
        }
273
                                                                                // cut value is midpoint value
274
        cv = (PA(n_lo-1,d) + PA(n_lo,d))/2.0;
275
}
276

    
277
//----------------------------------------------------------------------
278
//        annPlaneSplit - split point array about a cutting plane
279
//                Split the points in an array about a given plane along a
280
//                given cutting dimension.  On exit, br1 and br2 are set so
281
//                that:
282
//                
283
//                                pa[ 0 ..br1-1] <  cv
284
//                                pa[br1..br2-1] == cv
285
//                                pa[br2.. n -1] >  cv
286
//
287
//                All indexing is done indirectly through the index array pidx.
288
//
289
//----------------------------------------------------------------------
290

    
291
void annPlaneSplit(                                // split points by a plane
292
        ANNpointArray                pa,                                // points to split
293
        ANNidxArray                        pidx,                        // point indices
294
        int                                        n,                                // number of points
295
        int                                        d,                                // dimension along which to split
296
        ANNcoord                        cv,                                // cutting value
297
        int                                        &br1,                        // first break (values < cv)
298
        int                                        &br2)                        // second break (values == cv)
299
{
300
        int l = 0;
301
        int r = n-1;
302
        for(;;) {                                                        // partition pa[0..n-1] about cv
303
                while (l < n && PA(l,d) < cv) l++;
304
                while (r >= 0 && PA(r,d) >= cv) r--;
305
                if (l > r) break;
306
                PASWAP(l,r);
307
                l++; r--;
308
        }
309
        br1 = l;                                        // now: pa[0..br1-1] < cv <= pa[br1..n-1]
310
        r = n-1;
311
        for(;;) {                                                        // partition pa[br1..n-1] about cv
312
                while (l < n && PA(l,d) <= cv) l++;
313
                while (r >= br1 && PA(r,d) > cv) r--;
314
                if (l > r) break;
315
                PASWAP(l,r);
316
                l++; r--;
317
        }
318
        br2 = l;                                        // now: pa[br1..br2-1] == cv < pa[br2..n-1]
319
}
320

    
321

    
322
//----------------------------------------------------------------------
323
//        annBoxSplit - split point array about a orthogonal rectangle
324
//                Split the points in an array about a given orthogonal
325
//                rectangle.  On exit, n_in is set to the number of points
326
//                that are inside (or on the boundary of) the rectangle.
327
//
328
//                All indexing is done indirectly through the index array pidx.
329
//
330
//----------------------------------------------------------------------
331

    
332
void annBoxSplit(                                // split points by a box
333
        ANNpointArray                pa,                                // points to split
334
        ANNidxArray                        pidx,                        // point indices
335
        int                                        n,                                // number of points
336
        int                                        dim,                        // dimension of space
337
        ANNorthRect                        &box,                        // the box
338
        int                                        &n_in)                        // number of points inside (returned)
339
{
340
        int l = 0;
341
        int r = n-1;
342
        for(;;) {                                                        // partition pa[0..n-1] about box
343
                while (l < n && box.inside(dim, PP(l))) l++;
344
                while (r >= 0 && !box.inside(dim, PP(r))) r--;
345
                if (l > r) break;
346
                PASWAP(l,r);
347
                l++; r--;
348
        }
349
        n_in = l;                                        // now: pa[0..n_in-1] inside and rest outside
350
}
351

    
352
//----------------------------------------------------------------------
353
//        annSplitBalance - compute balance factor for a given plane split
354
//                Balance factor is defined as the number of points lying
355
//                below the splitting value minus n/2 (median).  Thus, a
356
//                median split has balance 0, left of this is negative and
357
//                right of this is positive.  (The points are unchanged.)
358
//----------------------------------------------------------------------
359

    
360
int annSplitBalance(                        // determine balance factor of a split
361
        ANNpointArray                pa,                                // points to split
362
        ANNidxArray                        pidx,                        // point indices
363
        int                                        n,                                // number of points
364
        int                                        d,                                // dimension along which to split
365
        ANNcoord                        cv)                                // cutting value
366
{
367
        int n_lo = 0;
368
        for(int i = 0; i < n; i++) {                // count number less than cv
369
                if (PA(i,d) < cv) n_lo++;
370
        }
371
        return n_lo - n/2;
372
}
373

    
374
//----------------------------------------------------------------------
375
//        annBox2Bnds - convert bounding box to list of bounds
376
//                Given two boxes, an inner box enclosed within a bounding
377
//                box, this routine determines all the sides for which the
378
//                inner box is strictly contained with the bounding box,
379
//                and adds an appropriate entry to a list of bounds.  Then
380
//                we allocate storage for the final list of bounds, and return
381
//                the resulting list and its size.
382
//----------------------------------------------------------------------
383

    
384
void annBox2Bnds(                                                // convert inner box to bounds
385
        const ANNorthRect        &inner_box,                // inner box
386
        const ANNorthRect        &bnd_box,                // enclosing box
387
        int                                        dim,                        // dimension of space
388
        int                                        &n_bnds,                // number of bounds (returned)
389
        ANNorthHSArray                &bnds)                        // bounds array (returned)
390
{
391
        int i;
392
        n_bnds = 0;                                                                        // count number of bounds
393
        for (i = 0; i < dim; i++) {
394
                if (inner_box.lo[i] > bnd_box.lo[i])        // low bound is inside
395
                                n_bnds++;
396
                if (inner_box.hi[i] < bnd_box.hi[i])        // high bound is inside
397
                                n_bnds++;
398
        }
399

    
400
        bnds = new ANNorthHalfSpace[n_bnds];                // allocate appropriate size
401

    
402
        int j = 0;
403
        for (i = 0; i < dim; i++) {                                        // fill the array
404
                if (inner_box.lo[i] > bnd_box.lo[i]) {
405
                                bnds[j].cd = i;
406
                                bnds[j].cv = inner_box.lo[i];
407
                                bnds[j].sd = +1;
408
                                j++;
409
                }
410
                if (inner_box.hi[i] < bnd_box.hi[i]) {
411
                                bnds[j].cd = i;
412
                                bnds[j].cv = inner_box.hi[i];
413
                                bnds[j].sd = -1;
414
                                j++;
415
                }
416
        }
417
}
418

    
419
//----------------------------------------------------------------------
420
//        annBnds2Box - convert list of bounds to bounding box
421
//                Given an enclosing box and a list of bounds, this routine
422
//                computes the corresponding inner box.  It is assumed that
423
//                the box points have been allocated already.
424
//----------------------------------------------------------------------
425

    
426
void annBnds2Box(
427
        const ANNorthRect        &bnd_box,                // enclosing box
428
        int                                        dim,                        // dimension of space
429
        int                                        n_bnds,                        // number of bounds
430
        ANNorthHSArray                bnds,                        // bounds array
431
        ANNorthRect                        &inner_box)                // inner box (returned)
432
{
433
        annAssignRect(dim, inner_box, bnd_box);                // copy bounding box to inner
434

    
435
        for (int i = 0; i < n_bnds; i++) {
436
                bnds[i].project(inner_box.lo);                        // project each endpoint
437
                bnds[i].project(inner_box.hi);
438
        }
439
}