1
2 /* autopano-sift, Automatic panorama image creation
3 * Copyright (C) 2004 -- Sebastian Nowozin
4 *
5 * This program is free software released under the GNU General Public
6 * License, which is included in this software package (doc/LICENSE).
7 */
8
9 /* KDTree.cs
10 *
11 * A vanilla k-d tree implementation.
12 *
13 * (C) Copyright 2004 -- Sebastian Nowozin (nowozin@cs.tu-berlin.de)
14 *
15 * Based on "An introductory tutorial on kd-trees" by Andrew W. Moore,
16 * available at http://www.ri.cmu.edu/pubs/pub_2818.html
17 */
18
19 #include "AutoPanoSift.h"
20
21
22 /* SortedLimitedList replacement by Eric Engle
23 *
24 * Changes:
25 * Modified Add(), and implemented Set to handle node setting semantics
26 * not provided by the .NET library.
27 *
28 * Performance notes:
29 * This routine uses a simple insertion sort to handle calls to Add.
30 * Each element is compared from right to left. If obj is smaller than the
31 * current array object, that object is slid to the right. Otherwise the hole
32 * from the last slide operation is used to hold obj.
33 * Most of the calls to Add() will return -1, i.e. in the normal case only a
34 * fraction of the items will be smaller than the largest item on the list.
35 * This common case is recognized in the first comparison. Iteration occurs
36 * only for those items that belong on the list, so for the normal case self
37 * operation is faster than it's strictly linear performance would suggest.
38 */
39
SortedLimitedList_new0()40 SortedLimitedList* SortedLimitedList_new0 ()
41 {
42 SortedLimitedList* self = (SortedLimitedList*)malloc(sizeof(SortedLimitedList));
43 return self;
44 }
45
SortedLimitedList_delete(SortedLimitedList * self)46 void SortedLimitedList_delete (SortedLimitedList* self)
47 {
48 ArrayList_delete(&self->base);
49 }
50
SortedLimitedList_new(int maxElements,void * deletefn)51 SortedLimitedList* SortedLimitedList_new (int maxElements, void* deletefn)
52 {
53 SortedLimitedList* self = SortedLimitedList_new0();
54 ArrayList_init((ArrayList*)self, deletefn);
55 self->max = maxElements;
56 self->deletefn = (void (*)(void *)) deletefn;
57 return self;
58 }
59
60 // Sets the argument index to the argument object.
61 // Replaces the node if it already exists,
62 // adds a new node if at the end of the list,
63 // does nothing otherwise.
SortedLimitedList_SetItem(SortedLimitedList * self,int idx,void * value)64 void SortedLimitedList_SetItem (SortedLimitedList* self, int idx, void* value)
65 {
66 if (idx < ArrayList_Count(&self->base)) {
67 ArrayList_SetItem(&self->base, idx, value);
68 } else if (idx == ArrayList_Count(&self->base)) { // TODO: should check for max?
69 ArrayList_AddItem(&self->base, value);
70 } else {
71 if (self->deletefn) {
72 self->deletefn(value);
73 }
74 }
75 }
76
SortedLimitedList_Count(SortedLimitedList * self)77 int SortedLimitedList_Count (SortedLimitedList* self)
78 {
79 return ArrayList_Count(&self->base);
80 }
81
SortedLimitedList_GetItem(SortedLimitedList * self,int i)82 void* SortedLimitedList_GetItem (SortedLimitedList* self, int i)
83 {
84 return ArrayList_GetItem(&self->base, i);
85 }
86
SortedLimitedList_RemoveAt(SortedLimitedList * self,int i)87 void SortedLimitedList_RemoveAt (SortedLimitedList* self, int i)
88 {
89 return ArrayList_RemoveAt(&self->base, i);
90 }
91
92
93 // Processes list from right to left, sliding each node that is greater
94 // than 'self' to the right. The loop ends when either the first node is
95 // reached, meaning obj is a new minimum, or it's proper sorted position
96 // in the list is reached.
97 // Returns position of obj or -1 if obj was not placed.
98
SortedLimitedList_AddItem(SortedLimitedList * self,void * value)99 int SortedLimitedList_AddItem (SortedLimitedList* self, void* value)
100 {
101 int pos = ArrayList_Count(&self->base);
102 while (pos > 0 && self->comparator.compareTo(&self->comparator, ArrayList_GetItem(&self->base, pos-1), value) >= 0) {
103 if (pos < self->max) {
104 if (pos==self->max-1) {
105 if (ArrayList_Count(&self->base) == self->max) {
106 if (self->deletefn) {
107 self->deletefn(ArrayList_GetItem(&self->base, pos));
108 }
109 }
110 }
111 SortedLimitedList_SetItem(self, pos, ArrayList_GetItem(&self->base, pos-1));
112 }
113 pos --;
114 }
115
116 if (pos < self->max) {
117 if (pos==self->max-1) {
118 if (ArrayList_Count(&self->base) == self->max) {
119 if (self->deletefn) {
120 self->deletefn(ArrayList_GetItem(&self->base, pos));
121 }
122 }
123 }
124 SortedLimitedList_SetItem(self, pos, value);
125 } else {
126 if (self->deletefn) {
127 self->deletefn(value);
128 }
129 pos = -1;
130 }
131
132 return pos;
133 }
134
135
136
137
IKDTreeDomain_GetDimensionCount(IKDTreeDomain * self)138 int IKDTreeDomain_GetDimensionCount(IKDTreeDomain* self)
139 {
140 return self->getDimensionCount(self);
141 }
142
IKDTreeDomain_GetDimensionElement(IKDTreeDomain * self,int dim)143 int IKDTreeDomain_GetDimensionElement(IKDTreeDomain* self, int dim)
144 {
145 return self->getDimensionElement(self, dim);
146 }
147
148
KDTree_new0()149 KDTree* KDTree_new0 ()
150 {
151 KDTree* self = (KDTree*)malloc(sizeof(KDTree));
152 self->left = NULL;
153 self->right = NULL;
154 self->dr = NULL;
155 return self;
156 }
157
KDTree_delete(KDTree * self)158 void KDTree_delete(KDTree* self)
159 {
160 if (self) {
161 KDTree_delete(self->left);
162 KDTree_delete(self->right);
163 // TODO: KDTreeDomain_delete(self->dr)?
164 free(self);
165 }
166 }
167
KDTreeBestEntry_Equals(KDTreeBestEntry * be1,KDTreeBestEntry * be2)168 bool KDTreeBestEntry_Equals (KDTreeBestEntry* be1, KDTreeBestEntry* be2)
169 {
170 return (be1->neighbour == be2->neighbour);
171 }
172
KDTreeBestEntry_new0()173 KDTreeBestEntry* KDTreeBestEntry_new0()
174 {
175 KDTreeBestEntry* self = (KDTreeBestEntry*)malloc(sizeof(KDTreeBestEntry));
176 return self;
177 }
178
KDTreeBestEntry_delete(KDTreeBestEntry * self)179 void KDTreeBestEntry_delete(KDTreeBestEntry* self)
180 {
181 if (self) {
182 free(self);
183 }
184 }
185
KDTreeBestEntry_new3(IKDTreeDomain * neighbour,int distanceSq,bool squared)186 KDTreeBestEntry* KDTreeBestEntry_new3 (IKDTreeDomain* neighbour, int distanceSq, bool squared)
187 {
188 KDTreeBestEntry* self= KDTreeBestEntry_new0();
189 self->neighbour = neighbour;
190 self->distanceSq = distanceSq;
191 self->squared = true;
192 return self;
193 }
194
KDTreeBestEntry_new(IKDTreeDomain * neighbour,double dist)195 KDTreeBestEntry* KDTreeBestEntry_new (IKDTreeDomain* neighbour, double dist)
196 {
197 KDTreeBestEntry* self= KDTreeBestEntry_new0();
198 self->neighbour = neighbour;
199 self->distance = dist;
200 self->squared = false;
201 return self;
202 }
203
KDTreeBestEntry_Neighbour(KDTreeBestEntry * self)204 IKDTreeDomain* KDTreeBestEntry_Neighbour (KDTreeBestEntry* self)
205 {
206 return self->neighbour;
207 }
208
KDTreeBestEntry_CompareTo(IComparator * self,KDTreeBestEntry * be1,KDTreeBestEntry * be2)209 int KDTreeBestEntry_CompareTo (IComparator* self, KDTreeBestEntry* be1, KDTreeBestEntry* be2)
210 {
211 if (be1->squared) {
212 if (be1->distanceSq < be2->distanceSq)
213 return (-1);
214 else if (be1->distanceSq > be2->distanceSq)
215 return (1);
216
217 return (0);
218 } else {
219 if (be1->distance < be2->distance)
220 return (-1);
221 else if (be1->distance > be2->distance)
222 return (1);
223
224 return (0);
225 }
226 }
227
KDTreeHREntry_new0()228 KDTreeHREntry* KDTreeHREntry_new0()
229 {
230 KDTreeHREntry* self = (KDTreeHREntry*)malloc(sizeof(KDTreeHREntry));
231 self->rect = NULL;
232 self->tree = NULL;
233 self->pivot = NULL;
234 return self;
235 }
236
KDTreeHREntry_delete(KDTreeHREntry * self)237 void KDTreeHREntry_delete(KDTreeHREntry* self)
238 {
239 if (self) {
240 HyperRectangle_unref(self->rect);
241 //KDTree_delete(self->tree);
242 //IKDTreeDomain_delete(self->pivot);
243 free(self);
244 }
245 }
246
247
KDTreeHREntry_new(HyperRectangle * rect,KDTree * tree,IKDTreeDomain * pivot,double dist)248 KDTreeHREntry* KDTreeHREntry_new (HyperRectangle* rect, KDTree* tree, IKDTreeDomain* pivot,
249 double dist)
250 {
251 KDTreeHREntry* self = KDTreeHREntry_new0();
252 self->rect = HyperRectangle_ref(rect);
253 self->tree = tree;
254 self->pivot = pivot;
255 self->dist = dist;
256 return self;
257 }
258
KDTreeHREntry_CompareTo(IComparator * self,KDTreeHREntry * hre1,KDTreeHREntry * hre2)259 int KDTreeHREntry_CompareTo (IComparator* self, KDTreeHREntry* hre1, KDTreeHREntry* hre2)
260 {
261 if (hre1->dist < hre2->dist)
262 return (-1);
263 else if (hre1->dist > hre2->dist)
264 return (1);
265
266 return (0);
267 }
268
HyperRectangle_new0()269 HyperRectangle* HyperRectangle_new0()
270 {
271 HyperRectangle* self = (HyperRectangle*)malloc(sizeof(HyperRectangle));
272 self->ref = 0;
273 return self;
274 }
275
HyperRectangle_delete(HyperRectangle * self)276 void HyperRectangle_delete (HyperRectangle* self)
277 {
278 if (self) {
279 if (self->leftTop) {
280 free(self->leftTop);
281 self->leftTop = NULL;
282 }
283 if (self->rightBottom) {
284 free(self->rightBottom);
285 self->rightBottom = NULL;
286 }
287 self->dim = 0;
288 free(self);
289 }
290 }
291
HyperRectangle_ref(HyperRectangle * self)292 HyperRectangle* HyperRectangle_ref (HyperRectangle* self)
293 {
294 self->ref++;
295 return self;
296 }
297
HyperRectangle_unref(HyperRectangle * self)298 void HyperRectangle_unref (HyperRectangle* self)
299 {
300 self->ref--;
301 if (self->ref == 0) {
302 //HyperRectangle_delete(self);
303 }
304 }
305
306
HyperRectangle_new(int dim)307 HyperRectangle* HyperRectangle_new (int dim)
308 {
309 HyperRectangle* self = HyperRectangle_new0();
310 self->dim = dim;
311 self->leftTop = (int*)malloc(dim*sizeof(int));
312 self->rightBottom = (int*)malloc(dim*sizeof(int));
313 return self;
314 }
315
HyperRectangle_clone(HyperRectangle * self)316 HyperRectangle* HyperRectangle_clone (HyperRectangle* self)
317 {
318 HyperRectangle* rec = HyperRectangle_new (self->dim);
319
320 int n;
321 for ( n = 0 ; n < self->dim ; ++n) {
322 rec->leftTop[n] = self->leftTop[n];
323 rec->rightBottom[n] = self->rightBottom[n];
324 }
325
326 return (rec);
327 }
328
HyperRectangle_CreateUniverseRectangle(int dim)329 HyperRectangle* HyperRectangle_CreateUniverseRectangle (int dim)
330 {
331 HyperRectangle* rec = HyperRectangle_new (dim);
332
333 int n;
334 for ( n = 0 ; n < dim ; ++n) {
335 rec->leftTop[n] = INT_MIN;
336 rec->rightBottom[n] = INT_MAX;
337 }
338
339 return (rec);
340 }
341
HyperRectangle_SplitAt(HyperRectangle * self,int splitDim,int splitVal)342 HyperRectangle* HyperRectangle_SplitAt (HyperRectangle* self, int splitDim, int splitVal)
343 {
344 if (self->leftTop[splitDim] >= splitVal || self->rightBottom[splitDim] < splitVal)
345 FatalError("SplitAt with splitpoint outside rec");
346
347 HyperRectangle* r2 = HyperRectangle_clone(self);
348 self->rightBottom[splitDim] = splitVal;
349 r2->leftTop[splitDim] = splitVal;
350
351 return (r2);
352 }
353
HyperRectangle_IsIn(HyperRectangle * self,IKDTreeDomain * target)354 bool HyperRectangle_IsIn (HyperRectangle* self, IKDTreeDomain* target)
355 {
356 if (IKDTreeDomain_GetDimensionCount(target) != self->dim)
357 FatalError ("IsIn dimension mismatch");
358
359 int n;
360 for (n = 0 ; n < self->dim ; ++n) {
361 int targD = IKDTreeDomain_GetDimensionElement(target, n);
362
363 if (targD < self->leftTop[n] || targD >= self->rightBottom[n])
364 return (false);
365 }
366
367 return (true);
368 }
369
370 // Return true if any part of this HR is reachable from target by no
371 // more than 'distRad', false otherwise.
372 // The algorithm is specified in the kd-tree paper mentioned at the
373 // top of this file, in section 6-7. But there is a mistake in the
374 // third distinct case, which should read "hrMax" instead of "hrMin".
HyperRectangle_IsInReach(HyperRectangle * self,IKDTreeDomain * target,double distRad)375 bool HyperRectangle_IsInReach (HyperRectangle* self, IKDTreeDomain* target, double distRad)
376 {
377 return (HyperRectangle_Distance (self, target) < distRad);
378 }
379
380 // Return the distance from the nearest point from within the HR to
381 // the target point.
HyperRectangle_Distance(HyperRectangle * self,IKDTreeDomain * target)382 double HyperRectangle_Distance (HyperRectangle* self, IKDTreeDomain* target)
383 {
384 int closestPointN;
385 int distance = 0;
386
387 // first compute the closest point within hr to the target. if
388 // this point is within reach of target, then there is an
389 // intersection between the hypersphere around target with radius
390 // 'dist' and this hyperrectangle.
391 int n;
392 for ( n = 0 ; n < self->dim ; ++n) {
393 int tI = IKDTreeDomain_GetDimensionElement (target, n);
394 int hrMin = self->leftTop[n];
395 int hrMax = self->rightBottom[n];
396
397 closestPointN = 0;
398 if (tI <= hrMin) {
399 closestPointN = hrMin;
400 } else if (tI > hrMin && tI < hrMax) {
401 closestPointN = tI;
402 } else if (tI >= hrMax) {
403 closestPointN = hrMax;
404 }
405
406 int dimDist = tI - closestPointN;
407 distance += dimDist * dimDist;
408 }
409
410 return (sqrt ((double) distance));
411 }
412
413
414 // Find the nearest neighbour to the hyperspace point 'target' within the
415 // kd-tree. After return 'resDist' contains the absolute distance from the
416 // target point. The nearest neighbour is returned.
KDTree_NearestNeighbour(KDTree * self,IKDTreeDomain * target,double * resDist)417 IKDTreeDomain* KDTree_NearestNeighbour (KDTree* self, IKDTreeDomain* target, double* resDist)
418 {
419 ArrayList* hrl = ArrayList_new0(HyperRectangle_delete);
420
421 HyperRectangle* hr =
422 HyperRectangle_CreateUniverseRectangle (IKDTreeDomain_GetDimensionCount(target));
423
424 ArrayList_AddItem(hrl, hr);
425
426 IKDTreeDomain* nearest = KDTree_NearestNeighbourI (self, target, hr,
427 Double_PositiveInfinity, resDist, hrl);
428 *resDist = sqrt (*resDist);
429
430 ArrayList_delete(hrl);
431
432 return (nearest);
433 }
434
435
436 // Internal recursive algorithm for the kd-tree nearest neighbour search.
KDTree_NearestNeighbourI(KDTree * self,IKDTreeDomain * target,HyperRectangle * hr,double maxDistSq,double * resDistSq,ArrayList * hrl)437 IKDTreeDomain* KDTree_NearestNeighbourI (KDTree* self, IKDTreeDomain* target, HyperRectangle* hr,
438 double maxDistSq, double* resDistSq, ArrayList* hrl)
439 {
440 //WriteLine ("C NearestNeighbourI called");
441
442 *resDistSq = Double_PositiveInfinity;
443
444 IKDTreeDomain* pivot = self->dr;
445
446 HyperRectangle* leftHr = hr;
447 HyperRectangle* rightHr = HyperRectangle_SplitAt(leftHr, self->splitDim,
448 IKDTreeDomain_GetDimensionElement (pivot, self->splitDim));
449
450 ArrayList_AddItem(hrl, rightHr);
451
452 HyperRectangle* nearerHr = NULL;
453 HyperRectangle* furtherHr =NULL;
454 KDTree* nearerKd = NULL;
455 KDTree* furtherKd = NULL;
456
457 // step 5-7
458 if (IKDTreeDomain_GetDimensionElement (target, self->splitDim) <=
459 IKDTreeDomain_GetDimensionElement (pivot, self->splitDim))
460 {
461 nearerKd = self->left;
462 nearerHr = leftHr;
463 furtherKd = self->right;
464 furtherHr = rightHr;
465 } else {
466 nearerKd = self->right;
467 nearerHr = rightHr;
468 furtherKd = self->left;
469 furtherHr = leftHr;
470 }
471
472 // step 8
473 IKDTreeDomain* nearest = NULL;
474 double distSq;
475 if (nearerKd == NULL) {
476 distSq = Double_PositiveInfinity;
477 } else {
478 nearest = KDTree_NearestNeighbourI (nearerKd, target, nearerHr,
479 maxDistSq, &distSq, hrl);
480 }
481
482 // step 9
483 maxDistSq = min (maxDistSq, distSq);
484
485 // step 10
486 if (HyperRectangle_IsInReach (furtherHr, target, sqrt (maxDistSq))) {
487 double ptDistSq = KDTree_DistanceSq (pivot, target);
488 if (ptDistSq < distSq) {
489 // steps 10.1.1 to 10.1.3
490 nearest = pivot;
491 distSq = ptDistSq;
492 maxDistSq = distSq;
493 }
494
495 // step 10.2
496 double tempDistSq;
497 IKDTreeDomain* tempNearest = NULL;
498 if (furtherKd == NULL) {
499 tempDistSq = Double_PositiveInfinity;
500 } else {
501 tempNearest = KDTree_NearestNeighbourI (furtherKd, target,
502 furtherHr, maxDistSq, & tempDistSq, hrl);
503 }
504
505 // step 10.3
506 if (tempDistSq < distSq) {
507 nearest = tempNearest;
508 distSq = tempDistSq;
509 }
510 }
511
512 *resDistSq = distSq;
513 return (nearest);
514 }
515
KDTree_NearestNeighbourList(KDTree * self,IKDTreeDomain * target,double * resDist,int q)516 SortedLimitedList* KDTree_NearestNeighbourList (KDTree* self, IKDTreeDomain* target,
517 double* resDist, int q)
518 {
519 ArrayList* hrl = ArrayList_new0(HyperRectangle_delete);
520
521 HyperRectangle* hr =
522 HyperRectangle_CreateUniverseRectangle (IKDTreeDomain_GetDimensionCount(target));
523
524 ArrayList_AddItem(hrl, hr);
525
526 SortedLimitedList* best = SortedLimitedList_new (q, KDTreeBestEntry_delete);
527 best->comparator.compareTo = (int ( *)(IComparator *,const void *,const void *))KDTreeBestEntry_CompareTo;
528
529 /*IKDTreeDomain* nearest = */KDTree_NearestNeighbourListI (self, best, q, target, hr,
530 Double_PositiveInfinity, resDist, hrl);
531 *resDist = sqrt (*resDist);
532
533 int i;
534 for(i=0; i<SortedLimitedList_Count(best); i++) {
535 KDTreeBestEntry* be = (KDTreeBestEntry*) SortedLimitedList_GetItem(best, i);
536 be->distance = sqrt (be->distance);
537 }
538
539 ArrayList_delete(hrl);
540
541 return (best);
542 }
543
544
KDTree_NearestNeighbourListI(KDTree * self,SortedLimitedList * best,int q,IKDTreeDomain * target,HyperRectangle * hr,double maxDistSq,double * resDistSq,ArrayList * hrl)545 IKDTreeDomain* KDTree_NearestNeighbourListI (KDTree* self, SortedLimitedList* best,
546 int q, IKDTreeDomain* target, HyperRectangle* hr, double maxDistSq,
547 double* resDistSq, ArrayList* hrl)
548 {
549 //WriteLine ("C NearestNeighbourI called");
550
551 *resDistSq = Double_PositiveInfinity;
552
553 IKDTreeDomain* pivot = self->dr;
554
555 KDTreeBestEntry* be = KDTreeBestEntry_new (self->dr, KDTree_DistanceSq (target, self->dr));
556 SortedLimitedList_AddItem (best, be);
557
558 HyperRectangle* leftHr = hr;
559 HyperRectangle* rightHr = HyperRectangle_SplitAt (leftHr, self->splitDim,
560 IKDTreeDomain_GetDimensionElement (pivot, self->splitDim));
561
562 ArrayList_AddItem(hrl, rightHr);
563
564 HyperRectangle* nearerHr = NULL;
565 HyperRectangle* furtherHr = NULL;
566 KDTree* nearerKd = NULL;
567 KDTree* furtherKd = NULL;
568
569 // step 5-7
570 if (IKDTreeDomain_GetDimensionElement (target, self->splitDim) <=
571 IKDTreeDomain_GetDimensionElement (pivot, self->splitDim))
572 {
573 nearerKd = self->left;
574 nearerHr = leftHr;
575 furtherKd = self->right;
576 furtherHr = rightHr;
577 } else {
578 nearerKd = self->right;
579 nearerHr = rightHr;
580 furtherKd = self->left;
581 furtherHr = leftHr;
582 }
583
584 // step 8
585 IKDTreeDomain* nearest = NULL;
586 double distSq;
587
588 // No child, bottom reached!
589 if (nearerKd == NULL) {
590 distSq = Double_PositiveInfinity;
591 } else {
592 nearest = KDTree_NearestNeighbourListI (nearerKd, best, q, target, nearerHr,
593 maxDistSq, &distSq, hrl);
594 }
595
596 // step 9
597 //maxDistSq = Math.Min (maxDistSq, distanceSq);
598 if (SortedLimitedList_Count(best) >= q)
599 maxDistSq = ((KDTreeBestEntry*) SortedLimitedList_GetItem(best, q - 1))->distance;
600 else
601 maxDistSq = Double_PositiveInfinity;
602
603 // step 10
604 if (HyperRectangle_IsInReach (furtherHr, target, sqrt (maxDistSq))) {
605 double ptDistSq = KDTree_DistanceSq (pivot, target);
606 if (ptDistSq < distSq) {
607 // steps 10.1.1 to 10.1.3
608 nearest = pivot;
609 distSq = ptDistSq;
610
611 // TODO: use k-element list
612 /*
613 best.Add (new BestEntry (pivot, ptDistSq));
614 best.Sort ();
615 */
616
617 maxDistSq = distSq;
618 }
619
620 // step 10.2
621 double tempDistSq;
622 IKDTreeDomain* tempNearest = NULL;
623 if (furtherKd == NULL) {
624 tempDistSq = Double_PositiveInfinity;
625 } else {
626 tempNearest = KDTree_NearestNeighbourListI (furtherKd, best, q, target,
627 furtherHr, maxDistSq, &tempDistSq, hrl);
628 }
629
630 // step 10.3
631 if (tempDistSq < distSq) {
632 nearest = tempNearest;
633 distSq = tempDistSq;
634 }
635 }
636
637 *resDistSq = distSq;
638 return (nearest);
639 }
640
641 // Limited Best-Bin-First k-d-tree nearest neighbour search.
642 //
643 // (Using the algorithm described in the paper "Shape indexing using
644 // approximate nearest-neighbour search in high-dimensional spaces",
645 // available at http://www.cs.ubc.ca/spider/lowe/papers/cvpr97-abs.html)
646 //
647 // Find the approximate nearest neighbour to the hyperspace point 'target'
648 // within the kd-tree using 'searchSteps' tail recursions at most (each
649 // recursion deciding about one neighbours' fitness).
650 //
651 // After return 'resDist' contains the absolute distance of the
652 // approximate nearest neighbour from the target point. The approximate
653 // nearest neighbour is returned.
KDTree_NearestNeighbourListBBF(KDTree * self,IKDTreeDomain * target,int q,int searchSteps)654 SortedLimitedList* KDTree_NearestNeighbourListBBF (KDTree* self, IKDTreeDomain* target,
655 int q, int searchSteps)
656 {
657 ArrayList* hrl = ArrayList_new0(HyperRectangle_delete);
658
659 HyperRectangle* hr =
660 HyperRectangle_CreateUniverseRectangle (IKDTreeDomain_GetDimensionCount(target));
661
662 ArrayList_AddItem(hrl, hr);
663
664 SortedLimitedList* best = SortedLimitedList_new (q, KDTreeBestEntry_delete);
665 best->comparator.compareTo = (int ( *)(IComparator *,const void *,const void *)) KDTreeBestEntry_CompareTo;
666 SortedLimitedList* searchHr = SortedLimitedList_new (searchSteps, KDTreeHREntry_delete);
667 searchHr->comparator.compareTo = (int ( *)(IComparator *,const void *,const void *))KDTreeHREntry_CompareTo;
668
669 int dummyDist;
670 /*IKDTreeDomain* nearest = */KDTree_NearestNeighbourListBBFI (self, best, q, target, hr,
671 INT_MAX, &dummyDist, searchHr, &searchSteps,
672 hrl);
673
674 SortedLimitedList_delete(searchHr);
675
676 int i;
677 for(i=0; i<SortedLimitedList_Count(best); i++) {
678 KDTreeBestEntry* be = (KDTreeBestEntry*) SortedLimitedList_GetItem(best, i);
679 be->distance = sqrt ((double)be->distanceSq);
680 }
681
682 ArrayList_delete(hrl);
683
684 return (best);
685 }
686
687
KDTree_NearestNeighbourListBBFI(KDTree * self,SortedLimitedList * best,int q,IKDTreeDomain * target,HyperRectangle * hr,int maxDistSq,int * resDistSq,SortedLimitedList * searchHr,int * searchSteps,ArrayList * hrl)688 IKDTreeDomain* KDTree_NearestNeighbourListBBFI (KDTree* self, SortedLimitedList* best,
689 int q, IKDTreeDomain* target, HyperRectangle* hr, int maxDistSq,
690 int* resDistSq, SortedLimitedList* searchHr, int* searchSteps,
691 ArrayList* hrl)
692 {
693 //WriteLine ("C NearestNeighbourI called");
694
695 *resDistSq = INT_MAX;
696
697 IKDTreeDomain* pivot = self->dr;
698
699 KDTreeBestEntry* be = KDTreeBestEntry_new3 (self->dr, KDTree_DistanceSq (target, self->dr), true);
700 SortedLimitedList_AddItem(best, be);
701
702 HyperRectangle* leftHr = hr;
703 HyperRectangle* rightHr = HyperRectangle_SplitAt (leftHr, self->splitDim,
704 IKDTreeDomain_GetDimensionElement (pivot, self->splitDim));
705
706 ArrayList_AddItem(hrl, rightHr);
707
708 HyperRectangle* nearerHr = NULL;
709 HyperRectangle* furtherHr = NULL;
710 KDTree* nearerKd = NULL;
711 KDTree* furtherKd = NULL;
712
713 // step 5-7
714 if (IKDTreeDomain_GetDimensionElement (target, self->splitDim) <=
715 IKDTreeDomain_GetDimensionElement (pivot, self->splitDim))
716 {
717 nearerKd = self->left;
718 nearerHr = leftHr;
719 furtherKd = self->right;
720 furtherHr = rightHr;
721 } else {
722 nearerKd = self->right;
723 nearerHr = rightHr;
724 furtherKd = self->left;
725 furtherHr = leftHr;
726 }
727
728 // step 8
729 IKDTreeDomain* nearest = NULL;
730 int distSq;
731
732 KDTreeHREntry* hre = KDTreeHREntry_new (furtherHr, furtherKd, pivot,
733 HyperRectangle_Distance (furtherHr, target));
734 SortedLimitedList_AddItem (searchHr, hre);
735
736 // No child, bottom reached!
737 if (nearerKd == NULL) {
738 distSq = INT_MAX;
739 } else {
740 nearest = KDTree_NearestNeighbourListBBFI (nearerKd, best, q, target, nearerHr,
741 maxDistSq, &distSq, searchHr, searchSteps, hrl);
742 }
743
744 // step 9
745 if (SortedLimitedList_Count(best) >= q) {
746 maxDistSq = ((KDTreeBestEntry*) SortedLimitedList_GetItem(best,q - 1))->distanceSq;
747 } else
748 maxDistSq = INT_MAX;
749
750 if (SortedLimitedList_Count(searchHr) > 0) {
751 KDTreeHREntry* hre = (KDTreeHREntry*) SortedLimitedList_GetItem(searchHr, 0);
752 SortedLimitedList_RemoveAt (searchHr, 0);
753
754 furtherHr = hre->rect;
755 furtherKd = hre->tree;
756 pivot = hre->pivot;
757 KDTreeHREntry_delete(hre);
758 }
759
760 // step 10
761 *searchSteps -= 1;
762 if (*searchSteps > 0 &&
763 HyperRectangle_IsInReach (furtherHr, target, sqrt ((double)maxDistSq)))
764 {
765 int ptDistSq = KDTree_DistanceSq (pivot, target);
766 if (ptDistSq < distSq) {
767 // steps 10.1.1 to 10.1.3
768 nearest = pivot;
769 distSq = ptDistSq;
770
771 maxDistSq = distSq;
772 }
773
774 // step 10.2
775 int tempDistSq;
776 IKDTreeDomain* tempNearest = NULL;
777 if (furtherKd == NULL) {
778 tempDistSq = INT_MAX;
779 } else {
780 tempNearest = KDTree_NearestNeighbourListBBFI (furtherKd, best, q,
781 target, furtherHr, maxDistSq, &tempDistSq, searchHr,
782 searchSteps, hrl);
783 }
784
785 // step 10.3
786 if (tempDistSq < distSq) {
787 nearest = tempNearest;
788 distSq = tempDistSq;
789 }
790 }
791
792 *resDistSq = distSq;
793 return (nearest);
794 }
795
796
KDTree_DistanceSq(IKDTreeDomain * t1,IKDTreeDomain * t2)797 int KDTree_DistanceSq (IKDTreeDomain* t1, IKDTreeDomain* t2)
798 {
799 int distance = 0;
800
801 int n;
802 for ( n = 0 ; n < IKDTreeDomain_GetDimensionCount(t1) ; ++n) {
803 int dimDist = IKDTreeDomain_GetDimensionElement (t1, n) -
804 IKDTreeDomain_GetDimensionElement (t2, n);
805 distance += dimDist * dimDist;
806 }
807
808 return (distance);
809 }
810
KDTree_GoodCandidate(ArrayList * exset,int * splitDim)811 IKDTreeDomain* KDTree_GoodCandidate (ArrayList* exset, int* splitDim)
812 {
813 IKDTreeDomain* first = (IKDTreeDomain*) ArrayList_GetItem(exset, 0);
814 if (first == NULL) {
815 FatalError ("Not of type IKDTreeDomain (TODO: custom exception)");
816 }
817
818 int dim = IKDTreeDomain_GetDimensionCount(first);
819
820 // initialize temporary hr search min/max values
821 double* minHr = (double*)malloc(sizeof(double)* dim);
822 double* maxHr = (double*)malloc(sizeof(double)* dim);
823 int i;
824 for (i = 0 ; i < dim ; ++i) {
825 minHr[i] = Double_PositiveInfinity;
826 maxHr[i] = Double_NegativeInfinity;
827 }
828
829 int j;
830 for(j=0; j<ArrayList_Count(exset); j++) {
831 IKDTreeDomain* dom = (IKDTreeDomain*) ArrayList_GetItem(exset, j);
832 int k;
833 for (k = 0 ; k < dim ; ++k) {
834 double dimE = IKDTreeDomain_GetDimensionElement (dom, k);
835
836 if (dimE < minHr[k])
837 minHr[k] = dimE;
838 if (dimE > maxHr[k])
839 maxHr[k] = dimE;
840 }
841 }
842
843 // find the maximum range dimension
844 double* diffHr = (double*)malloc(sizeof(double)* dim);
845 int maxDiffDim = 0;
846 double maxDiff = 0.0;
847
848 int k;
849 for (k = 0 ; k < dim ; ++k) {
850 diffHr[k] = maxHr[k] - minHr[k];
851 if (diffHr[k] > maxDiff) {
852 maxDiff = diffHr[k];
853 maxDiffDim = k;
854 }
855 }
856
857 free(maxHr); maxHr = NULL;
858
859 // the splitting dimension is maxDiffDim
860 // now find a exemplar as close to the arithmetic middle as possible
861 double middle = (maxDiff / 2.0) + minHr[maxDiffDim];
862 IKDTreeDomain* exemplar = NULL;
863 double exemMinDiff = Double_PositiveInfinity;
864
865 free(minHr); minHr=NULL;
866
867 int l;
868 for(l=0; l<ArrayList_Count(exset); l++) {
869 IKDTreeDomain* dom = (IKDTreeDomain*) ArrayList_GetItem(exset, l);
870 double curDiff = abs (IKDTreeDomain_GetDimensionElement (dom, maxDiffDim) - middle);
871 if (curDiff < exemMinDiff) {
872 exemMinDiff = curDiff;
873 exemplar = dom;
874 }
875 }
876
877 free(diffHr); diffHr = NULL;
878
879 // return the values
880 *splitDim = maxDiffDim;
881
882 return (exemplar);
883 }
884
885 // Build a kd-tree from a list of elements. All elements must implement
886 // the IKDTreeDomain interface and must have the same dimensionality.
KDTree_CreateKDTree(ArrayList * exset)887 KDTree* KDTree_CreateKDTree (ArrayList* exset)
888 {
889 if (ArrayList_Count(exset) == 0)
890 return (NULL);
891
892 KDTree* cur = KDTree_new0 ();
893 cur->dr = KDTree_GoodCandidate (exset, &cur->splitDim);
894
895 ArrayList* leftElems = ArrayList_new0 (NULL);
896 ArrayList* rightElems = ArrayList_new0 (NULL);
897
898 // split the exemplar set into left/right elements relative to the
899 // splitting dimension
900 double bound = IKDTreeDomain_GetDimensionElement (cur->dr, cur->splitDim);
901 int i;
902 for(i=0; i<ArrayList_Count(exset); i++) {
903 IKDTreeDomain* dom = (IKDTreeDomain*) ArrayList_GetItem(exset, i);
904 // ignore the current element
905 if (dom == cur->dr)
906 continue;
907
908 if (IKDTreeDomain_GetDimensionElement (dom, cur->splitDim) <= bound) {
909 ArrayList_AddItem(leftElems, dom);
910 } else {
911 ArrayList_AddItem(rightElems, dom);
912 }
913 }
914
915 // recurse
916 cur->left = KDTree_CreateKDTree (leftElems);
917 cur->right = KDTree_CreateKDTree (rightElems);
918
919 ArrayList_delete(leftElems);
920 ArrayList_delete(rightElems);
921
922 return (cur);
923 }
924
925
926