root / rgbdslam / gicp / ann_1.1.1 / src / bd_fix_rad_search.cpp @ 9240aaa3
History | View | Annotate | Download (2.64 KB)
1 | 9240aaa3 | Alex | //----------------------------------------------------------------------
|
---|---|---|---|
2 | // File: bd_fix_rad_search.cpp
|
||
3 | // Programmer: David Mount
|
||
4 | // Description: Standard bd-tree search
|
||
5 | // Last modified: 05/03/05 (Version 1.1)
|
||
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 1.1 05/03/05
|
||
22 | // Initial release
|
||
23 | //----------------------------------------------------------------------
|
||
24 | |||
25 | #include "bd_tree.h" // bd-tree declarations |
||
26 | #include "kd_fix_rad_search.h" // kd-tree FR search declarations |
||
27 | |||
28 | //----------------------------------------------------------------------
|
||
29 | // Approximate searching for bd-trees.
|
||
30 | // See the file kd_FR_search.cpp for general information on the
|
||
31 | // approximate nearest neighbor search algorithm. Here we
|
||
32 | // include the extensions for shrinking nodes.
|
||
33 | //----------------------------------------------------------------------
|
||
34 | |||
35 | //----------------------------------------------------------------------
|
||
36 | // bd_shrink::ann_FR_search - search a shrinking node
|
||
37 | //----------------------------------------------------------------------
|
||
38 | |||
39 | void ANNbd_shrink::ann_FR_search(ANNdist box_dist)
|
||
40 | { |
||
41 | // check dist calc term cond.
|
||
42 | if (ANNmaxPtsVisited != 0 && ANNptsVisited > ANNmaxPtsVisited) return; |
||
43 | |||
44 | ANNdist inner_dist = 0; // distance to inner box |
||
45 | for (int i = 0; i < n_bnds; i++) { // is query point in the box? |
||
46 | if (bnds[i].out(ANNkdFRQ)) { // outside this bounding side? |
||
47 | // add to inner distance
|
||
48 | inner_dist = (ANNdist) ANN_SUM(inner_dist, bnds[i].dist(ANNkdFRQ)); |
||
49 | } |
||
50 | } |
||
51 | if (inner_dist <= box_dist) { // if inner box is closer |
||
52 | child[ANN_IN]->ann_FR_search(inner_dist);// search inner child first
|
||
53 | child[ANN_OUT]->ann_FR_search(box_dist);// ...then outer child
|
||
54 | } |
||
55 | else { // if outer box is closer |
||
56 | child[ANN_OUT]->ann_FR_search(box_dist);// search outer child first
|
||
57 | child[ANN_IN]->ann_FR_search(inner_dist);// ...then outer child
|
||
58 | } |
||
59 | ANN_FLOP(3*n_bnds) // increment floating ops |
||
60 | ANN_SHR(1) // one more shrinking node |
||
61 | } |