Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (16.3 KB)

1
//----------------------------------------------------------------------
2
// File:                        kd_dump.cc
3
// Programmer:                David Mount
4
// Description:                Dump and Load for kd- and 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 1.0  04/01/05
24
//                Moved dump out of kd_tree.cc into this file.
25
//                Added kd-tree load constructor.
26
//----------------------------------------------------------------------
27
// This file contains routines for dumping kd-trees and bd-trees and
28
// reloading them. (It is an abuse of policy to include both kd- and
29
// bd-tree routines in the same file, sorry.  There should be no problem
30
// in deleting the bd- versions of the routines if they are not
31
// desired.)
32
//----------------------------------------------------------------------
33

    
34
#include "kd_tree.h"                                        // kd-tree declarations
35
#include "bd_tree.h"                                        // bd-tree declarations
36
#include <string.h>
37
using namespace std;                                        // make std:: available
38

    
39
//----------------------------------------------------------------------
40
//                Constants
41
//----------------------------------------------------------------------
42

    
43
const int                STRING_LEN                = 500;        // maximum string length
44
const double        EPSILON                        = 1E-5; // small number for float comparison
45

    
46
enum ANNtreeType {KD_TREE, BD_TREE};        // tree types (used in loading)
47

    
48
//----------------------------------------------------------------------
49
//                Procedure declarations
50
//----------------------------------------------------------------------
51

    
52
static ANNkd_ptr annReadDump(                        // read dump file
53
        istream                                &in,                                        // input stream
54
        ANNtreeType                        tree_type,                                // type of tree expected
55
        ANNpointArray                &the_pts,                                // new points (if applic)
56
        ANNidxArray                        &the_pidx,                                // point indices (returned)
57
        int                                        &the_dim,                                // dimension (returned)
58
        int                                        &the_n_pts,                                // number of points (returned)
59
        int                                        &the_bkt_size,                        // bucket size (returned)
60
        ANNpoint                        &the_bnd_box_lo,                // low bounding point
61
        ANNpoint                        &the_bnd_box_hi);                // high bounding point
62

    
63
static ANNkd_ptr annReadTree(                        // read tree-part of dump file
64
        istream                                &in,                                        // input stream
65
        ANNtreeType                        tree_type,                                // type of tree expected
66
        ANNidxArray                        the_pidx,                                // point indices (modified)
67
        int                                        &next_idx);                                // next index (modified)
68

    
69
//----------------------------------------------------------------------
70
//        ANN kd- and bd-tree Dump Format
71
//                The dump file begins with a header containing the version of
72
//                ANN, an optional section containing the points, followed by
73
//                a description of the tree.        The tree is printed in preorder.
74
//
75
//                Format:
76
//                #ANN <version number> <comments> [END_OF_LINE]
77
//                points <dim> <n_pts>                        (point coordinates: this is optional)
78
//                0 <xxx> <xxx> ... <xxx>                        (point indices and coordinates)
79
//                1 <xxx> <xxx> ... <xxx>
80
//                  ...
81
//                tree <dim> <n_pts> <bkt_size>
82
//                <xxx> <xxx> ... <xxx>                        (lower end of bounding box)
83
//                <xxx> <xxx> ... <xxx>                        (upper end of bounding box)
84
//                                If the tree is null, then a single line "null" is
85
//                                output.         Otherwise the nodes of the tree are printed
86
//                                one per line in preorder.  Leaves and splitting nodes 
87
//                                have the following formats:
88
//                Leaf node:
89
//                                leaf <n_pts> <bkt[0]> <bkt[1]> ... <bkt[n-1]>
90
//                Splitting nodes:
91
//                                split <cut_dim> <cut_val> <lo_bound> <hi_bound>
92
//
93
//                For bd-trees:
94
//
95
//                Shrinking nodes:
96
//                                shrink <n_bnds>
97
//                                                <cut_dim> <cut_val> <side>
98
//                                                <cut_dim> <cut_val> <side>
99
//                                                ... (repeated n_bnds times)
100
//----------------------------------------------------------------------
101

    
102
void ANNkd_tree::Dump(                                        // dump entire tree
103
                ANNbool with_pts,                                // print points as well?
104
                ostream &out)                                        // output stream
