Statistics
| Branch: | Revision:

root / rgbdslam / gicp / ann_1.1.1 / src / ANN.cpp @ 9240aaa3

History | View | Annotate | Download (6.26 KB)

1
//----------------------------------------------------------------------
2
// File:                        ANN.cpp
3
// Programmer:                Sunil Arya and David Mount
4
// Description:                Methods for ANN.h and ANNx.h
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
//        Revision 1.0  04/01/05
24
//                Added performance counting to annDist()
25
//----------------------------------------------------------------------
26

    
27
#include <ANN/ANNx.h>                                        // all ANN includes
28
#include <ANN/ANNperf.h>                                // ANN performance 
29

    
30
using namespace std;                                        // make std:: accessible
31

    
32
//----------------------------------------------------------------------
33
//        Point methods
34
//----------------------------------------------------------------------
35

    
36
//----------------------------------------------------------------------
37
//        Distance utility.
38
//                (Note: In the nearest neighbor search, most distances are
39
//                computed using partial distance calculations, not this
40
//                procedure.)
41
//----------------------------------------------------------------------
42

    
43
ANNdist annDist(                                                // interpoint squared distance
44
        int                                        dim,
45
        ANNpoint                        p,
46
        ANNpoint                        q)
47
{
48
        register int d;
49
        register ANNcoord diff;
50
        register ANNcoord dist;
51

    
52
        dist = 0;
53
        for (d = 0; d < dim; d++) {
54
                diff = p[d] - q[d];
55
                dist = ANN_SUM(dist, ANN_POW(diff));
56
        }
57
        ANN_FLOP(3*dim)                                        // performance counts
58
        ANN_PTS(1)
59
        ANN_COORD(dim)
60
        return dist;
61
}
62

    
63
//----------------------------------------------------------------------
64
//        annPrintPoint() prints a point to a given output stream.
65
//----------------------------------------------------------------------
66

    
67
void annPrintPt(                                                // print a point
68
        ANNpoint                        pt,                                // the point
69
        int                                        dim,                        // the dimension
70
        std::ostream                &out)                        // output stream
71
{
72
        for (int j = 0; j < dim; j++) {
73
                out << pt[j];
74
                if (j < dim-1) out << " ";
75
        }
76
}
77

    
78
//----------------------------------------------------------------------
79
//        Point allocation/deallocation:
80
//
81
//                Because points (somewhat like strings in C) are stored
82
//                as pointers.  Consequently, creating and destroying
83
//                copies of points may require storage allocation.  These
84
//                procedures do this.
85
//
86
//                annAllocPt() and annDeallocPt() allocate a deallocate
87
//                storage for a single point, and return a pointer to it.
88
//
89
//                annAllocPts() allocates an array of points as well a place
90
//                to store their coordinates, and initializes the points to
91
//                point to their respective coordinates.  It allocates point
92
//                storage in a contiguous block large enough to store all the
93
//                points.  It performs no initialization.
94
//
95
//                annDeallocPts() should only be used on point arrays allocated
96
//                by annAllocPts since it assumes that points are allocated in
97
//                a block.
98
//
99
//                annCopyPt() copies a point taking care to allocate storage
100
//                for the new point.
101
//
102
//                annAssignRect() assigns the coordinates of one rectangle to
103
//                another.  The two rectangles must have the same dimension
104
//                (and it is not possible to test this here).
105
//----------------------------------------------------------------------
106

    
107
ANNpoint annAllocPt(int dim, ANNcoord c)                // allocate 1 point
108
{
109
        ANNpoint p = new ANNcoord[dim];
110
        for (int i = 0; i < dim; i++) p[i] = c;
111
        return p;
112
}
113
   
114
ANNpointArray annAllocPts(int n, int dim)                // allocate n pts in dim
115
{
116
        ANNpointArray pa = new ANNpoint[n];                        // allocate points
117
        ANNpoint          p  = new ANNcoord[n*dim];                // allocate space for coords
118
        for (int i = 0; i < n; i++) {
119
                pa[i] = &(p[i*dim]);
120
        }
121
        return pa;
122
}
123

    
124
void annDeallocPt(ANNpoint &p)                                        // deallocate 1 point
125
{
126
        delete [] p;
127
        p = NULL;
128
}
129
   
130
void annDeallocPts(ANNpointArray &pa)                        // deallocate points
131
{
132
        delete [] pa[0];                                                        // dealloc coordinate storage
133
        delete [] pa;                                                                // dealloc points
134
        pa = NULL;
135
}
136
   
137
ANNpoint annCopyPt(int dim, ANNpoint source)        // copy point
138
{
139
        ANNpoint p = new ANNcoord[dim];
140
        for (int i = 0; i < dim; i++) p[i] = source[i];
141
        return p;
142
}
143
   
144
                                                                                                // assign one rect to another
145
void annAssignRect(int dim, ANNorthRect &dest, const ANNorthRect &source)
146
{
147
        for (int i = 0; i < dim; i++) {
148
                dest.lo[i] = source.lo[i];
149
                dest.hi[i] = source.hi[i];
150
        }
151
}
152

    
153
                                                                                                // is point inside rectangle?
154
ANNbool ANNorthRect::inside(int dim, ANNpoint p)
155
{
156
        for (int i = 0; i < dim; i++) {
157
                if (p[i] < lo[i] || p[i] > hi[i]) return ANNfalse;
158
        }
159
        return ANNtrue;
160
}
161

    
162
//----------------------------------------------------------------------
163
//        Error handler
164
//----------------------------------------------------------------------
165

    
166
void annError(char *msg, ANNerr level)
167
{
168
        if (level == ANNabort) {
169
                cerr << "ANN: ERROR------->" << msg << "<-------------ERROR\n";
170
                exit(1);
171
        }
172
        else {
173
                cerr << "ANN: WARNING----->" << msg << "<-------------WARNING\n";
174
        }
175
}
176

    
177
//----------------------------------------------------------------------
178
//        Limit on number of points visited
179
//                We have an option for terminating the search early if the
180
//                number of points visited exceeds some threshold.  If the
181
//                threshold is 0 (its default)  this means there is no limit
182
//                and the algorithm applies its normal termination condition.
183
//                This is for applications where there are real time constraints
184
//                on the running time of the algorithm.
185
//----------------------------------------------------------------------
186

    
187
int        ANNmaxPtsVisited = 0;        // maximum number of pts visited
188
int        ANNptsVisited;                        // number of pts visited in search
189

    
190
//----------------------------------------------------------------------
191
//        Global function declarations
192
//----------------------------------------------------------------------
193

    
194
void annMaxPtsVisit(                        // set limit on max. pts to visit in search
195
        int                                        maxPts)                        // the limit
196
{
197
        ANNmaxPtsVisited = maxPts;
198
}