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 |
} |