105
{
106
        out << "#ANN " << ANNversion << "\n";
107
        out.precision(ANNcoordPrec);                // use full precision in dumping
108
        if (with_pts) {                                                // print point coordinates
109
                out << "points " << dim << " " << n_pts << "\n";
110
                for (int i = 0; i < n_pts; i++) {
111
                        out << i << " ";
112
                        annPrintPt(pts[i], dim, out);
113
                        out << "\n";
114
                }
115
        }
116
        out << "tree "                                                // print tree elements
117
                << dim << " "
118
                << n_pts << " "
119
                << bkt_size << "\n";
120

    
121
        annPrintPt(bnd_box_lo, dim, out);        // print lower bound
122
        out << "\n";
123
        annPrintPt(bnd_box_hi, dim, out);        // print upper bound
124
        out << "\n";
125

    
126
        if (root == NULL)                                        // empty tree?
127
                out << "null\n";
128
        else {
129
                root->dump(out);                                // invoke printing at root
130
        }
131
        out.precision(0);                                        // restore default precision
132
}
133

    
134
void ANNkd_split::dump(                                        // dump a splitting node
135
                ostream &out)                                        // output stream
136
{
137
        out << "split " << cut_dim << " " << cut_val << " ";
138
        out << cd_bnds[ANN_LO] << " " << cd_bnds[ANN_HI] << "\n";
139

    
140
        child[ANN_LO]->dump(out);                        // print low child
141
        child[ANN_HI]->dump(out);                        // print high child
142
}
143

    
144
void ANNkd_leaf::dump(                                        // dump a leaf node
145
                ostream &out)                                        // output stream
146
{
147
        if (this == KD_TRIVIAL) {                        // canonical trivial leaf node
148
                out << "leaf 0\n";                                // leaf no points
149
        }
150
        else{
151
                out << "leaf " << n_pts;
152
                for (int j = 0; j < n_pts; j++) {
153
                        out << " " << bkt[j];
154
                }
155
                out << "\n";
156
        }
157
}
158

    
159
void ANNbd_shrink::dump(                                // dump a shrinking node
160
                ostream &out)                                        // output stream
161
{
162
        out << "shrink " << n_bnds << "\n";
163
        for (int j = 0; j < n_bnds; j++) {
164
                out << bnds[j].cd << " " << bnds[j].cv << " " << bnds[j].sd << "\n";
165
        }
166
        child[ANN_IN]->dump(out);                        // print in-child
167
        child[ANN_OUT]->dump(out);                        // print out-child
168
}
169

    
170
//----------------------------------------------------------------------
171
// Load kd-tree from dump file
172
//                This rebuilds a kd-tree which was dumped to a file.         The dump
173
//                file contains all the basic tree information according to a
174
//                preorder traversal.         We assume that the dump file also contains
175
//                point data.         (This is to guarantee the consistency of the tree.)
176
//                If not, then an error is generated.
177
//
178
//                Indirectly, this procedure allocates space for points, point
179
//                indices, all nodes in the tree, and the bounding box for the
180
//                tree.  When the tree is destroyed, all but the points are
181
//                deallocated.
182
//
183
//                This routine calls annReadDump to do all the work.
184
//----------------------------------------------------------------------
185

    
186
ANNkd_tree::ANNkd_tree(                                        // build from dump file
187
        istream                                &in)                                        // input stream for dump file
