root / rgbdslam / gicp / ann_1.1.1 / src / brute.cpp @ 9240aaa3
History  View  Annotate  Download (4 KB)
1 
//


2 
// File: brute.cpp

3 
// Programmer: Sunil Arya and David Mount

4 
// Description: Bruteforce nearest neighbors

5 
// Last modified: 05/03/05 (Version 1.1)

6 
//

7 
// Copyright (c) 19972005 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.1 05/03/05

24 
// Added fixedradius kNN search

25 
//

26  
27 
#include <ANN/ANNx.h> // all ANN includes 
28 
#include "pr_queue_k.h" // k element priority queue 
29  
30 
//

31 
// Bruteforce search simply stores a pointer to the list of

32 
// data points and searches linearly for the nearest neighbor.

33 
// The k nearest neighbors are stored in a kelement priority

34 
// queue (which is implemented in a pretty dumb way as well).

35 
//

36 
// If ANN_ALLOW_SELF_MATCH is ANNfalse then data points at distance

37 
// zero are not considered.

38 
//

39 
// Note that the error bound eps is passed in, but it is ignored.

40 
// These routines compute exact nearest neighbors (which is needed

41 
// for validation purposes in ann_test.cpp).

42 
//

43  
44 
ANNbruteForce::ANNbruteForce( // constructor from point array

45 
ANNpointArray pa, // point array

46 
int n, // number of points 
47 
int dd) // dimension 
48 
{ 
49 
dim = dd; n_pts = n; pts = pa; 
50 
} 
51  
52 
ANNbruteForce::~ANNbruteForce() { } // destructor (empty)

53  
54 
void ANNbruteForce::annkSearch( // approx k near neighbor search 
55 
ANNpoint q, // query point

56 
int k, // number of near neighbors to return 
57 
ANNidxArray nn_idx, // nearest neighbor indices (returned)

58 
ANNdistArray dd, // dist to near neighbors (returned)

59 
double eps) // error bound (ignored) 
60 
{ 
61 
ANNmin_k mk(k); // construct a klimited priority queue

62 
int i;

63  
64 
if (k > n_pts) { // too many near neighbors? 
65 
annError("Requesting more near neighbors than data points", ANNabort);

66 
} 
67 
// run every point through queue

68 
for (i = 0; i < n_pts; i++) { 
69 
// compute distance to point

70 
ANNdist sqDist = annDist(dim, pts[i], q); 
71 
if (ANN_ALLOW_SELF_MATCH  sqDist != 0) 
72 
mk.insert(sqDist, i); 
73 
} 
74 
for (i = 0; i < k; i++) { // extract the k closest points 
75 
dd[i] = mk.ith_smallest_key(i); 
76 
nn_idx[i] = mk.ith_smallest_info(i); 
77 
} 
78 
} 
79  
80 
int ANNbruteForce::annkFRSearch( // approx fixedradius kNN search 
81 
ANNpoint q, // query point

82 
ANNdist sqRad, // squared radius

83 
int k, // number of near neighbors to return 
84 
ANNidxArray nn_idx, // nearest neighbor array (returned)

85 
ANNdistArray dd, // dist to near neighbors (returned)

86 
double eps) // error bound 
87 
{ 
88 
ANNmin_k mk(k); // construct a klimited priority queue

89 
int i;

90 
int pts_in_range = 0; // number of points in query range 
91 
// run every point through queue

92 
for (i = 0; i < n_pts; i++) { 
93 
// compute distance to point

94 
ANNdist sqDist = annDist(dim, pts[i], q); 
95 
if (sqDist <= sqRad && // within radius bound 
96 
(ANN_ALLOW_SELF_MATCH  sqDist != 0)) { // ...and no self match 
97 
mk.insert(sqDist, i); 
98 
pts_in_range++; 
99 
} 
100 
} 
101 
for (i = 0; i < k; i++) { // extract the k closest points 
102 
if (dd != NULL) 
103 
dd[i] = mk.ith_smallest_key(i); 
104 
if (nn_idx != NULL) 
105 
nn_idx[i] = mk.ith_smallest_info(i); 
106 
} 
107  
108 
return pts_in_range;

109 
} 