root / rgbdslam / gicp / ann_1.1.1 / src / bd_pr_search.cpp @ 9240aaa3
History | View | Annotate | Download (2.67 KB)
1 |
//----------------------------------------------------------------------
|
---|---|
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 |
} |