Hugintrunk  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
KDTreeImpl.h
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2007-2008 Anael Orlinski
3 *
4 * This file is part of Panomatic.
5 *
6 * Panomatic is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * Panomatic is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with Panomatic; if not, write to the Free Software
18 * <http://www.gnu.org/licenses/>.
19 */
20 
21 #include "KDTree.h"
22 #include <limits>
23 #include <list>
24 #include <vector>
25 
26 namespace KDTreeSpace
27 {
28 
29 /******************************************************************************
30 *
31 * KDTree()
32 *
33 * Constructor for the tree, use this one to build the tree
34 *
35 ******************************************************************************/
36 
37 template <class KE, class VTYPE>
38 KDTree<KE, VTYPE>::KDTree(const ItemVector_t& iElemsList, int iDimensions) :
39  _pivot(NULL), _leftKD(NULL), _rightKD(NULL), _dims(iDimensions)
40 {
41  // disallow to build a tree with 0 elements.
42  if (!iElemsList.size())
43  {
44  return;
45  }
46 
47  // construct a vector with pointers to the elems lists, and run init on it.
48  ItemPtrVector_t aElemsPtrList;
49  ItemVectorIt_t aElemsIt = iElemsList.begin();
50  ItemVectorIt_t aElemsItEnd = iElemsList.end();
51  for(; aElemsIt != aElemsItEnd; ++aElemsIt)
52  {
53  const KE& aTmp = *aElemsIt;
54  aElemsPtrList.push_back(&aTmp);
55  }
56  init(aElemsPtrList);
57 }
58 
59 /******************************************************************************
60 *
61 * KDTree()
62 *
63 * Constructor for the sub KDTrees
64 *
65 ******************************************************************************/
66 
67 template <class KE, class VTYPE>
68 KDTree<KE, VTYPE>::KDTree(const ItemPtrVector_t& iElemsPtrList, int iDimensions) :
69  _pivot(NULL), _leftKD(NULL), _rightKD(NULL), _dims(iDimensions)
70 {
71  init(iElemsPtrList);
72 }
73 
74 /******************************************************************************
75 *
76 * ~KDTree()
77 *
78 * Destructor
79 *
80 ******************************************************************************/
81 
82 template <class KE, class VTYPE>
84 {
85  delete(_leftKD);
86  delete(_rightKD);
87 }
88 
89 /******************************************************************************
90 *
91 * init()
92 *
93 * Build the local KDTree and recurse to create childs
94 *
95 ******************************************************************************/
96 
97 template <class KE, class VTYPE>
98 void KDTree<KE, VTYPE>::init(const ItemPtrVector_t& iElemsPtrList)
99 {
100  // choose the pivot element
101  typename std::vector<const KE*>::const_iterator aPivotIt = choosePivot(iElemsPtrList);
102  _pivot = *aPivotIt;
103 
104  // create the left/right vectors.
105  ItemPtrVector_t aLeftElems, aRightElems;
106 
107  // split the elements set according to the split dimension
108  ItemPtrVectorIt_t aElemsIt = iElemsPtrList.begin();
109  ItemPtrVectorIt_t aElemsItEnd = iElemsPtrList.end();
110  VTYPE aPivotDimValue = _pivot->getVectorElem(_splitDim);
111  for(; aElemsIt != aElemsItEnd; ++aElemsIt)
112  {
113  // skip the pivot element.
114  if (aElemsIt == aPivotIt)
115  {
116  continue;
117  }
118 
119  if ((*aElemsIt)->getVectorElem(_splitDim) <= aPivotDimValue)
120  {
121  aLeftElems.push_back(*aElemsIt);
122  }
123  else
124  {
125  aRightElems.push_back(*aElemsIt);
126  }
127  }
128 
129  //int aLeftSize = (int)aLeftElems.size();
130  //int aRightSize = (int)aRightElems.size();
131 
132  // recursively create the sub-KDTrees
133  if (aLeftElems.size())
134  {
135  _leftKD = new KDTree<KE, VTYPE> (aLeftElems, _dims);
136  }
137  else
138  {
139  _leftKD = NULL;
140  }
141 
142  if (aRightElems.size())
143  {
144  _rightKD = new KDTree<KE, VTYPE> (aRightElems, _dims);
145  }
146  else
147  {
148  _rightKD = NULL;
149  }
150 
151 }
152 
153 /******************************************************************************
154 *
155 * choosePivot()
156 *
157 * Will choose a splitting dimension for the current tree and get closest
158 * point
159 *
160 ******************************************************************************/
161 
162 template <class KE, class VTYPE>
163 typename std::vector<const KE*>::const_iterator KDTree<KE, VTYPE>::choosePivot(const ItemPtrVector_t& iElemsPtrList)
164 {
165  // search for minimum and maximum in each dimension
166  std::vector<VTYPE> aMinVals(_dims, std::numeric_limits<VTYPE>::max());
167  std::vector<VTYPE> aMaxVals(_dims, - std::numeric_limits<VTYPE>::max());
168 
169  ItemPtrVectorIt_t aElemsIt = iElemsPtrList.begin();
170  ItemPtrVectorIt_t aElemsEnd = iElemsPtrList.end();
171  for(; aElemsIt != aElemsEnd; ++aElemsIt)
172  {
173  for (int aDim = 0; aDim < _dims; ++aDim)
174  {
175  VTYPE& aCurValue = (*aElemsIt)->getVectorElem(aDim);
176  if (aCurValue < aMinVals[aDim])
177  {
178  aMinVals[aDim] = aCurValue;
179  }
180  if (aCurValue > aMaxVals[aDim])
181  {
182  aMaxVals[aDim] = aCurValue;
183  }
184  }
185  }
186 
187  // look for the dimension that has the largest range of values
188  _splitDim = 0;
189  VTYPE aLargestRangeValue = - std::numeric_limits<VTYPE>::max();
190  for (int aDim = 0; aDim < _dims; ++aDim)
191  {
192  if ( (aMaxVals[aDim] - aMinVals[aDim]) > aLargestRangeValue )
193  {
194  _splitDim = aDim;
195  aLargestRangeValue = aMaxVals[aDim] - aMinVals[aDim];
196  }
197  }
198 
199  // compute the median value in the chosen dimension
200  VTYPE aMedian = aLargestRangeValue / 2 + aMinVals[_splitDim];
201 
202  // look for the element closest to the median in the chosen dimension
203  typename std::vector<const KE*>::const_iterator aClosestElemIt;
204  VTYPE aClosestDiff = std::numeric_limits<VTYPE>::max();
205  for(aElemsIt = iElemsPtrList.begin(); aElemsIt != aElemsEnd; ++aElemsIt)
206  {
207  VTYPE aCurValue = fabs((*aElemsIt)->getVectorElem(_splitDim) - aMedian);
208  if (aCurValue < aClosestDiff)
209  {
210  aClosestDiff = aCurValue;
211  aClosestElemIt = aElemsIt;
212  }
213  }
214 
215  return aClosestElemIt;
216 }
217 
218 /******************************************************************************
219 *
220 * CalcSqDist()
221 *
222 * calc the square distance between 2 entries
223 *
224 ******************************************************************************/
225 
226 template <class KE, class VTYPE>
227 double KDTree<KE, VTYPE>::calcSqDist(const KE* i1, const KE* i2)
228 {
229  double aDist = 0.0;
230  for (int n = 0 ; n < _dims ; ++n)
231  {
232  double aDiff = i1->getVectorElem(n) - i2->getVectorElem(n);
233  aDist += aDiff * aDiff;
234  }
235  return (aDist);
236 }
237 
238 
239 template <class KE, class VTYPE>
240 std::set<BestMatch<KE>, std::greater<BestMatch<KE> > > KDTree<KE, VTYPE>::getNearestNeighboursBBF(const KE& iTarget, int iNbBestMatches, int iNbSearchSteps)
241 {
242  // create a hyper rectangle for whole space
243  HyperRectangle<KE, VTYPE> aHR(_dims);
244 
245  // create a limited set to hold best matches.
246  BestMatchLimitedSet_t aBestMatches(iNbBestMatches);
247 
248  // create a limited set to hold the search queue
249  std::list<QueueEntry<KE, VTYPE> > aSearchQueue;
250 
251  // recursively process the search
252  recurseNearestNeighboursBBF(iTarget, aHR, aBestMatches, aSearchQueue, iNbSearchSteps);
253 
254  return aBestMatches.getSet();
255 }
256 
257 template <class KE, class VTYPE>
260  BestMatchLimitedSet_t& ioBestMatches,
261  QueueEntryList_t& ioSearchQueue,
262  int& ioRemainingUnqueues)
263 {
264  // add the node entry to the bestmatches set
265  ioBestMatches.insert(BestMatch<KE>(_pivot, calcSqDist(&iTarget, _pivot)));
266 
267  //std::set<BestMatch<KE>, std::greater<BestMatch<KE> > >::iterator aBEB;
268  //for (aBEB = ioBestMatches.begin(); aBEB!= ioBestMatches.end(); ++aBEB)
269  // cout << "Best Match " << aBEB->_distance << endl;
270 
271  // split the space by a plane perpendicular to the current node dimension
272  // and going through the node point.
273  HyperRectangle<KE, VTYPE> aLeftHR(iHR);
274  HyperRectangle<KE, VTYPE> aRightHR(iHR);
275  //cout << "split dim :" << _splitDim << " " << _pivot->getVectorElem(_splitDim) << endl;
276  //cout << "iHR" << endl;
277  //iHR.display();
278  if (!iHR.split(aLeftHR, aRightHR, _splitDim, _pivot->getVectorElem(_splitDim)))
279  {
280  return;
281  }
282  //cout << "leftHR" << endl;
283  //aLeftHR.display();
284  //cout << "rightHR" << endl;
285  //aRightHR.display();
286 
287 
288 
289  // Create some pointers and refs to the nearer and further HyperRectangles and KDTrees.
290  KDTree<KE, VTYPE> * aNearKD = _leftKD;
291  KDTree<KE, VTYPE> * aFarKD = _rightKD;
292  HyperRectangle<KE, VTYPE> & aNearHR = aLeftHR;
293  HyperRectangle<KE, VTYPE> & aFarHR = aRightHR;
294 
295  // Determine which HR and sub-KD tree is near/far
296  // by comparing the Target and pivot values at node dimension
297  if (iTarget.getVectorElem(_splitDim) > _pivot->getVectorElem(_splitDim))
298  {
299  aNearKD = _rightKD;
300  aFarKD = _leftKD;
301  aNearHR = aRightHR;
302  aFarHR = aLeftHR;
303  }
304 
305  //std::cout << "Near HR, target is in " << aNearHR.isTargetIn(iTarget) << endl;
306  //std::cout << "Far HR, target is in " << aFarHR.isTargetIn(iTarget) << endl;
307 
308 
309  // add the far HyperRectangle to the queue. The queue is sorted by
310  // the distance from target point to the hyperrectangle :
311  // "The distance to a bin is defined to be the minimum distance between q and any
312  // point on the bin boundary."
313  if (aFarKD)
314  {
315  ioSearchQueue.push_back(QueueEntry<KE, VTYPE>(aFarHR, aFarKD, aFarHR.calcSqDistance(iTarget)));
316  }
317 
318  //list<QueueEntry<KE> >::iterator aSQ;
319  //for (aSQ = ioSearchQueue.begin(); aSQ!= ioSearchQueue.end(); ++aSQ)
320  // cout << "SQ " << aSQ->_dist << endl;
321 
322  // go direcly in depth until a leaf is found.
323  if (aNearKD)
324  {
325  //cout << "Going in depth" << endl;
326  aNearKD->recurseNearestNeighboursBBF(iTarget, aNearHR, ioBestMatches, ioSearchQueue, ioRemainingUnqueues);
327  }
328  // else process the queue
329  else if (ioRemainingUnqueues > 0 && ioBestMatches.size() && ioSearchQueue.size())
330  {
331  //cout << "Going in queue" << endl;
332 
333  // decrease the number of 'queue processings'
334  ioRemainingUnqueues--;
335 
336  // get the (squared) distance of the worse 'best match'. this is the first one in the best list.
337  double aHyperSphereRadius = ioBestMatches.begin()->_distance;
338 
339  // get the first element out of the queue, then remove it
340  typename std::list<QueueEntry<KE, VTYPE> >::iterator aSQ, aSmallestIt;
341  double aSmallestDist = std::numeric_limits<double>::max();
342  for (aSQ = ioSearchQueue.begin(); aSQ!= ioSearchQueue.end(); ++aSQ)
343  {
344  if (aSQ->_dist < aSmallestDist)
345  {
346  aSmallestDist = aSQ->_dist;
347  aSmallestIt = aSQ;
348  }
349  }
350  QueueEntry<KE, VTYPE> aQueueElem = *(aSmallestIt);
351  ioSearchQueue.erase(aSmallestIt);
352  //QueueEntry<KE> aQueueElem = *(ioSearchQueue.begin());
353  //ioSearchQueue.erase(ioSearchQueue.begin());
354 
355  // if the hypersphere and hyper rectangle intersect, there is maybe a better match in the intersection
356  if (aQueueElem._HR.hasHyperSphereIntersect(iTarget, aHyperSphereRadius))
357  {
358  aQueueElem._kdTree->recurseNearestNeighboursBBF(iTarget, aQueueElem._HR, ioBestMatches, ioSearchQueue, ioRemainingUnqueues);
359  }
360  else
361  {
362  //cout << "No intersect" << endl;
363  }
364  }
365 }
366 
367 
368 
369 
370 // default constructor
371 template <class KE, class TYPE>
373  _dim(0)
374 {
375 
376 }
377 
378 
379 // constructor
380 template <class KE, class TYPE>
382  _leftTop(std::vector<TYPE>(iDim, -std::numeric_limits<TYPE>::max())),
383  _rightBottom(std::vector<TYPE>(iDim, std::numeric_limits<TYPE>::max())),
384  _dim(iDim)
385 {
386 
387 }
388 
389 // copy constructor
390 template <class KE, class TYPE>
392  _leftTop(std::vector<TYPE>(iOther._leftTop)),
393  _rightBottom(std::vector<TYPE>(iOther._rightBottom)),
394  _dim(iOther._dim)
395 {
396 
397 }
398 
399 
400 
401 
402 // split into smaller HyperRectangle
403 template <class KE, class TYPE>
404 bool HyperRectangle<KE, TYPE>::split(HyperRectangle& oLeft, HyperRectangle& oRight, int iSplitDim, TYPE iSplitVal)
405 {
406  // check if the split value is in the range for the given dimension
407  if (_leftTop[iSplitDim] >= iSplitVal || _rightBottom[iSplitDim] < iSplitVal)
408  {
409  return false;
410  }
411 
412  // make copies of the current vector to destination bin
413  //oLeft._leftTop = _leftTop;
414  //oRight._leftTop = _leftTop;
415  //oLeft._rightBottom = _rightBottom;
416  //oRight._rightBottom = _rightBottom;
417  //oLeft._dim = _dim;
418  //oRight._dim = _dim;
419 
420  // split
421  oLeft._rightBottom[iSplitDim] = iSplitVal;
422  oRight._leftTop[iSplitDim] = iSplitVal;
423 
424  return true;
425 }
426 
427 template <class KE, class TYPE>
429 {
430  double aSqDistance = 0;
431 
432  // compute the closest point within hr to the target.
433  for (int n = 0 ; n < _dim ; ++n)
434  {
435  TYPE aTargetVal = iTarget.getVectorElem(n);
436  TYPE aHRMin = _leftTop[n];
437  TYPE aHRMax = _rightBottom[n];
438 
439  double aDimDist = aTargetVal;
440  if (aTargetVal <= aHRMin)
441  {
442  aDimDist = aTargetVal - aHRMin;
443  }
444  else if (aTargetVal > aHRMin && aTargetVal < aHRMax)
445  {
446  aDimDist = 0;
447  }
448  else if (aTargetVal >= aHRMax)
449  {
450  aDimDist = aTargetVal - aHRMax;
451  }
452  aSqDistance += aDimDist * aDimDist;
453  }
454 
455  return aSqDistance;
456 }
457 
458 template <class KE, class TYPE>
459 bool HyperRectangle<KE, TYPE>::hasHyperSphereIntersect(const KE& iTarget, double iSqDistance)
460 {
461  // avoid calculating the square root by using basic math.
462  //
463 
464  double aDist = calcSqDistance(iTarget);
465  //if (aDist > 1.0 && iSqDistance)
466 
467  //cout << "hypersphereintersect " << sqrt(aDist) << " " << sqrt(iSqDistance) << endl;
468 
469  return (aDist < iSqDistance);
470 }
471 
472 template <class KE, class TYPE>
474 {
475  for (int n = 0 ; n < _dim ; ++n)
476  {
477  if (_leftTop[n] > -std::numeric_limits<TYPE>::max()
478  || _rightBottom[n] < std::numeric_limits<TYPE>::max())
479  {
480  std::cout << "dim[" << n << "] = {" << _leftTop[n] << " , " << _rightBottom[n] << "}" << std::endl;
481  }
482  }
483  std::cout << std::endl;
484 }
485 
486 template <class KE, class TYPE>
487 bool HyperRectangle<KE, TYPE>::isTargetIn (const KE& iTarget)
488 {
489  if (iTarget.getVectorSize() != _dim)
490  {
491  std::cout << "is target in dimension mismatch" << std::endl;
492  }
493 
494  for (int n = 0 ; n < _dim ; ++n)
495  {
496  TYPE aDimVal = iTarget.getVectorElem(n);
497  if (aDimVal <= _leftTop[n] || aDimVal >= _rightBottom[n])
498  {
499  return (false);
500  }
501  }
502 
503  return true;
504 }
505 
506 template <class KE, class VTYPE>
507 bool operator < (const QueueEntry<KE, VTYPE> & iA, const QueueEntry<KE, VTYPE> & iB)
508 {
509  return (iA._dist < iB._dist);
510 }
511 
512 template <class KE>
513 bool operator > (const BestMatch<KE> & iA, const BestMatch<KE> & iB)
514 {
515  return (iA._distance > iB._distance);
516 }
517 
518 } //namespace KDTree
519 
520 
521 
void insert(const _Key &x)
Definition: BoundedSet.h:100
bool split(HyperRectangle &oLeft, HyperRectangle &oRight, int iSplitDim, TYPE iSplitVal)
Definition: KDTreeImpl.h:404
std::set< _Key, _Compare > & getSet()
Definition: BoundedSet.h:106
double calcSqDist(const KE *i1, const KE *i2)
Definition: KDTreeImpl.h:227
bool hasHyperSphereIntersect(const KE &iTarget, double iSqDistance)
Definition: KDTreeImpl.h:459
std::vector< TYPE > _leftTop
Definition: KDTree.h:78
BestMatchSet_t getNearestNeighboursBBF(const KE &iTarget, int iNbBestMatches, int iNbSearchSteps)
Definition: KDTreeImpl.h:240
double calcSqDistance(const KE &iTarget)
Definition: KDTreeImpl.h:428
std::vector< KE > ItemVector_t
Definition: KDTree.h:106
KDTree< KE, VTYPE > * _kdTree
Definition: KDTree.h:92
size_t size() const
Returns the size of the limited_multiset.
Definition: BoundedSet.h:70
std::list< QueueEntry< KE, VTYPE > > QueueEntryList_t
Definition: KDTree.h:112
std::vector< const KE * >::const_iterator ItemPtrVectorIt_t
Definition: KDTree.h:109
iterator begin()
Definition: BoundedSet.h:75
void init(const ItemPtrVector_t &iElemsPtrList)
Definition: KDTreeImpl.h:98
bool isTargetIn(const KE &iTarget)
Definition: KDTreeImpl.h:487
void recurseNearestNeighboursBBF(const KE &iTarget, HyperRectangle< KE, VTYPE > &iHR, BestMatchLimitedSet_t &ioBestMatches, QueueEntryList_t &ioSearchQueue, int &ioRemainingUnqueues)
Definition: KDTreeImpl.h:258
bool operator>(const BestMatch< KE > &iA, const BestMatch< KE > &iB)
Definition: KDTreeImpl.h:513
HyperRectangle< KE, VTYPE > & _HR
Definition: KDTree.h:91
std::vector< const KE * > ItemPtrVector_t
Definition: KDTree.h:108
ItemPtrVectorIt_t choosePivot(const ItemPtrVector_t &iElemsPtrList)
Definition: KDTreeImpl.h:163
static T max(T x, T y)
Definition: svm.cpp:65
std::vector< TYPE > _rightBottom
Definition: KDTree.h:78
std::vector< KE >::const_iterator ItemVectorIt_t
Definition: KDTree.h:107
KDTree(const ItemVector_t &iElemsList, int iDimensions)
Definition: KDTreeImpl.h:38