188
{
189
        int the_dim;                                                                // local dimension
190
        int the_n_pts;                                                                // local number of points
191
        int the_bkt_size;                                                        // local number of points
192
        ANNpoint the_bnd_box_lo;                                        // low bounding point
193
        ANNpoint the_bnd_box_hi;                                        // high bounding point
194
        ANNpointArray the_pts;                                                // point storage
195
        ANNidxArray the_pidx;                                                // point index storage
196
        ANNkd_ptr the_root;                                                        // root of the tree
197

    
198
        the_root = annReadDump(                                                // read the dump file
199
                in,                                                                                // input stream
200
                KD_TREE,                                                                // expecting a kd-tree
201
                the_pts,                                                                // point array (returned)
202
                the_pidx,                                                                // point indices (returned)
203
                the_dim, the_n_pts, the_bkt_size,                // basic tree info (returned)
204
                the_bnd_box_lo, the_bnd_box_hi);                // bounding box info (returned)
205

    
206
                                                                                                // create a skeletal tree
207
        SkeletonTree(the_n_pts, the_dim, the_bkt_size, the_pts, the_pidx);
208

    
209
        bnd_box_lo = the_bnd_box_lo;
210
        bnd_box_hi = the_bnd_box_hi;
211

    
212
        root = the_root;                                                        // set the root
213
}
214

    
215
ANNbd_tree::ANNbd_tree(                                        // build bd-tree from dump file
216
        istream                                &in) : ANNkd_tree()                // input stream for dump file
217
{
218
        int the_dim;                                                                // local dimension
219
        int the_n_pts;                                                                // local number of points
220
        int the_bkt_size;                                                        // local number of points
221
        ANNpoint the_bnd_box_lo;                                        // low bounding point
222
        ANNpoint the_bnd_box_hi;                                        // high bounding point
223
        ANNpointArray the_pts;                                                // point storage
224
        ANNidxArray the_pidx;                                                // point index storage
225
        ANNkd_ptr the_root;                                                        // root of the tree
226

    
227
        the_root = annReadDump(                                                // read the dump file
228
                in,                                                                                // input stream
229
                BD_TREE,                                                                // expecting a bd-tree
230
                the_pts,                                                                // point array (returned)
231
                the_pidx,                                                                // point indices (returned)
232
                the_dim, the_n_pts, the_bkt_size,                // basic tree info (returned)
233
                the_bnd_box_lo, the_bnd_box_hi);                // bounding box info (returned)
234

    
235
                                                                                                // create a skeletal tree
236
        SkeletonTree(the_n_pts, the_dim, the_bkt_size, the_pts, the_pidx);
237
        bnd_box_lo = the_bnd_box_lo;
238
        bnd_box_hi = the_bnd_box_hi;
239

    
240
        root = the_root;                                                        // set the root
241
}
242

    
243
//----------------------------------------------------------------------
244
//        annReadDump - read a dump file
245
//
246
//                This procedure reads a dump file, constructs a kd-tree
247
//                and returns all the essential information needed to actually
248
//                construct the tree.         Because this procedure is used for
249
//                constructing both kd-trees and bd-trees, the second argument
250
//                is used to indicate which type of tree we are expecting.
251
//----------------------------------------------------------------------
252

    
253
static ANNkd_ptr annReadDump(
254
        istream                                &in,                                        // input stream
255
        ANNtreeType                        tree_type,                                // type of tree expected
256
        ANNpointArray                &the_pts,                                // new points (returned)
257
        ANNidxArray                        &the_pidx,                                // point indices (returned)
258
        int                                        &the_dim,                                // dimension (returned)
259
        int                                        &the_n_pts,                                // number of points (returned)
260
        int                                        &the_bkt_size,                        // bucket size (returned)
261
        ANNpoint                        &the_bnd_box_lo,                // low bounding point (ret'd)
262
        ANNpoint                        &the_bnd_box_hi)                // high bounding point (ret'd)
