KdTree.h
Go to the documentation of this file.
1 #ifndef GfxTL__KDTREE_HEADER__
2 #define GfxTL__KDTREE_HEADER__
3 #include <GfxTL/BaseTree.h>
4 #include <GfxTL/ScalarTypeDeferer.h>
5 #include <GfxTL/NullClass.h>
6 #include <GfxTL/VectorXD.h>
7 #include <GfxTL/AABox.h>
8 #include <GfxTL/ScalarTypeConversion.h>
9 #include <GfxTL/MathHelper.h>
10 #include <GfxTL/VectorKernel.h>
11 #include <GfxTL/LimitedHeap.h>
12 #include <GfxTL/NearestNeighbor.h>
13 #include <GfxTL/FlatCopyVector.h>
14 #include <algorithm>
15 #include <memory>
16 #include <deque>
17 #include <malloc.h>
18 
19 namespace GfxTL
20 {
21  template< class BaseT >
22  class KdTreeCell
23  : public BaseT
24  {
25  public:
26  typedef KdTreeCell< BaseT > ThisType;
27  typedef typename BaseT::value_type value_type;
28  typedef typename ScalarTypeDeferer< value_type >::ScalarType ScalarType;
29  enum { NChildren = 2 };
30 
31  KdTreeCell()
32  {
33  m_children[0] = m_children[1] = NULL;
34  }
35 
36  ~KdTreeCell()
37  {
38  if (m_children[0] != (KdTreeCell*)0x1)
39  {
40  delete m_children[0];
41  }
42  delete m_children[1];
43  }
44 
45  ThisType& operator[](unsigned int i)
46  {
47  return *m_children[i];
48  }
49 
50  const ThisType& operator[](unsigned int i) const
51  {
52  return *m_children[i];
53  }
54 
55  void Child(unsigned int i, ThisType* cell)
56  {
57  m_children[i] = cell;
58  }
59 
60  unsigned int& SplitAxis()
61  {
62  return m_splitAxis;
63  }
64 
65  const unsigned int SplitAxis() const
66  {
67  return m_splitAxis;
68  }
69 
70  ScalarType& SplitValue()
71  {
72  return m_splitValue;
73  }
74 
75  const ScalarType SplitValue() const
76  {
77  return m_splitValue;
78  }
79 
80  template< class VectorT >
81  bool operator()(const VectorT& v) const
82  {
83  return v[m_splitAxis] <= m_splitValue;
84  }
85 
86  /*void *operator new(size_t size)
87  {
88  return _mm_malloc(size, 16);
89  }
90 
91  void operator delete(void *ptr)
92  {
93  _mm_free(ptr);
94  }*/
95 
96  private:
97  unsigned int m_splitAxis;
98  ScalarType m_splitValue;
99  ThisType* m_children[2];
100  };
101 
102  template< class DataStrategyT >
103  struct BaseKdTreeStrategy
104  {
105  typedef typename DataStrategyT::value_type value_type;
106 
107  class CellData
108  : public DataStrategyT::CellData
109  {};
110 
111  template< class BaseT >
112  struct StrategyBase
113  : public DataStrategyT::template StrategyBase< BaseT >
114  {
115  typedef typename DataStrategyT::template StrategyBase< BaseT >::CellType
116  CellType;
117  typedef typename DataStrategyT::template StrategyBase< BaseT > BaseType;
118  typedef typename BaseType::DereferencedType DereferencedType;
119 
120  template< class BuildInformationT >
121  bool ShouldSubdivide(const CellType& cell, BuildInformationT& bi)
122  {
123  return false;
124  }
125 
126  template< class BuildInformationT >
127  void EnterGlobalBuildInformation(const CellType& cell,
128  BuildInformationT* bi)
129  {}
130 
131  template< class BuildInformationT >
132  void LeaveGlobalBuildInformation(const CellType& cell,
133  const BuildInformationT& bi)
134  {}
135 
136  template< class TraversalInformationT >
137  void UpdateCellWithBack(const TraversalInformationT&,
138  CellType*)
139  {}
140 
141 protected:
142  template< class TraversalBaseT >
143  class TraversalInformationBase
144  : public DataStrategyT::template StrategyBase< BaseT >
145  ::template TraversalInformation< TraversalBaseT >
146  {};
147  };
148  };
149 
150  template< class StrategiesT, template< class > class MetricT,
151  template< class > class VectorKernelT = DynVectorKernel >
152  class KdTree
153  : public StrategiesT::template StrategyBase
154  <
155  MetricT
156  <
157  VectorKernelT
158  <
159  BaseTree
160  <
161  KdTreeCell< typename StrategiesT::CellData >
162  >
163  >
164  >
165  >
166  {
167 public:
168  typedef KdTreeCell< typename StrategiesT::CellData > CellType;
169  typedef typename StrategiesT::template StrategyBase
170  <
171  MetricT
172  <
173  VectorKernelT
174  <
175  BaseTree
176  <
177  KdTreeCell< typename StrategiesT::CellData >
178  >
179  >
180  >
181  > BaseType;
182  typedef typename BaseType::value_type value_type;
183  typedef typename BaseType::DereferencedType DereferencedType;
184  typedef typename ScalarTypeDeferer< value_type >::ScalarType ScalarType;
185 
186  class BuildInformation
187  : public BaseType::BuildInformation
188  {
189  public:
190  unsigned int& CreateChild()
191  {
192  return m_createChild;
193  }
194  const unsigned int CreateChild() const
195  {
196  return m_createChild;
197  }
198 
199  private:
200  unsigned int m_createChild;
201  };
202 
203  template< class PointT >
204  class BaseGlobalTraversalInformation
205  {
206  public:
207  typedef PointT PointType;
208 
209  const PointT& Point() const
210  {
211  return m_point;
212  }
213  void Point(const PointT& p)
214  {
215  m_point = p;
216  }
217 
218  private:
219  PointT m_point;
220  };
221 
222  template< class GlobalInfoT >
223  class BaseTraversalInformation
224  {
225  public:
226  typedef GlobalInfoT GlobalType;
227  void Global(const GlobalInfoT* globalInfo)
228  {
229  m_globalInfo = globalInfo;
230  }
231  const GlobalInfoT& Global() const
232  {
233  return *m_globalInfo;
234  }
235 
236  private:
237  const GlobalInfoT* m_globalInfo;
238  };
239 
240  void Build()
241  {
242  typedef std::pair< CellType*, BuildInformation > Pair;
243  BaseType::Clear();
244  if (!BaseType::size())
245  {
246  return;
247  }
248  BaseType::Root() = new CellType;
249  std::deque< Pair > stack(1);
250  // init build information directly on stack to avoid
251  // copying.
252  stack.back().first = BaseType::Root();
253  InitRootBuildInformation(&stack.back().second);
254  BaseType::InitRoot(stack.back().second, BaseType::Root());
255  while (stack.size())
256  {
257  Pair& p = stack.back();
258  if (p.second.CreateChild() == CellType::NChildren)
259  {
260  BaseType::LeaveGlobalBuildInformation(*p.first, p.second);
261  stack.pop_back();
262  continue;
263  }
264  if (BaseType::IsLeaf(*p.first))
265  {
266  if (!BaseType::ShouldSubdivide(*p.first, p.second))
267  {
268  stack.pop_back();
269  continue;
270  }
271  Subdivide(p.second, p.first);
272  if (BaseType::IsLeaf(*p.first)) // couldn't subdivide?
273  {
274  stack.pop_back();
275  continue;
276  }
277  }
278  else
279  {
280  BaseType::LeaveGlobalBuildInformation(*p.first, p.second);
281  }
282  while (p.second.CreateChild() < CellType::NChildren &&
283  !this->ExistChild(*p.first, p.second.CreateChild()))
284  {
285  ++p.second.CreateChild();
286  }
287  if (p.second.CreateChild() == CellType::NChildren)
288  {
289  stack.pop_back();
290  continue;
291  }
292  BaseType::EnterGlobalBuildInformation(*p.first, &p.second);
293  stack.resize(stack.size() + 1); // create new entry
294  stack.back().first = &(*p.first)[p.second.CreateChild()];
295  InitBuildInformation(*p.first, p.second,
296  p.second.CreateChild(), &stack.back().second);
297  InitCell(*p.first, p.second, p.second.CreateChild(),
298  stack.back().second, &(*p.first)[p.second.CreateChild()]);
299  ++p.second.CreateChild();
300  }
301  }
302 
303  void InsertBack()
304  {
305  typedef typename BaseType::template GlobalTraversalInformation <
306  BaseGlobalTraversalInformation< value_type > >
307  GlobalInfoType;
308  // traverse the tree and insert the point
309  typename BaseType::template TraversalInformation <
310  BaseTraversalInformation< GlobalInfoType > > ti;
311  BaseType::InitRootTraversalInformation(*BaseType::Root(), &ti);
312  InsertBack(*BaseType::Root(), ti);
313  }
314 
315  void Remove(DereferencedType s)
316  {
317  Remove(*BaseType::Root(), s);
318  }
319 
320  template< class PointT, class DistScalarT, class ContainerT >
321  void PointsInBall(const PointT& p,
322  /*typename BaseType::template DistanceType
323  <
324  typename ScalarTypeDeferer< PointT >::ScalarType,
325  ScalarType
326  >::Type*/DistScalarT sqrRadius,
327  ContainerT* points) const
328  {
329  typedef typename BaseType::template GlobalTraversalInformation <
330  BaseGlobalTraversalInformation< PointT > >
331  GlobalInfoType;
332  GlobalInfoType gti;
333  typename BaseType::template TraversalInformation <
334  BaseTraversalInformation< GlobalInfoType > > ti;
335  points->clear();
336  gti.Point(p);
337  ti.Global(&gti);
338  InitRootTraversalInformation(*BaseType::Root(), &ti);
339  PointsInBall(sqrRadius, *BaseType::Root(), ti, points);
340  }
341 
342  template< class PointT >
343  struct PointsInBallAuxInfo
344  {
345  typedef typename BaseType::template GlobalTraversalInformation <
346  BaseGlobalTraversalInformation< PointT > > GlobalInfoType;
347  typedef typename BaseType::template TraversalInformation <
348  BaseTraversalInformation< GlobalInfoType > > TraversalInfoType;
349  typedef typename BaseType::template DistanceType <
350  typename ScalarTypeDeferer< PointT >::ScalarType,
351  ScalarType
352  >::Type DistanceType;
353  PointsInBallAuxInfo() {}
354  PointsInBallAuxInfo(const CellType* cell, const TraversalInfoType& ti)
355  : m_cell(cell)
356  , m_ti(ti)
357  {}
358  const CellType* m_cell;
359  TraversalInfoType m_ti;
360  };
361 
362  template< class PointT >
363  struct PointsInBallAuxData
364  : public FlatCopyVector< PointsInBallAuxInfo< PointT > >
365  {
366  typedef typename PointsInBallAuxInfo< PointT >::TraversalInfoType TraversalInfoType;
367  typedef typename PointsInBallAuxInfo< PointT >::DistanceType DistanceType;
368  };
369 
370  template< class PointT, class DistScalarT, class ContainerT >
371  void PointsInBall(const PointT& p,
372  DistScalarT sqrRadius,
373  ContainerT* points, PointsInBallAuxData< PointT >* auxData) const
374  {
375  points->clear();
376  PrivatePointsInBall(p, sqrRadius, points, auxData);
377  }
378 
379  template< class PointT, class LimitedHeapT >
380  void NearestNeighbors(const PointT& p, unsigned int k,
381  LimitedHeapT* neighbors) const
382  {
383  typedef typename BaseType::template GlobalTraversalInformation <
384  BaseGlobalTraversalInformation< PointT > > GlobalInfoType;
385  typedef typename BaseType::template TraversalInformation <
386  BaseTraversalInformation< GlobalInfoType > > TraversalInformation;
387  // typedef typename BaseType::template DistanceType <
388  // typename ScalarTypeDeferer< PointT >::ScalarType,
389  // ScalarType
390  // >::Type DistanceType;
391  GlobalInfoType gti;
392  TraversalInformation ti;
393  neighbors->clear();
394  neighbors->Limit(k);
395  gti.Point(p);
396  ti.Global(&gti);
397  BaseType::InitRootTraversalInformation(*BaseType::Root(), &ti);
398  PrivateNearestNeighbors(*BaseType::Root(), ti, neighbors);
399  neighbors->AssertHeap();
400  }
401 
402  template< class PointT >
403  struct NearestNeighborsAuxInfo
404  {
405  typedef typename BaseType::template GlobalTraversalInformation <
406  BaseGlobalTraversalInformation< PointT > > GlobalInfoType;
407  typedef typename BaseType::template TraversalInformation <
408  BaseTraversalInformation< GlobalInfoType > > TraversalInfoType;
409  typedef typename BaseType::template DistanceType <
410  typename ScalarTypeDeferer< PointT >::ScalarType,
411  ScalarType
412  >::Type DistanceType;
413  NearestNeighborsAuxInfo() {}
414  NearestNeighborsAuxInfo(const CellType* cell, const TraversalInfoType& ti, DistanceType sqrDist)
415  : m_cell(cell)
416  , m_ti(ti)
417  , m_sqrDist(sqrDist)
418  {}
419  const CellType* m_cell;
420  TraversalInfoType m_ti;
421  DistanceType m_sqrDist;
422  };
423 
424  template< class PointT >
425  struct NearestNeighborsAuxData
426  : public FlatCopyVector< NearestNeighborsAuxInfo< PointT > >
427  {
428  typedef typename NearestNeighborsAuxInfo< PointT >::TraversalInfoType TraversalInfoType;
429  typedef typename NearestNeighborsAuxInfo< PointT >::DistanceType DistanceType;
430  };
431 
432  template< class PointT, class LimitedHeapT >
433  void NearestNeighbors(const PointT& p, unsigned int k,
434  LimitedHeapT* neighbors, NearestNeighborsAuxData< PointT >* auxData) const
435  {
436  neighbors->clear();
437  neighbors->Limit(k);
438  PrivateNearestNeighbors(p, auxData, neighbors);
439  neighbors->AssertHeap();
440  }
441 
442  template< class PointT >
443  struct NNTypeHelper
444  {
445  typedef NN <
446  typename BaseType::template DistanceType <
447  typename ScalarTypeDeferer< PointT >::ScalarType,
448  ScalarType
449  >::Type, typename BaseType::DereferencedType >
450  NNType;
451  };
452 
453  template< class PointT, class EpsilonT, template< class > class ContainerT >
454  void ApproximateNearestNeighbors(const PointT& p, unsigned int k,
455  EpsilonT epsilon,
456  LimitedHeap< typename NNTypeHelper<PointT>::NNType,
457  std::less< typename NNTypeHelper<PointT>::NNType >,
458  ContainerT >* neighbors) const
459  {
460  typedef typename BaseType::template GlobalTraversalInformation <
461  BaseGlobalTraversalInformation< PointT > > GlobalInfoType;
462  GlobalInfoType gti;
463  typename BaseType::template TraversalInformation <
464  BaseTraversalInformation< GlobalInfoType > > ti;
465  neighbors->clear();
466  neighbors->Limit(k);
467  gti.Point(p);
468  ti.Global(&gti);
469  BaseType::InitRootTraversalInformation(*BaseType::Root(), &ti);
470  ApproximateNearestNeighbors(*BaseType::Root(), ti, 1 + epsilon, neighbors);
471  neighbors->AssertHeap();
472  }
473 
474  template< class PointT >
475  bool Contains(const PointT& p, DereferencedType* dref) const
476  {
477  typedef typename BaseType::template GlobalTraversalInformation <
478  BaseGlobalTraversalInformation< PointT > >
479  GlobalInfoType;
480  GlobalInfoType gti;
481  typename BaseType::template TraversalInformation <
482  BaseTraversalInformation< GlobalInfoType > > ti;
483  gti.Point(p);
484  ti.Global(&gti);
485  BaseType::InitRootTraversalInformation(*BaseType::Root(), &ti);
486  return Contains(*BaseType::Root(), ti, dref);
487  }
488 
489 private:
490  void InitRootBuildInformation(BuildInformation* bi)
491  {
492  bi->CreateChild() = 0;
493  BaseType::InitRootBuildInformation(bi);
494  }
495 
496  void InitBuildInformation(const CellType& parent,
497  const BuildInformation& parentInfo, unsigned int childIdx,
498  BuildInformation* bi)
499  {
500  bi->CreateChild() = 0;
501  BaseType::InitBuildInformation(parent, parentInfo, childIdx,
502  bi);
503  }
504 
505  template< class BuildInformationT >
506  void InitCell(const CellType& parent,
507  const BuildInformationT& parentInfo, unsigned int childIdx,
508  const BuildInformationT& bi, CellType* cell)
509  {
510  BaseType::InitCell(parent, parentInfo, childIdx, bi, cell);
511  }
512 
513  template< class TraversalInformationT >
514  void InitTraversalInformation(const CellType& parent,
515  const TraversalInformationT& pTi, unsigned int childIdx,
516  TraversalInformationT* ti) const
517  {
518  ti->Global(&pTi.Global());
519  BaseType::InitTraversalInformation(parent, pTi, childIdx, ti);
520  }
521 
522  template< class TraversalInformationT >
523  void InsertBack(CellType& cell, const TraversalInformationT& ti)
524  {
525  typename BaseType::CellRange range;
526  BaseType::GetCellRange(cell, ti, &range);
527  if (IsLeaf(cell))
528  {
529  BaseType::InsertBack(range, &cell);
530  return;
531  }
532  bool l = SplitAndInsert(cell, range, &(cell[0]), &(cell[1]));
533  TraversalInformationT cti;
534  if (l)
535  {
536  UpdateCellWithBack(ti, &(cell[0]));
537  InitTraversalInformation(cell, ti, 0, &cti);
538  InsertBack(cell[0], cti);
539  }
540  else
541  {
542  UpdateCellWithBack(ti, &(cell[1]));
543  InitTraversalInformation(cell, ti, 1, &cti);
544  InsertBack(cell[1], cti);
545  }
546  }
547 
548  void Remove(CellType& cell, DereferencedType s)
549  {
550  if (IsLeaf(cell))
551  {
552  BaseType::Remove(s, &cell);
553  return;
554  }
555  if (BaseType::Remove(cell, s))
556  {
557  Remove(cell[0], s);
558  }
559  else
560  {
561  Remove(cell[1], s);
562  }
563  }
564 
565  void Subdivide(BuildInformation& bi, CellType* cell)
566  {
567  BaseType::ComputeSplit(bi, cell);
568  CellType* left, *right;
569  left = new CellType;
570  right = new CellType;
571  // unsigned int splitIter = 1;
572  //splitAgain:
573  BaseType::SplitData(*cell, *cell, bi, left, right);
574  if (left->Size() == cell->Size())
575  {
576  delete right;
577  right = NULL;
578  }
579  else if (right->Size() == cell->Size())
580  {
581  delete left;
582  left = NULL;
583  }
584  //if(left->Size() == cell->Size() || right->Size() == cell->Size())
585  //{
586  // //if(splitIter < BaseType::m_dim)
587  // //{
588  // // ++splitIter;
589  // // if(BaseType::AlternateSplit(bi, cell))
590  // // goto splitAgain;
591  // //}
592  // for(unsigned int i = 0; i < 2; ++i)
593  // {
594  // left->Child(i, NULL);
595  // right->Child(i, NULL);
596  // }
597  // delete left;
598  // delete right;
599  // left = right = NULL;
600  //}
601  cell->Child(0, left);
602  cell->Child(1, right);
603  if (BaseType::IsLeaf(*cell))
604  {
605  cell->Child(0, BaseType::InnerNodeMarker());
606  }
607  }
608 
609  template< class TraversalInformationT, class DistScalarT,
610  class ContainerT >
611  void PointsInBall(DistScalarT sqrRadius, const CellType& cell,
612  const TraversalInformationT& ti, ContainerT* points) const
613  {
614  if (BaseType::IsLeaf(cell))
615  {
616  // find all points within ball
617  typename BaseType::CellRange range;
618  BaseType::GetCellRange(cell, ti, &range);
619  for (typename BaseType::HandleType h = range.first;
620  h != range.second; ++h)
621  {
622  DistScalarT d = BaseType::SqrDistance(ti.Global().Point(),
623  at(BaseType::Dereference(h)));
624  if (d <= sqrRadius)
625  points->push_back(typename ContainerT::value_type(
626  d, BaseType::Dereference(h)));
627  }
628  return;
629  }
630  TraversalInformationT cti;
631  // check distance to left box
632  if (BaseType::ExistChild(cell, 0))
633  {
634  InitTraversalInformation(cell, ti, 0, &cti);
635  if (BaseType::CellSqrDistance(cell[0], cti) <= sqrRadius)
636  {
637  PointsInBall(sqrRadius, cell[0], cti, points);
638  }
639  }
640  // check distance to right box
641  if (BaseType::ExistChild(cell, 1))
642  {
643  InitTraversalInformation(cell, ti, 1, &cti);
644  if (BaseType::CellSqrDistance(cell[1], cti) <= sqrRadius)
645  {
646  PointsInBall(sqrRadius, cell[1], cti, points);
647  }
648  }
649  }
650 
651  template< class PointT, class DistScalarT, class ContainerT >
652  void PrivatePointsInBall(const PointT& p, DistScalarT sqrRadius,
653  ContainerT* points, PointsInBallAuxData< PointT >* auxData) const
654  {
655  typedef PointsInBallAuxData< PointT > AuxDataType;
656  typedef typename AuxDataType::TraversalInfoType TraversalInfoType;
657  typedef typename TraversalInfoType::GlobalType GlobalInfoType;
658  typedef typename AuxDataType::value_type AuxDataValueType;
659  typedef typename AuxDataType::DistanceType DistanceType;
660  if (!BaseType::Root())
661  {
662  return;
663  }
664  auxData->reserve(128);
665  GlobalInfoType gti;
666  TraversalInfoType ti;
667  gti.Point(p);
668  ti.Global(&gti);
669  BaseType::InitRootTraversalInformation(*BaseType::Root(), &ti);
670  const CellType* current = BaseType::Root();
671  while (1)
672  {
673  if (BaseType::IsLeaf(*current))
674  {
675  typename BaseType::CellRange range;
676  BaseType::GetCellRange(*current, ti, &range);
677  typename BaseType::HandleType h = range.first;
678  // let's find nearest neighbors from points within the cell
679  for (; h != range.second; ++h)
680  {
681  typename ContainerT::value_type nn(
682  BaseType::SqrDistance(p, BaseType::at(BaseType::Dereference(h))),
683  BaseType::Dereference(h));
684  if (nn.sqrDist <= sqrRadius)
685  {
686  points->push_back(nn);
687  }
688  }
689  }
690  else if (!BaseType::ExistChild(*current, 0))
691  {
692  TraversalInfoType rti;
693  InitTraversalInformation(*current, ti, 1, &rti);
694  if (BaseType::CellSqrDistance(current[1], rti) <= sqrRadius)
695  {
696  current = &(*current)[1];
697  ti = rti;
698  continue;
699  }
700  }
701  else if (!BaseType::ExistChild(*current, 1))
702  {
703  TraversalInfoType lti;
704  InitTraversalInformation(*current, ti, 0, &lti);
705  if (BaseType::CellSqrDistance((*current)[0], lti) <= sqrRadius)
706  {
707  current = &(*current)[0];
708  ti = lti;
709  continue;
710  }
711  }
712  else
713  {
714  TraversalInfoType lti, rti;
715  DistanceType dl, dr;
716  InitTraversalInformation(*current, ti, 0, &lti);
717  InitTraversalInformation(*current, ti, 1, &rti);
718  // descent into left child first
719  dl = BaseType::CellSqrDistance((*current)[0], lti);
720  dr = BaseType::CellSqrDistance((*current)[1], rti);
721  if (dl <= sqrRadius)
722  {
723  if (dr <= sqrRadius)
724  {
725  auxData->push_back(AuxDataValueType(&(*current)[1], rti));
726  }
727  current = &(*current)[0];
728  ti = lti;
729  continue;
730  }
731  else if (dr <= sqrRadius)
732  {
733  current = &(*current)[1];
734  ti = rti;
735  continue;
736  }
737  }
738  // pop stack
739  if (!auxData->size())
740  {
741  return;
742  }
743  current = auxData->back().m_cell;
744  ti = auxData->back().m_ti;
745  auxData->pop_back();
746  }
747  }
748 
749  template< class TraversalInformationT, class LimitedHeapT >
750  void PrivateNearestNeighbors(const CellType& cell,
751  const TraversalInformationT& ti,
752  LimitedHeapT* neighbors) const
753  {
754  typedef typename LimitedHeapT::value_type NNType;
755  if (BaseType::IsLeaf(cell)
756  || (neighbors->size() + cell.Size() <= neighbors->Limit()))
757  {
758  typename BaseType::CellRange range;
759  BaseType::GetCellRange(cell, ti, &range);
760  typename BaseType::HandleType h = range.first;
761  // let's find nearest neighbors from points within the cell
762  for (; h != range.second; ++h)
763  {
764  NNType nn(BaseType::SqrDistance(ti.Global().Point(), BaseType::at(BaseType::Dereference(h))),
765  BaseType::Dereference(h));
766  neighbors->PushHeap(nn);
767  }
768  return;
769  }
770  if (!BaseType::ExistChild(cell, 0))
771  {
772  TraversalInformationT rti;
773  typename NNType::ScalarType dr;
774  InitTraversalInformation(cell, ti, 1, &rti);
775  dr = BaseType::CellSqrDistance(cell[1], rti);
776  if ((neighbors->size() < neighbors->Limit()
777  || dr <= neighbors->front().sqrDist))
778  {
779  PrivateNearestNeighbors(cell[1], rti, neighbors);
780  }
781  return;
782  }
783  else if (!BaseType::ExistChild(cell, 1))
784  {
785  TraversalInformationT lti;
786  typename NNType::ScalarType dl;
787  InitTraversalInformation(cell, ti, 0, &lti);
788  dl = BaseType::CellSqrDistance(cell[0], lti);
789  if ((neighbors->size() < neighbors->Limit()
790  || dl <= neighbors->front().sqrDist))
791  {
792  PrivateNearestNeighbors(cell[0], lti, neighbors);
793  }
794  return;
795  }
796  TraversalInformationT lti, rti;
797 
798  typename NNType::ScalarType dl, dr;
799  InitTraversalInformation(cell, ti, 0, &lti);
800  InitTraversalInformation(cell, ti, 1, &rti);
801  // descent into closer child first
802  dl = BaseType::CellSqrDistance(cell[0], lti);
803  dr = BaseType::CellSqrDistance(cell[1], rti);
804  if (dl <= dr)
805  {
806  if ((neighbors->size() < neighbors->Limit()
807  || dl <= neighbors->front().sqrDist))
808  {
809  PrivateNearestNeighbors(cell[0], lti, neighbors);
810  }
811  if ((neighbors->size() < neighbors->Limit()
812  || dr <= neighbors->front().sqrDist))
813  {
814  PrivateNearestNeighbors(cell[1], rti, neighbors);
815  }
816  }
817  else
818  {
819  if ((neighbors->size() < neighbors->Limit()
820  || dr <= neighbors->front().sqrDist))
821  {
822  PrivateNearestNeighbors(cell[1], rti, neighbors);
823  }
824  if ((neighbors->size() < neighbors->Limit()
825  || dl <= neighbors->front().sqrDist))
826  {
827  PrivateNearestNeighbors(cell[0], lti, neighbors);
828  }
829  }
830  }
831 
832  template< class PointT, class LimitedHeapT >
833  void PrivateNearestNeighbors(const PointT& p,
834  NearestNeighborsAuxData< PointT >* auxData,
835  LimitedHeapT* neighbors) const
836  {
837  typedef NearestNeighborsAuxData< PointT > AuxDataType;
838  typedef typename AuxDataType::TraversalInfoType TraversalInfoType;
839  typedef typename TraversalInfoType::GlobalType GlobalInfoType;
840  typedef typename AuxDataType::value_type AuxDataValueType;
841  if (!BaseType::Root())
842  {
843  return;
844  }
845  auxData->reserve(128);
846  GlobalInfoType gti;
847  TraversalInfoType ti;
848  gti.Point(p);
849  ti.Global(&gti);
850  BaseType::InitRootTraversalInformation(*BaseType::Root(), &ti);
851  const CellType* current = BaseType::Root();
852  while (1)
853  {
854  typedef typename LimitedHeapT::value_type NNType;
855  if (BaseType::IsLeaf(*current)
856  || (neighbors->size() + current->Size() <= neighbors->Limit()))
857  {
858  typename BaseType::CellRange range;
859  BaseType::GetCellRange(*current, ti, &range);
860  typename BaseType::HandleType h = range.first;
861  // let's find nearest neighbors from points within the cell
862  for (; h != range.second; ++h)
863  {
864  NNType nn(BaseType::SqrDistance(ti.Global().Point(), BaseType::at(BaseType::Dereference(h))),
865  BaseType::Dereference(h));
866  neighbors->PushHeap(nn);
867  }
868  }
869  else if (!BaseType::ExistChild(*current, 0))
870  {
871  TraversalInfoType rti;
872  typename NNType::ScalarType dr;
873  InitTraversalInformation(*current, ti, 1, &rti);
874  dr = BaseType::CellSqrDistance(current[1], rti);
875  if (neighbors->size() < neighbors->Limit()
876  || dr <= neighbors->front().sqrDist)
877  {
878  current = &(*current)[1];
879  ti = rti;
880  continue;
881  }
882  }
883  else if (!BaseType::ExistChild(*current, 1))
884  {
885  TraversalInfoType lti;
886  typename NNType::ScalarType dl;
887  InitTraversalInformation(*current, ti, 0, &lti);
888  dl = BaseType::CellSqrDistance((*current)[0], lti);
889  if (neighbors->size() < neighbors->Limit()
890  || dl <= neighbors->front().sqrDist)
891  {
892  current = &(*current)[0];
893  ti = lti;
894  continue;
895  }
896  }
897  else
898  {
899  TraversalInfoType lti, rti;
900  typename NNType::ScalarType dl, dr;
901  InitTraversalInformation(*current, ti, 0, &lti);
902  InitTraversalInformation(*current, ti, 1, &rti);
903  // descent into closer child first
904  dl = BaseType::CellSqrDistance((*current)[0], lti);
905  dr = BaseType::CellSqrDistance((*current)[1], rti);
906  if (dl <= dr)
907  {
908  if (neighbors->size() < neighbors->Limit()
909  || dl <= neighbors->front().sqrDist)
910  {
911  auxData->push_back(AuxDataValueType(&(*current)[1], rti, dr));
912  current = &(*current)[0];
913  ti = lti;
914  continue;
915  }
916  if ((neighbors->size() < neighbors->Limit()
917  || dr <= neighbors->front().sqrDist))
918  {
919  current = &(*current)[1];
920  ti = rti;
921  continue;
922  }
923  }
924  else
925  {
926  if (neighbors->size() < neighbors->Limit()
927  || dr <= neighbors->front().sqrDist)
928  {
929  auxData->push_back(AuxDataValueType(&(*current)[0], lti, dl));
930  current = &(*current)[1];
931  ti = rti;
932  continue;
933  }
934  if ((neighbors->size() < neighbors->Limit()
935  || dl <= neighbors->front().sqrDist))
936  {
937  current = &(*current)[0];
938  ti = lti;
939  continue;
940  }
941  }
942  }
943  // pop stack
944  while (1)
945  {
946  if (!auxData->size())
947  {
948  return;
949  }
950  if (neighbors->size() < neighbors->Limit()
951  || auxData->back().m_sqrDist <= neighbors->front().sqrDist)
952  {
953  current = auxData->back().m_cell;
954  ti = auxData->back().m_ti;
955  auxData->pop_back();
956  break;
957  }
958  auxData->pop_back();
959  }
960  }
961  }
962 
963  template< class TraversalInformationT, class NNT,
964  class EpsilonT, template< class > class ContainerT >
965  void ApproximateNearestNeighbors(const CellType& cell,
966  const TraversalInformationT& ti, EpsilonT epsilon,
967  LimitedHeap< NNT, std::less<NNT>, ContainerT >* neighbors) const
968  {
969  typedef NNT NNType;
970  if (IsLeaf(cell)
971  || (neighbors->size() + cell.Size() <= neighbors->Limit()))
972  {
973  typename BaseType::CellRange range;
974  GetCellRange(cell, ti, &range);
975  typename BaseType::HandleType h = range.first,
976  hend = range.second;
977  // let's find nearest neighbors from points within the cell
978  for (; h != hend; ++h)
979  {
980  neighbors->PushHeap(NNType(
981  SqrDistance(ti.Global().Point(), at(Dereference(h))),
982  Dereference(h)));
983  }
984  return;
985  }
986  TraversalInformationT lti, rti;
987  InitTraversalInformation(cell, ti, 0, &lti);
988  InitTraversalInformation(cell, ti, 1, &rti);
989  // descent into closer child first
990  typename LimitedHeap< NNT, std::less<NNT>, ContainerT >::value_type::ScalarType
991  dl, dr;
992  dl = CellSqrDistance(cell[0], lti);
993  dr = CellSqrDistance(cell[1], rti);
994  if (dl <= dr)
995  {
996  if ((neighbors->size() < neighbors->Limit()
997  || epsilon * dl <= neighbors->front().sqrDist)
998  && BaseType::ExistChild(cell, 0))
999  {
1000  ApproximateNearestNeighbors(cell[0], lti, epsilon, neighbors);
1001  }
1002  if ((neighbors->size() < neighbors->Limit()
1003  || epsilon * dr <= neighbors->front().sqrDist)
1004  && BaseType::ExistChild(cell, 1))
1005  {
1006  ApproximateNearestNeighbors(cell[1], rti, epsilon, neighbors);
1007  }
1008  }
1009  else
1010  {
1011  if ((neighbors->size() < neighbors->Limit()
1012  || epsilon * dr <= neighbors->front().sqrDist)
1013  && BaseType::ExistChild(cell, 1))
1014  {
1015  ApproximateNearestNeighbors(cell[1], rti, epsilon, neighbors);
1016  }
1017  if ((neighbors->size() < neighbors->Limit()
1018  || epsilon * dl <= neighbors->front().sqrDist)
1019  && BaseType::ExistChild(cell, 0))
1020  {
1021  ApproximateNearestNeighbors(cell[0], lti, epsilon, neighbors);
1022  }
1023  }
1024  }
1025 
1026  template< class TraversalInformationT >
1027  bool Contains(const CellType& cell, const TraversalInformationT& ti,
1028  DereferencedType* dref) const
1029  {
1030  if (IsLeaf(cell))
1031  {
1032  typename BaseType::CellRange range;
1033  GetCellRange(cell, ti, &range);
1034  typename BaseType::HandleType h = range.first,
1035  hend = range.second;
1036  for (; h != hend; ++h)
1037  if (at(Dereference(h)) == ti.Global().Point())
1038  {
1039  *dref = Dereference(h);
1040  return true;
1041  }
1042  return false;
1043  }
1044  if (cell(ti.Global().Point()))
1045  {
1046  if (!BaseType::ExistChild(cell, 0))
1047  {
1048  return false;
1049  }
1050  TraversalInformationT lti;
1051  InitTraversalInformation(cell, ti, 0, &lti);
1052  return Contains(cell[0], lti, dref);
1053  }
1054  else
1055  {
1056  if (!BaseType::ExistChild(cell, 1))
1057  {
1058  return false;
1059  }
1060  TraversalInformationT rti;
1061  InitTraversalInformation(cell, rti, 1, &rti);
1062  return Contains(cell[1], rti, dref);
1063  }
1064  }
1065  };
1066 };
1067 
1068 #endif
GfxTL::KdTree::PointsInBall
void PointsInBall(const PointT &p, DistScalarT sqrRadius, ContainerT *points) const
Definition: KdTree.h:321
NearestNeighbor.h
GfxTL::KdTreeCell::operator[]
ThisType & operator[](unsigned int i)
Definition: KdTree.h:45
GfxTL::KdTreeCell::operator()
bool operator()(const VectorT &v) const
Definition: KdTree.h:81
GfxTL::KdTree::PointsInBallAuxInfo::m_cell
const CellType * m_cell
Definition: KdTree.h:358
GfxTL::KdTree::BaseGlobalTraversalInformation::Point
void Point(const PointT &p)
Definition: KdTree.h:213
GfxTL::ScalarTypeDeferer
Definition: ScalarTypeDeferer.h:12
GfxTL::KdTreeCell::SplitValue
const ScalarType SplitValue() const
Definition: KdTree.h:75
GfxTL::KdTree::NearestNeighborsAuxInfo::GlobalInfoType
BaseType::template GlobalTraversalInformation< BaseGlobalTraversalInformation< PointT > > GlobalInfoType
Definition: KdTree.h:406
GfxTL::BaseKdTreeStrategy::StrategyBase::ShouldSubdivide
bool ShouldSubdivide(const CellType &cell, BuildInformationT &bi)
Definition: KdTree.h:121
GfxTL::DynVectorKernel
Definition: DynVectorKernel.h:7
GfxTL::KdTree
Definition: KdTree.h:152
GfxTL::KdTree::PointsInBall
void PointsInBall(const PointT &p, DistScalarT sqrRadius, ContainerT *points, PointsInBallAuxData< PointT > *auxData) const
Definition: KdTree.h:371
GfxTL::KdTree::BaseType
StrategiesT::template StrategyBase< MetricT< VectorKernelT< BaseTree< KdTreeCell< typename StrategiesT::CellData > > > > > BaseType
Definition: KdTree.h:181
GfxTL::KdTree::BuildInformation::CreateChild
const unsigned int CreateChild() const
Definition: KdTree.h:194
GfxTL::KdTreeCell::Child
void Child(unsigned int i, ThisType *cell)
Definition: KdTree.h:55
GfxTL::KdTree::PointsInBallAuxInfo::TraversalInfoType
BaseType::template TraversalInformation< BaseTraversalInformation< GlobalInfoType > > TraversalInfoType
Definition: KdTree.h:348
GfxTL::KdTree::value_type
BaseType::value_type value_type
Definition: KdTree.h:182
GfxTL::ScalarTypeDeferer::ScalarType
PointT::value_type ScalarType
Definition: ScalarTypeDeferer.h:14
GfxTL::KdTree::NNTypeHelper
Definition: KdTree.h:443
GfxTL::KdTree::PointsInBallAuxInfo::m_ti
TraversalInfoType m_ti
Definition: KdTree.h:359
LimitedHeap.h
GfxTL::KdTree::NearestNeighborsAuxInfo::TraversalInfoType
BaseType::template TraversalInformation< BaseTraversalInformation< GlobalInfoType > > TraversalInfoType
Definition: KdTree.h:408
GfxTL::KdTree::BaseTraversalInformation::Global
void Global(const GlobalInfoT *globalInfo)
Definition: KdTree.h:227
GfxTL::KdTree::NearestNeighborsAuxInfo::m_sqrDist
DistanceType m_sqrDist
Definition: KdTree.h:421
GfxTL::KdTreeCell::value_type
BaseT::value_type value_type
Definition: KdTree.h:27
ScalarTypeConversion.h
VectorXD.h
GfxTL::KdTree::PointsInBallAuxData
Definition: KdTree.h:363
GfxTL::NN
Definition: NearestNeighbor.h:8
GfxTL::FlatCopyVector
Definition: FlatCopyVector.h:10
GfxTL::KdTree::NearestNeighborsAuxInfo::m_cell
const CellType * m_cell
Definition: KdTree.h:419
GfxTL::KdTree::NearestNeighborsAuxInfo::NearestNeighborsAuxInfo
NearestNeighborsAuxInfo(const CellType *cell, const TraversalInfoType &ti, DistanceType sqrDist)
Definition: KdTree.h:414
GfxTL::BaseKdTreeStrategy::StrategyBase::EnterGlobalBuildInformation
void EnterGlobalBuildInformation(const CellType &cell, BuildInformationT *bi)
Definition: KdTree.h:127
GfxTL::KdTreeCell::ScalarType
ScalarTypeDeferer< value_type >::ScalarType ScalarType
Definition: KdTree.h:28
GfxTL::KdTree::NearestNeighborsAuxData
Definition: KdTree.h:425
GfxTL::KdTreeCell
Definition: KdTree.h:22
GfxTL::KdTreeCell::KdTreeCell
KdTreeCell()
Definition: KdTree.h:31
GfxTL::KdTreeCell::SplitAxis
const unsigned int SplitAxis() const
Definition: KdTree.h:65
GfxTL::KdTree::BaseGlobalTraversalInformation::Point
const PointT & Point() const
Definition: KdTree.h:209
GfxTL::KdTree::PointsInBallAuxInfo::DistanceType
BaseType::template DistanceType< typename ScalarTypeDeferer< PointT >::ScalarType, ScalarType >::Type DistanceType
Definition: KdTree.h:352
GfxTL::KdTree::BaseTraversalInformation
Definition: KdTree.h:223
GfxTL::BaseTree
Definition: BaseTree.h:9
FlatCopyVector.h
GfxTL::KdTree::BaseGlobalTraversalInformation
Definition: KdTree.h:204
AABox.h
GfxTL::KdTree::NearestNeighborsAuxInfo
Definition: KdTree.h:403
BaseTree.h
GfxTL::KdTree::PointsInBallAuxInfo::PointsInBallAuxInfo
PointsInBallAuxInfo(const CellType *cell, const TraversalInfoType &ti)
Definition: KdTree.h:354
GfxTL::KdTree::ScalarType
ScalarTypeDeferer< value_type >::ScalarType ScalarType
Definition: KdTree.h:184
GfxTL::BaseKdTreeStrategy::StrategyBase::CellType
DataStrategyT::template StrategyBase< BaseT >::CellType CellType
Definition: KdTree.h:116
GfxTL::KdTree::Build
void Build()
Definition: KdTree.h:240
armarx::PointT
pcl::PointXYZRGBL PointT
Definition: Common.h:28
GfxTL::BaseKdTreeStrategy::StrategyBase::UpdateCellWithBack
void UpdateCellWithBack(const TraversalInformationT &, CellType *)
Definition: KdTree.h:137
GfxTL::BaseKdTreeStrategy::StrategyBase::BaseType
DataStrategyT::template StrategyBase< BaseT > BaseType
Definition: KdTree.h:117
GfxTL::KdTreeCell::NChildren
@ NChildren
Definition: KdTree.h:29
armarx::aron::similarity::FloatSimilarity::Type
Type
The Type enum.
Definition: FloatSimilarity.h:8
GfxTL::KdTree::BuildInformation
Definition: KdTree.h:186
GfxTL::KdTree::NearestNeighborsAuxData::DistanceType
NearestNeighborsAuxInfo< PointT >::DistanceType DistanceType
Definition: KdTree.h:429
GfxTL::KdTree::NearestNeighborsAuxInfo::NearestNeighborsAuxInfo
NearestNeighborsAuxInfo()
Definition: KdTree.h:413
GfxTL::KdTree::PointsInBallAuxInfo::PointsInBallAuxInfo
PointsInBallAuxInfo()
Definition: KdTree.h:353
GfxTL::KdTreeCell::ThisType
KdTreeCell< BaseT > ThisType
Definition: KdTree.h:26
GfxTL
Definition: AABox.h:8
GfxTL::KdTree::PointsInBallAuxInfo::GlobalInfoType
BaseType::template GlobalTraversalInformation< BaseGlobalTraversalInformation< PointT > > GlobalInfoType
Definition: KdTree.h:346
GfxTL::KdTreeCell::~KdTreeCell
~KdTreeCell()
Definition: KdTree.h:36
GfxTL::KdTree::ApproximateNearestNeighbors
void ApproximateNearestNeighbors(const PointT &p, unsigned int k, EpsilonT epsilon, LimitedHeap< typename NNTypeHelper< PointT >::NNType, std::less< typename NNTypeHelper< PointT >::NNType >, ContainerT > *neighbors) const
Definition: KdTree.h:454
GfxTL::KdTreeCell::SplitAxis
unsigned int & SplitAxis()
Definition: KdTree.h:60
GfxTL::KdTree::NNTypeHelper::NNType
NN< typename BaseType::template DistanceType< typename ScalarTypeDeferer< PointT >::ScalarType, ScalarType >::Type, typename BaseType::DereferencedType > NNType
Definition: KdTree.h:450
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
GfxTL::BaseKdTreeStrategy::StrategyBase::LeaveGlobalBuildInformation
void LeaveGlobalBuildInformation(const CellType &cell, const BuildInformationT &bi)
Definition: KdTree.h:132
GfxTL::KdTree::NearestNeighborsAuxInfo::m_ti
TraversalInfoType m_ti
Definition: KdTree.h:420
GfxTL::KdTree::DereferencedType
BaseType::DereferencedType DereferencedType
Definition: KdTree.h:183
GfxTL::BaseKdTreeStrategy::StrategyBase
Definition: KdTree.h:112
VectorKernel.h
GfxTL::KdTree::PointsInBallAuxData::TraversalInfoType
PointsInBallAuxInfo< PointT >::TraversalInfoType TraversalInfoType
Definition: KdTree.h:366
GfxTL::KdTree::NearestNeighborsAuxInfo::DistanceType
BaseType::template DistanceType< typename ScalarTypeDeferer< PointT >::ScalarType, ScalarType >::Type DistanceType
Definition: KdTree.h:412
GfxTL::BaseKdTreeStrategy
Definition: KdTree.h:103
GfxTL::KdTree::BaseTraversalInformation::GlobalType
GlobalInfoT GlobalType
Definition: KdTree.h:226
GfxTL::KdTree::PointsInBallAuxData::DistanceType
PointsInBallAuxInfo< PointT >::DistanceType DistanceType
Definition: KdTree.h:367
NullClass.h
GfxTL::KdTree::BaseGlobalTraversalInformation::PointType
PointT PointType
Definition: KdTree.h:207
GfxTL::KdTreeCell::operator[]
const ThisType & operator[](unsigned int i) const
Definition: KdTree.h:50
GfxTL::LimitedHeap
Definition: LimitedHeap.h:14
GfxTL::KdTree::BaseTraversalInformation::Global
const GlobalInfoT & Global() const
Definition: KdTree.h:231
GfxTL::KdTree::CellType
KdTreeCell< typename StrategiesT::CellData > CellType
Definition: KdTree.h:168
GfxTL::KdTree::PointsInBallAuxInfo
Definition: KdTree.h:343
GfxTL::KdTree::InsertBack
void InsertBack()
Definition: KdTree.h:303
GfxTL::BaseKdTreeStrategy::StrategyBase::DereferencedType
BaseType::DereferencedType DereferencedType
Definition: KdTree.h:118
GfxTL::KdTreeCell::SplitValue
ScalarType & SplitValue()
Definition: KdTree.h:70
GfxTL::KdTree::NearestNeighborsAuxData::TraversalInfoType
NearestNeighborsAuxInfo< PointT >::TraversalInfoType TraversalInfoType
Definition: KdTree.h:428
MathHelper.h
GfxTL::BaseKdTreeStrategy::StrategyBase::TraversalInformationBase
Definition: KdTree.h:143
GfxTL::KdTree::BuildInformation::CreateChild
unsigned int & CreateChild()
Definition: KdTree.h:190
GfxTL::BaseKdTreeStrategy::CellData
Definition: KdTree.h:107
GfxTL::BaseKdTreeStrategy::value_type
DataStrategyT::value_type value_type
Definition: KdTree.h:105
ScalarTypeDeferer.h
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
GfxTL::KdTree::Contains
bool Contains(const PointT &p, DereferencedType *dref) const
Definition: KdTree.h:475
GfxTL::KdTree::NearestNeighbors
void NearestNeighbors(const PointT &p, unsigned int k, LimitedHeapT *neighbors, NearestNeighborsAuxData< PointT > *auxData) const
Definition: KdTree.h:433
GfxTL::KdTree::Remove
void Remove(DereferencedType s)
Definition: KdTree.h:315
GfxTL::KdTree::NearestNeighbors
void NearestNeighbors(const PointT &p, unsigned int k, LimitedHeapT *neighbors) const
Definition: KdTree.h:380