root / rgbdslam / gicp / ann_1.1.1 / src / bd_pr_search.cpp @ 9240aaa3
History | View | Annotate | Download (2.67 KB)
1 | 9240aaa3 | Alex | //----------------------------------------------------------------------
|
---|---|---|---|
2 | // File: bd_pr_search.cpp
|
||
3 | // Programmer: David Mount
|
||
4 | // Description: Priority search 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 | //----------------------------------------------------------------------
|
||
24 | |||
25 | #include "bd_tree.h" // bd-tree declarations |
||
26 | #include "kd_pr_search.h" // kd priority search declarations |
||
27 | |||
28 | //----------------------------------------------------------------------
|
||
29 | // Approximate priority searching for bd-trees.
|
||
30 | // See the file kd_pr_search.cc for general information on the
|
||
31 | // approximate nearest neighbor priority search algorithm. Here
|
||
32 | // we include the extensions for shrinking nodes.
|
||
33 | //----------------------------------------------------------------------
|
||
34 | |||
35 | //----------------------------------------------------------------------
|
||
36 | // bd_shrink::ann_search - search a shrinking node
|
||
37 | //----------------------------------------------------------------------
|
||
38 | |||
39 | void ANNbd_shrink::ann_pri_search(ANNdist box_dist)
|
||
40 | { |
||
41 | ANNdist inner_dist = 0; // distance to inner box |
||
42 | for (int i = 0; i < n_bnds; i++) { // is query point in the box? |
||
43 | if (bnds[i].out(ANNprQ)) { // outside this bounding side? |
||
44 | // add to inner distance
|
||
45 | inner_dist = (ANNdist) ANN_SUM(inner_dist, bnds[i].dist(ANNprQ)); |
||
46 | } |
||
47 | } |
||
48 | if (inner_dist <= box_dist) { // if inner box is closer |
||
49 | if (child[ANN_OUT] != KD_TRIVIAL) // enqueue outer if not trivial |
||
50 | ANNprBoxPQ->insert(box_dist,child[ANN_OUT]); |
||
51 | // continue with inner child
|
||
52 | child[ANN_IN]->ann_pri_search(inner_dist); |
||
53 | } |
||
54 | else { // if outer box is closer |
||
55 | if (child[ANN_IN] != KD_TRIVIAL) // enqueue inner if not trivial |
||
56 | ANNprBoxPQ->insert(inner_dist,child[ANN_IN]); |
||
57 | // continue with outer child
|
||
58 | child[ANN_OUT]->ann_pri_search(box_dist); |
||
59 | } |
||
60 | ANN_FLOP(3*n_bnds) // increment floating ops |
||
61 | ANN_SHR(1) // one more shrinking node |
||
62 | } |