263
{
264
        int j;
265
        char str[STRING_LEN];                                                // storage for string
266
        char version[STRING_LEN];                                        // ANN version number
267
        ANNkd_ptr the_root = NULL;
268

    
269
        //------------------------------------------------------------------
270
        //        Input file header
271
        //------------------------------------------------------------------
272
        in >> str;                                                                        // input header
273
        if (strcmp(str, "#ANN") != 0) {                                // incorrect header
274
                annError("Incorrect header for dump file", ANNabort);
275
        }
276
        in.getline(version, STRING_LEN);                        // get version (ignore)
277

    
278
        //------------------------------------------------------------------
279
        //        Input the points
280
        //                        An array the_pts is allocated and points are read from
281
        //                        the dump file.
282
        //------------------------------------------------------------------
283
        in >> str;                                                                        // get major heading
284
        if (strcmp(str, "points") == 0) {                        // points section
285
                in >> the_dim;                                                        // input dimension
286
                in >> the_n_pts;                                                // number of points
287
                                                                                                // allocate point storage
288
                the_pts = annAllocPts(the_n_pts, the_dim);
289
                for (int i = 0; i < the_n_pts; i++) {        // input point coordinates
290
                        ANNidx idx;                                                        // point index
291
                        in >> idx;                                                        // input point index
292
                        if (idx < 0 || idx >= the_n_pts) {
293
                                annError("Point index is out of range", ANNabort);
294
                        }
295
                        for (j = 0; j < the_dim; j++) {
296
                                in >> the_pts[idx][j];                        // read point coordinates
297
                        }
298
                }
299
                in >> str;                                                                // get next major heading
300
        }
301
        else {                                                                                // no points were input
302
                annError("Points must be supplied in the dump file", ANNabort);
303
        }
304

    
305
        //------------------------------------------------------------------
306
        //        Input the tree
307
        //                        After the basic header information, we invoke annReadTree
308
        //                        to do all the heavy work.  We create our own array of
309
        //                        point indices (so we can pass them to annReadTree())
310
        //                        but we do not deallocate them.        They will be deallocated
311
        //                        when the tree is destroyed.
312
        //------------------------------------------------------------------
313
        if (strcmp(str, "tree") == 0) {                                // tree section
314
                in >> the_dim;                                                        // read dimension
315
                in >> the_n_pts;                                                // number of points
316
                in >> the_bkt_size;                                                // bucket size
317
                the_bnd_box_lo = annAllocPt(the_dim);        // allocate bounding box pts
318
                the_bnd_box_hi = annAllocPt(the_dim);
319

    
320
                for (j = 0; j < the_dim; j++) {                        // read bounding box low
321
                        in >> the_bnd_box_lo[j];
322
                }
323
                for (j = 0; j < the_dim; j++) {                        // read bounding box low
324
                        in >> the_bnd_box_hi[j];
325
                }
326
                the_pidx = new ANNidx[the_n_pts];                // allocate point index array
327
                int next_idx = 0;                                                // number of indices filled
328
                                                                                                // read the tree and indices
329
                the_root = annReadTree(in, tree_type, the_pidx, next_idx);
330
                if (next_idx != the_n_pts) {                        // didn't see all the points?
331
                        annError("Didn't see as many points as expected", ANNwarn);
332
                }
333
        }
334
        else {
335
                annError("Illegal dump format.        Expecting section heading", ANNabort);
336
        }
337
        return the_root;
338
}
339

    
340
//----------------------------------------------------------------------
341
// annReadTree - input tree and return pointer
342
//
343
//                annReadTree reads in a node of the tree, makes any recursive
344
//                calls as needed to input the children of this node (if internal).
345
//                It returns a pointer to the node that was created.        An array
346
//                of point indices is given along with a pointer to the next
347
//                available location in the array.  As leaves are read, their
348
//                point indices are stored here, and the point buckets point
349
//                to the first entry in the array.
350
//
351
//                Recall that these are the formats.        The tree is given in
352
//                preorder.
353
//
354
//                Leaf node:
355
//                                leaf <n_pts> <bkt[0]> <bkt[1]> ... <bkt[n-1]>
356
//                Splitting nodes:
357
//                                split <cut_dim> <cut_val> <lo_bound> <hi_bound>
358
//
359
//                For bd-trees:
360
//
361
//                Shrinking nodes:
362
//                                shrink <n_bnds>
363
//                                                <cut_dim> <cut_val> <side>
364
//                                                <cut_dim> <cut_val> <side>
365
//                                                ... (repeated n_bnds times)
366
//----------------------------------------------------------------------
367

    
368
static ANNkd_ptr annReadTree(
369
        istream                                &in,                                        // input stream
370
        ANNtreeType                        tree_type,                                // type of tree expected
371
        ANNidxArray                        the_pidx,                                // point indices (modified)
372
        int                                        &next_idx)                                // next index (modified)
