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 |
} |