root / rgbdslam / gicp / ann_1.1.1 / src / kd_util.cpp @ 9240aaa3
History | View | Annotate | Download (14.7 KB)
1 |
//----------------------------------------------------------------------
|
---|---|
2 |
// File: kd_util.cpp
|
3 |
// Programmer: Sunil Arya and David Mount
|
4 |
// Description: Common utilities for kd-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 "kd_util.h" // kd-utility declarations |
26 |
|
27 |
#include <ANN/ANNperf.h> // performance evaluation |
28 |
|
29 |
//----------------------------------------------------------------------
|
30 |
// The following routines are utility functions for manipulating
|
31 |
// points sets, used in determining splitting planes for kd-tree
|
32 |
// construction.
|
33 |
//----------------------------------------------------------------------
|
34 |
|
35 |
//----------------------------------------------------------------------
|
36 |
// NOTE: Virtually all point indexing is done through an index (i.e.
|
37 |
// permutation) array pidx. Consequently, a reference to the d-th
|
38 |
// coordinate of the i-th point is pa[pidx[i]][d]. The macro PA(i,d)
|
39 |
// is a shorthand for this.
|
40 |
//----------------------------------------------------------------------
|
41 |
// standard 2-d indirect indexing
|
42 |
#define PA(i,d) (pa[pidx[(i)]][(d)])
|
43 |
// accessing a single point
|
44 |
#define PP(i) (pa[pidx[(i)]])
|
45 |
|
46 |
//----------------------------------------------------------------------
|
47 |
// annAspectRatio
|
48 |
// Compute the aspect ratio (ratio of longest to shortest side)
|
49 |
// of a rectangle.
|
50 |
//----------------------------------------------------------------------
|
51 |
|
52 |
double annAspectRatio(
|
53 |
int dim, // dimension |
54 |
const ANNorthRect &bnd_box) // bounding cube |
55 |
{ |
56 |
ANNcoord length = bnd_box.hi[0] - bnd_box.lo[0]; |
57 |
ANNcoord min_length = length; // min side length
|
58 |
ANNcoord max_length = length; // max side length
|
59 |
for (int d = 0; d < dim; d++) { |
60 |
length = bnd_box.hi[d] - bnd_box.lo[d]; |
61 |
if (length < min_length) min_length = length;
|
62 |
if (length > max_length) max_length = length;
|
63 |
} |
64 |
return max_length/min_length;
|
65 |
} |
66 |
|
67 |
//----------------------------------------------------------------------
|
68 |
// annEnclRect, annEnclCube
|
69 |
// These utilities compute the smallest rectangle and cube enclosing
|
70 |
// a set of points, respectively.
|
71 |
//----------------------------------------------------------------------
|
72 |
|
73 |
void annEnclRect(
|
74 |
ANNpointArray pa, // point array
|
75 |
ANNidxArray pidx, // point indices
|
76 |
int n, // number of points |
77 |
int dim, // dimension |
78 |
ANNorthRect &bnds) // bounding cube (returned)
|
79 |
{ |
80 |
for (int d = 0; d < dim; d++) { // find smallest enclosing rectangle |
81 |
ANNcoord lo_bnd = PA(0,d); // lower bound on dimension d |
82 |
ANNcoord hi_bnd = PA(0,d); // upper bound on dimension d |
83 |
for (int i = 0; i < n; i++) { |
84 |
if (PA(i,d) < lo_bnd) lo_bnd = PA(i,d);
|
85 |
else if (PA(i,d) > hi_bnd) hi_bnd = PA(i,d); |
86 |
} |
87 |
bnds.lo[d] = lo_bnd; |
88 |
bnds.hi[d] = hi_bnd; |
89 |
} |
90 |
} |
91 |
|
92 |
void annEnclCube( // compute smallest enclosing cube |
93 |
ANNpointArray pa, // point array
|
94 |
ANNidxArray pidx, // point indices
|
95 |
int n, // number of points |
96 |
int dim, // dimension |
97 |
ANNorthRect &bnds) // bounding cube (returned)
|
98 |
{ |
99 |
int d;
|
100 |
// compute smallest enclosing rect
|
101 |
annEnclRect(pa, pidx, n, dim, bnds); |
102 |
|
103 |
ANNcoord max_len = 0; // max length of any side |
104 |
for (d = 0; d < dim; d++) { // determine max side length |
105 |
ANNcoord len = bnds.hi[d] - bnds.lo[d]; |
106 |
if (len > max_len) { // update max_len if longest |
107 |
max_len = len; |
108 |
} |
109 |
} |
110 |
for (d = 0; d < dim; d++) { // grow sides to match max |
111 |
ANNcoord len = bnds.hi[d] - bnds.lo[d]; |
112 |
ANNcoord half_diff = (max_len - len) / 2;
|
113 |
bnds.lo[d] -= half_diff; |
114 |
bnds.hi[d] += half_diff; |
115 |
} |
116 |
} |
117 |
|
118 |
//----------------------------------------------------------------------
|
119 |
// annBoxDistance - utility routine which computes distance from point to
|
120 |
// box (Note: most distances to boxes are computed using incremental
|
121 |
// distance updates, not this function.)
|
122 |
//----------------------------------------------------------------------
|
123 |
|
124 |
ANNdist annBoxDistance( // compute distance from point to box
|
125 |
const ANNpoint q, // the point |
126 |
const ANNpoint lo, // low point of box |
127 |
const ANNpoint hi, // high point of box |
128 |
int dim) // dimension of space |
129 |
{ |
130 |
register ANNdist dist = 0.0; // sum of squared distances |
131 |
register ANNdist t;
|
132 |
|
133 |
for (register int d = 0; d < dim; d++) { |
134 |
if (q[d] < lo[d]) { // q is left of box |
135 |
t = ANNdist(lo[d]) - ANNdist(q[d]); |
136 |
dist = ANN_SUM(dist, ANN_POW(t)); |
137 |
} |
138 |
else if (q[d] > hi[d]) { // q is right of box |
139 |
t = ANNdist(q[d]) - ANNdist(hi[d]); |
140 |
dist = ANN_SUM(dist, ANN_POW(t)); |
141 |
} |
142 |
} |
143 |
ANN_FLOP(4*dim) // increment floating op count |
144 |
|
145 |
return dist;
|
146 |
} |
147 |
|
148 |
//----------------------------------------------------------------------
|
149 |
// annSpread - find spread along given dimension
|
150 |
// annMinMax - find min and max coordinates along given dimension
|
151 |
// annMaxSpread - find dimension of max spread
|
152 |
//----------------------------------------------------------------------
|
153 |
|
154 |
ANNcoord annSpread( // compute point spread along dimension
|
155 |
ANNpointArray pa, // point array
|
156 |
ANNidxArray pidx, // point indices
|
157 |
int n, // number of points |
158 |
int d) // dimension to check |
159 |
{ |
160 |
ANNcoord min = PA(0,d); // compute max and min coords |
161 |
ANNcoord max = PA(0,d);
|
162 |
for (int i = 1; i < n; i++) { |
163 |
ANNcoord c = PA(i,d); |
164 |
if (c < min) min = c;
|
165 |
else if (c > max) max = c; |
166 |
} |
167 |
return (max - min); // total spread is difference |
168 |
} |
169 |
|
170 |
void annMinMax( // compute min and max coordinates along dim |
171 |
ANNpointArray pa, // point array
|
172 |
ANNidxArray pidx, // point indices
|
173 |
int n, // number of points |
174 |
int d, // dimension to check |
175 |
ANNcoord &min, // minimum value (returned)
|
176 |
ANNcoord &max) // maximum value (returned)
|
177 |
{ |
178 |
min = PA(0,d); // compute max and min coords |
179 |
max = PA(0,d);
|
180 |
for (int i = 1; i < n; i++) { |
181 |
ANNcoord c = PA(i,d); |
182 |
if (c < min) min = c;
|
183 |
else if (c > max) max = c; |
184 |
} |
185 |
} |
186 |
|
187 |
int annMaxSpread( // compute dimension of max spread |
188 |
ANNpointArray pa, // point array
|
189 |
ANNidxArray pidx, // point indices
|
190 |
int n, // number of points |
191 |
int dim) // dimension of space |
192 |
{ |
193 |
int max_dim = 0; // dimension of max spread |
194 |
ANNcoord max_spr = 0; // amount of max spread |
195 |
|
196 |
if (n == 0) return max_dim; // no points, who cares? |
197 |
|
198 |
for (int d = 0; d < dim; d++) { // compute spread along each dim |
199 |
ANNcoord spr = annSpread(pa, pidx, n, d); |
200 |
if (spr > max_spr) { // bigger than current max |
201 |
max_spr = spr; |
202 |
max_dim = d; |
203 |
} |
204 |
} |
205 |
return max_dim;
|
206 |
} |
207 |
|
208 |
//----------------------------------------------------------------------
|
209 |
// annMedianSplit - split point array about its median
|
210 |
// Splits a subarray of points pa[0..n] about an element of given
|
211 |
// rank (median: n_lo = n/2) with respect to dimension d. It places
|
212 |
// the element of rank n_lo-1 correctly (because our splitting rule
|
213 |
// takes the mean of these two). On exit, the array is permuted so
|
214 |
// that:
|
215 |
//
|
216 |
// pa[0..n_lo-2][d] <= pa[n_lo-1][d] <= pa[n_lo][d] <= pa[n_lo+1..n-1][d].
|
217 |
//
|
218 |
// The mean of pa[n_lo-1][d] and pa[n_lo][d] is returned as the
|
219 |
// splitting value.
|
220 |
//
|
221 |
// All indexing is done indirectly through the index array pidx.
|
222 |
//
|
223 |
// This function uses the well known selection algorithm due to
|
224 |
// C.A.R. Hoare.
|
225 |
//----------------------------------------------------------------------
|
226 |
|
227 |
// swap two points in pa array
|
228 |
#define PASWAP(a,b) { int tmp = pidx[a]; pidx[a] = pidx[b]; pidx[b] = tmp; } |
229 |
|
230 |
void annMedianSplit(
|
231 |
ANNpointArray pa, // points to split
|
232 |
ANNidxArray pidx, // point indices
|
233 |
int n, // number of points |
234 |
int d, // dimension along which to split |
235 |
ANNcoord &cv, // cutting value
|
236 |
int n_lo) // split into n_lo and n-n_lo |
237 |
{ |
238 |
int l = 0; // left end of current subarray |
239 |
int r = n-1; // right end of current subarray |
240 |
while (l < r) {
|
241 |
register int i = (r+l)/2; // select middle as pivot |
242 |
register int k; |
243 |
|
244 |
if (PA(i,d) > PA(r,d)) // make sure last > pivot |
245 |
PASWAP(i,r) |
246 |
PASWAP(l,i); // move pivot to first position
|
247 |
|
248 |
ANNcoord c = PA(l,d); // pivot value
|
249 |
i = l; |
250 |
k = r; |
251 |
for(;;) { // pivot about c |
252 |
while (PA(++i,d) < c) ;
|
253 |
while (PA(--k,d) > c) ;
|
254 |
if (i < k) PASWAP(i,k) else break; |
255 |
} |
256 |
PASWAP(l,k); // pivot winds up in location k
|
257 |
|
258 |
if (k > n_lo) r = k-1; // recurse on proper subarray |
259 |
else if (k < n_lo) l = k+1; |
260 |
else break; // got the median exactly |
261 |
} |
262 |
if (n_lo > 0) { // search for next smaller item |
263 |
ANNcoord c = PA(0,d); // candidate for max |
264 |
int k = 0; // candidate's index |
265 |
for (int i = 1; i < n_lo; i++) { |
266 |
if (PA(i,d) > c) {
|
267 |
c = PA(i,d); |
268 |
k = i; |
269 |
} |
270 |
} |
271 |
PASWAP(n_lo-1, k); // max among pa[0..n_lo-1] to pa[n_lo-1] |
272 |
} |
273 |
// cut value is midpoint value
|
274 |
cv = (PA(n_lo-1,d) + PA(n_lo,d))/2.0; |
275 |
} |
276 |
|
277 |
//----------------------------------------------------------------------
|
278 |
// annPlaneSplit - split point array about a cutting plane
|
279 |
// Split the points in an array about a given plane along a
|
280 |
// given cutting dimension. On exit, br1 and br2 are set so
|
281 |
// that:
|
282 |
//
|
283 |
// pa[ 0 ..br1-1] < cv
|
284 |
// pa[br1..br2-1] == cv
|
285 |
// pa[br2.. n -1] > cv
|
286 |
//
|
287 |
// All indexing is done indirectly through the index array pidx.
|
288 |
//
|
289 |
//----------------------------------------------------------------------
|
290 |
|
291 |
void annPlaneSplit( // split points by a plane |
292 |
ANNpointArray pa, // points to split
|
293 |
ANNidxArray pidx, // point indices
|
294 |
int n, // number of points |
295 |
int d, // dimension along which to split |
296 |
ANNcoord cv, // cutting value
|
297 |
int &br1, // first break (values < cv) |
298 |
int &br2) // second break (values == cv) |
299 |
{ |
300 |
int l = 0; |
301 |
int r = n-1; |
302 |
for(;;) { // partition pa[0..n-1] about cv |
303 |
while (l < n && PA(l,d) < cv) l++;
|
304 |
while (r >= 0 && PA(r,d) >= cv) r--; |
305 |
if (l > r) break; |
306 |
PASWAP(l,r); |
307 |
l++; r--; |
308 |
} |
309 |
br1 = l; // now: pa[0..br1-1] < cv <= pa[br1..n-1]
|
310 |
r = n-1;
|
311 |
for(;;) { // partition pa[br1..n-1] about cv |
312 |
while (l < n && PA(l,d) <= cv) l++;
|
313 |
while (r >= br1 && PA(r,d) > cv) r--;
|
314 |
if (l > r) break; |
315 |
PASWAP(l,r); |
316 |
l++; r--; |
317 |
} |
318 |
br2 = l; // now: pa[br1..br2-1] == cv < pa[br2..n-1]
|
319 |
} |
320 |
|
321 |
|
322 |
//----------------------------------------------------------------------
|
323 |
// annBoxSplit - split point array about a orthogonal rectangle
|
324 |
// Split the points in an array about a given orthogonal
|
325 |
// rectangle. On exit, n_in is set to the number of points
|
326 |
// that are inside (or on the boundary of) the rectangle.
|
327 |
//
|
328 |
// All indexing is done indirectly through the index array pidx.
|
329 |
//
|
330 |
//----------------------------------------------------------------------
|
331 |
|
332 |
void annBoxSplit( // split points by a box |
333 |
ANNpointArray pa, // points to split
|
334 |
ANNidxArray pidx, // point indices
|
335 |
int n, // number of points |
336 |
int dim, // dimension of space |
337 |
ANNorthRect &box, // the box
|
338 |
int &n_in) // number of points inside (returned) |
339 |
{ |
340 |
int l = 0; |
341 |
int r = n-1; |
342 |
for(;;) { // partition pa[0..n-1] about box |
343 |
while (l < n && box.inside(dim, PP(l))) l++;
|
344 |
while (r >= 0 && !box.inside(dim, PP(r))) r--; |
345 |
if (l > r) break; |
346 |
PASWAP(l,r); |
347 |
l++; r--; |
348 |
} |
349 |
n_in = l; // now: pa[0..n_in-1] inside and rest outside
|
350 |
} |
351 |
|
352 |
//----------------------------------------------------------------------
|
353 |
// annSplitBalance - compute balance factor for a given plane split
|
354 |
// Balance factor is defined as the number of points lying
|
355 |
// below the splitting value minus n/2 (median). Thus, a
|
356 |
// median split has balance 0, left of this is negative and
|
357 |
// right of this is positive. (The points are unchanged.)
|
358 |
//----------------------------------------------------------------------
|
359 |
|
360 |
int annSplitBalance( // determine balance factor of a split |
361 |
ANNpointArray pa, // points to split
|
362 |
ANNidxArray pidx, // point indices
|
363 |
int n, // number of points |
364 |
int d, // dimension along which to split |
365 |
ANNcoord cv) // cutting value
|
366 |
{ |
367 |
int n_lo = 0; |
368 |
for(int i = 0; i < n; i++) { // count number less than cv |
369 |
if (PA(i,d) < cv) n_lo++;
|
370 |
} |
371 |
return n_lo - n/2; |
372 |
} |
373 |
|
374 |
//----------------------------------------------------------------------
|
375 |
// annBox2Bnds - convert bounding box to list of bounds
|
376 |
// Given two boxes, an inner box enclosed within a bounding
|
377 |
// box, this routine determines all the sides for which the
|
378 |
// inner box is strictly contained with the bounding box,
|
379 |
// and adds an appropriate entry to a list of bounds. Then
|
380 |
// we allocate storage for the final list of bounds, and return
|
381 |
// the resulting list and its size.
|
382 |
//----------------------------------------------------------------------
|
383 |
|
384 |
void annBox2Bnds( // convert inner box to bounds |
385 |
const ANNorthRect &inner_box, // inner box |
386 |
const ANNorthRect &bnd_box, // enclosing box |
387 |
int dim, // dimension of space |
388 |
int &n_bnds, // number of bounds (returned) |
389 |
ANNorthHSArray &bnds) // bounds array (returned)
|
390 |
{ |
391 |
int i;
|
392 |
n_bnds = 0; // count number of bounds |
393 |
for (i = 0; i < dim; i++) { |
394 |
if (inner_box.lo[i] > bnd_box.lo[i]) // low bound is inside |
395 |
n_bnds++; |
396 |
if (inner_box.hi[i] < bnd_box.hi[i]) // high bound is inside |
397 |
n_bnds++; |
398 |
} |
399 |
|
400 |
bnds = new ANNorthHalfSpace[n_bnds]; // allocate appropriate size |
401 |
|
402 |
int j = 0; |
403 |
for (i = 0; i < dim; i++) { // fill the array |
404 |
if (inner_box.lo[i] > bnd_box.lo[i]) {
|
405 |
bnds[j].cd = i; |
406 |
bnds[j].cv = inner_box.lo[i]; |
407 |
bnds[j].sd = +1;
|
408 |
j++; |
409 |
} |
410 |
if (inner_box.hi[i] < bnd_box.hi[i]) {
|
411 |
bnds[j].cd = i; |
412 |
bnds[j].cv = inner_box.hi[i]; |
413 |
bnds[j].sd = -1;
|
414 |
j++; |
415 |
} |
416 |
} |
417 |
} |
418 |
|
419 |
//----------------------------------------------------------------------
|
420 |
// annBnds2Box - convert list of bounds to bounding box
|
421 |
// Given an enclosing box and a list of bounds, this routine
|
422 |
// computes the corresponding inner box. It is assumed that
|
423 |
// the box points have been allocated already.
|
424 |
//----------------------------------------------------------------------
|
425 |
|
426 |
void annBnds2Box(
|
427 |
const ANNorthRect &bnd_box, // enclosing box |
428 |
int dim, // dimension of space |
429 |
int n_bnds, // number of bounds |
430 |
ANNorthHSArray bnds, // bounds array
|
431 |
ANNorthRect &inner_box) // inner box (returned)
|
432 |
{ |
433 |
annAssignRect(dim, inner_box, bnd_box); // copy bounding box to inner
|
434 |
|
435 |
for (int i = 0; i < n_bnds; i++) { |
436 |
bnds[i].project(inner_box.lo); // project each endpoint
|
437 |
bnds[i].project(inner_box.hi); |
438 |
} |
439 |
} |