373
{
374
        char tag[STRING_LEN];                                                // tag (leaf, split, shrink)
375
        int n_pts;                                                                        // number of points in leaf
376
        int cd;                                                                                // cut dimension
377
        ANNcoord cv;                                                                // cut value
378
        ANNcoord lb;                                                                // low bound
379
        ANNcoord hb;                                                                // high bound
380
        int n_bnds;                                                                        // number of bounding sides
381
        int sd;                                                                                // which side
382

    
383
        in >> tag;                                                                        // input node tag
384

    
385
        if (strcmp(tag, "null") == 0) {                                // null tree
386
                return NULL;
387
        }
388
        //------------------------------------------------------------------
389
        //        Read a leaf
390
        //------------------------------------------------------------------
391
        if (strcmp(tag, "leaf") == 0) {                                // leaf node
392

    
393
                in >> n_pts;                                                        // input number of points
394
                int old_idx = next_idx;                                        // save next_idx
395
                if (n_pts == 0) {                                                // trivial leaf
396
                        return KD_TRIVIAL;
397
                }
398
                else {
399
                        for (int i = 0; i < n_pts; i++) {        // input point indices
400
                                in >> the_pidx[next_idx++];                // store in array of indices
401
                        }
402
                }
403
                return new ANNkd_leaf(n_pts, &the_pidx[old_idx]);
404
        }
405
        //------------------------------------------------------------------
406
        //        Read a splitting node
407
        //------------------------------------------------------------------
408
        else if (strcmp(tag, "split") == 0) {                // splitting node
409

    
410
                in >> cd >> cv >> lb >> hb;
411

    
412
                                                                                                // read low and high subtrees
413
                ANNkd_ptr lc = annReadTree(in, tree_type, the_pidx, next_idx);
414
                ANNkd_ptr hc = annReadTree(in, tree_type, the_pidx, next_idx);
415
                                                                                                // create new node and return
416
                return new ANNkd_split(cd, cv, lb, hb, lc, hc);
417
        }
418
        //------------------------------------------------------------------
419
        //        Read a shrinking node (bd-tree only)
420
        //------------------------------------------------------------------
421
        else if (strcmp(tag, "shrink") == 0) {                // shrinking node
422
                if (tree_type != BD_TREE) {
423
                        annError("Shrinking node not allowed in kd-tree", ANNabort);
424
                }
425

    
426
                in >> n_bnds;                                                        // number of bounding sides
427
                                                                                                // allocate bounds array
428
                ANNorthHSArray bds = new ANNorthHalfSpace[n_bnds];
429
                for (int i = 0; i < n_bnds; i++) {
430
                        in >> cd >> cv >> sd;                                // input bounding halfspace
431
                                                                                                // copy to array
432
                        bds[i] = ANNorthHalfSpace(cd, cv, sd);
433
                }
434
                                                                                                // read inner and outer subtrees
435
                ANNkd_ptr ic = annReadTree(in, tree_type, the_pidx, next_idx);
436
                ANNkd_ptr oc = annReadTree(in, tree_type, the_pidx, next_idx);
437
                                                                                                // create new node and return
438
                return new ANNbd_shrink(n_bnds, bds, ic, oc);
439
        }
440
        else {
441
                annError("Illegal node type in dump file", ANNabort);
442
                exit(0);                                                                // to keep the compiler happy
443
        }
444
}