Statistics
| Branch: | Revision:

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:                Brute-force nearest neighbors
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 0.1  03/04/98
22
//                Initial release
23
//        Revision 1.1  05/03/05
24
//                Added fixed-radius 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
//                Brute-force 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 k-element 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 k-limited 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 fixed-radius 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 k-limited 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
}