AAKdTree.hpp
Go to the documentation of this file.
1 
2 namespace GfxTL
3 {
4  //-- AAKdCell
5 
6  template <class Point, class Base>
7  AAKdCell<Point, Base>::AAKdCell()
8  {
9  memset(_children, 0, sizeof(_children));
10  };
11 
12  template <class Point, class Base>
13  AAKdCell<Point, Base>::AAKdCell(const AAKdCell& cell) :
14  _axis(cell._axis), _splitValue(cell._splitValue)
15  {
16  if (cell._children[0])
17  {
18  _children[0] = new AAKdCell<Point, Base>(*cell._children[0]);
19  }
20  else
21  {
22  _children[0] = NULL;
23  }
24  if (cell._children[1])
25  {
26  _children[1] = new AAKdCell<Point, Base>(*cell._children[1]);
27  }
28  else
29  {
30  _children[1] = NULL;
31  }
32  }
33 
34  template <class Point, class Base>
35  AAKdCell<Point, Base>::~AAKdCell()
36  {
37  for (unsigned int i = 0; i < 2; ++i)
38  if (_children[i])
39  {
40  delete _children[i];
41  }
42  }
43 
44  template <class Point, class Base>
45  const AAKdCell<Point, Base>*
46  AAKdCell<Point, Base>::operator[](unsigned int index) const
47  {
48  return _children[index];
49  }
50 
51  template <class Point, class Base>
52  AAKdCell<Point, Base>*
53  AAKdCell<Point, Base>::operator[](unsigned int index)
54  {
55  return _children[index];
56  }
57 
58  template <class Point, class Base>
59  void
60  AAKdCell<Point, Base>::Child(unsigned int index, ThisType* child)
61  {
62  _children[index] = child;
63  }
64 
65  template <class Point, class Base>
66  typename AAKdCell<Point, Base>::ScalarType
67  AAKdCell<Point, Base>::Split() const
68  {
69  return _splitValue;
70  }
71 
72  template <class Point, class Base>
73  void
74  AAKdCell<Point, Base>::Split(ScalarType split)
75  {
76  _splitValue = split;
77  }
78 
79  template <class Point, class Base>
80  unsigned int
81  AAKdCell<Point, Base>::Axis() const
82  {
83  return _axis;
84  }
85 
86  template <class Point, class Base>
87  void
88  AAKdCell<Point, Base>::Axis(unsigned int axis)
89  {
90  _axis = axis;
91  }
92 
93  //-- AAKdTree
94 
95  template <class Strategies>
96  void
97  AAKdTree<Strategies>::Build()
98  {
99  Clear();
100 
101  CellType* root = new CellType;
102  RootCellData(AACube<PointType>(), root); // implemented by TreeData
103  Root(root);
104 
105  std::list<CellType*> stack;
106  stack.push_back(root);
107  while (stack.size())
108  {
109  CellType* c = stack.back();
110  stack.pop_back();
111  if (ShouldSubdivide(c))
112  {
113  Subdivide(c);
114  if (IsLeaf(*c)) // couldn't subdivide?
115  {
116  continue;
117  }
118  for (unsigned int i = 0; i < CellType::NChildren; ++i)
119  {
120  stack.push_back((*c)[i]);
121  }
122  }
123  }
124  };
125 
126  template <class Strategies>
127  void
128  AAKdTree<Strategies>::PointsInSphere(const PointType& center,
129  ScalarType radius,
130  std::vector<size_t>* points) const
131  {
132  points->clear();
133  if (!Root())
134  {
135  return;
136  }
137  PointsInSphere(Root(), center, radius, points);
138  };
139 
140  template <class Strategies>
141  void
142  AAKdTree<Strategies>::PointsInAACube(const AACube<PointType>& cube,
143  std::vector<size_t>* points) const
144  {
145  points->clear();
146  if (!Root())
147  {
148  return;
149  }
150  PointType center;
151  cube.Center(&center);
152  PointsInAACube(Root(), center, cube.Width() / 2, points);
153  }
154 
155  template <class Strategies>
156  void
157  AAKdTree<Strategies>::PointsInAACube(const PointType& center,
158  ScalarType width,
159  std::vector<size_t>* points) const
160  {
161  points->clear();
162  if (!Root())
163  {
164  return;
165  }
166  PointsInAACube(Root(), center, width / 2, points);
167  }
168 
169  template <class Strategies>
170  void
171  AAKdTree<Strategies>::RefreshWithNewTreeData(const AACube<PointType>& bc)
172  {
173  // iterate over all cells and readjust cell data
174  CellType* c = Root();
175  if (!c)
176  {
177  c = new CellType;
178  Root(c);
179  }
180  RootCellData(AACube<PointType>(), c);
181  std::list<CellType*> stack;
182  stack.push_back(c);
183  while (stack.size())
184  {
185  c = stack.back();
186  stack.pop_back();
187  if (!IsLeaf(c))
188  {
189  if (!ShouldSubdivide(c))
190  {
191  for (unsigned int i = 0; i < CellType::NChildren; ++i)
192  {
193  delete (*c)[i];
194  c->Child(i, NULL);
195  }
196  }
197  else
198  {
199  ReadjustData(c);
200  for (unsigned int i = 0; i < CellType::NChildren; ++i)
201  {
202  stack.push_back((*c)[i]);
203  }
204  }
205  }
206  else if (ShouldSubdivide(c))
207  {
208  Subdivide(c);
209  for (unsigned int i = 0; i < CellType::NChildren; ++i)
210  {
211  stack.push_back((*c)[i]);
212  }
213  }
214  }
215  StrategyBaseType::RefreshWithNewTreeData(bc);
216  }
217 
218  template <class Strategies>
219  void
220  AAKdTree<Strategies>::NearestNeighbor(const PointType& p,
221  size_t* neighbor,
222  ScalarType* dist) const
223  {
224  *dist = std::numeric_limits<ScalarType>::infinity();
225  if (!Root())
226  {
227  return;
228  }
229  AABox<PointType> rootBox;
230  rootBox.Infinite();
231  NearestNeighbor(*Root(), rootBox, p, neighbor, dist);
232  if (*dist < std::numeric_limits<ScalarType>::infinity())
233  {
234  *dist = std::sqrt(*dist);
235  }
236  }
237 
238  template <class Strategies>
239  void
240  AAKdTree<Strategies>::KNearestNeighbors(const PointType& p,
241  unsigned int k,
242  std::vector<NN>* neighbors,
243  ScalarType* dist) const
244  {
245  *dist = std::numeric_limits<ScalarType>::infinity();
246  if (!Root())
247  {
248  return;
249  }
250  size_t worstIdx = 0;
251  AABox<PointType> rootBox;
252  rootBox.Infinite();
253  neighbors->clear();
254  neighbors->reserve(k + 1);
255  KNearestNeighbors(*Root(), rootBox, p, k, neighbors, &worstIdx, dist);
256  if (*dist < std::numeric_limits<ScalarType>::infinity())
257  {
258  *dist = std::sqrt(*dist);
259  }
260  }
261 
262  template <class Strategies>
263  void
264  AAKdTree<Strategies>::NearestNeighbor(const CellType& cell,
265  const AABox<PointType>& box,
266  const PointType& p,
267  size_t* neighbor,
268  ScalarType* dist2) const
269  {
270  if (IsLeaf(cell))
271  {
272  // let's find nearest neighbor from points within the cell
273  for (size_t i = 0; i < cell.Size(); ++i)
274  {
275  size_t idx = Translate(cell.Points() + i);
276  PointType diff = PointTranslated(idx) - p;
277  ScalarType d2 = diff * diff;
278  if (d2 < *dist2)
279  {
280  *dist2 = d2;
281  *neighbor = idx;
282  }
283  }
284  return;
285  }
286  // descent into subtree containing p
287  unsigned int nearerChild, fartherChild;
288  if (p[cell.Axis()] <= cell.Split())
289  {
290  nearerChild = 0;
291  fartherChild = 1;
292  }
293  else
294  {
295  nearerChild = 1;
296  fartherChild = 0;
297  }
298  AABox<PointType> subBoxes[2];
299  box.Split(cell.Axis(), cell.Split(), &subBoxes[0], &subBoxes[1]);
300  NearestNeighbor(*cell[nearerChild], subBoxes[nearerChild], p, neighbor, dist2);
301 
302  // check if closer neighbor could be in other child
303  if (Intersect(subBoxes[fartherChild], p, *dist2))
304  NearestNeighbor(*cell[fartherChild], subBoxes[fartherChild], p, neighbor, dist2);
305  }
306 
307  template <class Strategies>
308  void
309  AAKdTree<Strategies>::KNearestNeighbors(const CellType& cell,
310  const AABox<PointType>& box,
311  const PointType& p,
312  unsigned int k,
313  std::vector<NN>* neighbors,
314  size_t* worstIdx,
315  ScalarType* dist2) const
316  {
317  if (IsLeaf(cell))
318  {
319  // let's find nearest neighbors from points within the cell
320  for (size_t i = 0; i < cell.Size(); ++i)
321  {
322  size_t idx = Translate(cell.Points() + i);
323  PointType diff = PointTranslated(idx) - p;
324  ScalarType d2 = diff * diff;
325  if (neighbors->size() < k)
326  {
327  neighbors->push_back(NN(idx, d2));
328  FindFurthestNearestNeighbor(*neighbors, neighbors->size(), worstIdx, dist2);
329  }
330  else if (d2 < *dist2)
331  {
332  (*neighbors)[*worstIdx] = NN(idx, d2);
333  FindFurthestNearestNeighbor(*neighbors, neighbors->size(), worstIdx, dist2);
334  }
335  }
336  return;
337  }
338  // descent into subtree containing p
339  unsigned int nearerChild, fartherChild;
340  if (p[cell.Axis()] <= cell.Split())
341  {
342  nearerChild = 0;
343  fartherChild = 1;
344  }
345  else
346  {
347  nearerChild = 1;
348  fartherChild = 0;
349  }
350  AABox<PointType> subBoxes[2];
351  box.Split(cell.Axis(), cell.Split(), &subBoxes[0], &subBoxes[1]);
352 
353  KNearestNeighbors(
354  *cell[nearerChild], subBoxes[nearerChild], p, k, neighbors, worstIdx, dist2);
355 
356  // check if closer neighbor could be in other child
357  if (neighbors->size() < k || Intersect(subBoxes[fartherChild], p, *dist2))
358  KNearestNeighbors(
359  *cell[fartherChild], subBoxes[fartherChild], p, k, neighbors, worstIdx, dist2);
360  }
361 
362  template <class Strategies>
363  void
364  AAKdTree<Strategies>::ReadjustData(CellType* cell)
365  {
366  if (!cell->Size())
367  {
368  return;
369  }
370  PointType pmin, pmax;
371  pmax = pmin = PointTranslated(Translate(cell->Points()));
372  for (size_t i = 1; i < cell->Size(); ++i)
373  {
374  const PointType& p = PointTranslated(Translate(cell->Points() + i));
375  for (unsigned int j = 0; j < Dim; ++j)
376  {
377  if (pmin[j] > p[j])
378  {
379  pmin[j] = p[j];
380  }
381  else if (pmax[j] < p[j])
382  {
383  pmax[j] = p[j];
384  }
385  }
386  }
387 
388  PointType pdiff = pmax - pmin;
389  unsigned int axis = 0;
390  ScalarType length = pdiff[0];
391  for (unsigned int j = 1; j < Dim; ++j)
392  {
393  if (pdiff[j] > length)
394  {
395  axis = j;
396  length = pdiff[j];
397  }
398  }
399 
400  ScalarType split = (pmax[axis] + pmin[axis]) / 2;
401 
402  CellType *left = (*cell)[0], *right = (*cell)[1];
403  SplitAlongAxis(*cell, axis, split, left, right); // implemented by TreeData
404  cell->Split(split);
405  cell->Axis(axis);
406  }
407 
408  template <class Strategies>
409  void
410  AAKdTree<Strategies>::PointsInSphere(const CellType* cell,
411  const PointType& center,
412  ScalarType radius,
413  std::vector<size_t>* points) const
414  {
415  if (IsLeaf(cell))
416  {
417  ScalarType radius2 = radius * radius;
418  for (size_t i = 0; i < cell->Size(); ++i)
419  {
420  size_t ind = Translate(cell->Points() + i);
421  const PointType& p = PointTranslated(ind);
422  PointType d = p - center;
423  if ((d * d) <= radius2)
424  {
425  points->push_back(ind);
426  }
427  }
428  }
429  else
430  {
431  ScalarType d = center[cell->Axis()] - cell->Split();
432  if (d + radius >= 0)
433  {
434  PointsInSphere((*cell)[1], center, radius, points);
435  }
436  if (d - radius <= 0)
437  {
438  PointsInSphere((*cell)[0], center, radius, points);
439  }
440  }
441  }
442 
443  template <class Strategies>
444  void
445  AAKdTree<Strategies>::PointsInAACube(const CellType* cell,
446  const PointType& center,
447  ScalarType radius,
448  std::vector<HandleType>* points) const
449  {
450  if (IsLeaf(cell))
451  {
452  for (size_t i = 0; i < cell->Size(); ++i)
453  {
454  size_t ind = Translate(cell->Points() + i);
455  const PointType& p = PointTranslated(ind);
456  PointType d = p - center;
457  bool isInside = true;
458  for (unsigned int j = 0; j < PointType::Dim; ++j)
459  {
460  if (Absolute(d[j]) > radius)
461  {
462  isInside = false;
463  break;
464  }
465  }
466  if (isInside)
467  {
468  points->push_back(ind);
469  }
470  }
471  }
472  else
473  {
474  ScalarType d = center[cell->Axis()] - cell->Split();
475  if (d + radius >= 0)
476  {
477  PointsInAACube((*cell)[1], center, radius, points);
478  }
479  if (d - radius <= 0)
480  {
481  PointsInAACube((*cell)[0], center, radius, points);
482  }
483  }
484  }
485 
486  template <class Strategies>
487  bool
488  AAKdTree<Strategies>::ShouldSubdivide(const CellType* cell) const
489  {
490  if (cell->Size() > 10)
491  {
492  return true;
493  }
494  return false;
495  }
496 
497  template <class Strategies>
498  void
499  AAKdTree<Strategies>::Subdivide(CellType* cell)
500  {
501  PointType pmin, pmax;
502  pmax = pmin = PointTranslated(Translate(cell->Points()));
503  for (size_t i = 1; i < cell->Size(); ++i)
504  {
505  const PointType& p = PointTranslated(Translate(cell->Points() + i));
506  for (unsigned int j = 0; j < Dim; ++j)
507  {
508  if (pmin[j] > p[j])
509  {
510  pmin[j] = p[j];
511  }
512  else if (pmax[j] < p[j])
513  {
514  pmax[j] = p[j];
515  }
516  }
517  }
518 
519  PointType pdiff = pmax - pmin;
520  unsigned int axis = 0;
521  ScalarType length = pdiff[0];
522  for (unsigned int j = 1; j < Dim; ++j)
523  {
524  if (pdiff[j] > length)
525  {
526  axis = j;
527  length = pdiff[j];
528  }
529  }
530 
531  ScalarType split = (pmax[axis] + pmin[axis]) / 2;
532 
533  CellType *left, *right;
534  left = new CellType;
535  right = new CellType;
536  SplitAlongAxis(*cell, axis, split, left, right); // implemented by TreeData
537 
538  if (left->Size() == cell->Size() || right->Size() == cell->Size())
539  {
540  delete left;
541  delete right;
542  return;
543  }
544  cell->Split(split);
545  cell->Axis(axis);
546  cell->Child(0, left);
547  cell->Child(1, right);
548  }
549 
550  template <class Strategies>
551  typename AAKdTree<Strategies>::ScalarType
552  AAKdTree<Strategies>::Absolute(ScalarType f) const
553  {
554  if (f < 0)
555  {
556  return -f;
557  }
558  return f;
559  }
560 
561 }; // namespace GfxTL
GfxTL::AAKdTree::Build
void Build()
Definition: AAKdTree.hpp:97
GfxTL::AACube
Definition: AACube.h:11
armarx::view_selection::skills::direction::state::right
state::Type right(state::Type previous)
Definition: LookDirection.cpp:264
visionx::armem::pointcloud::PointType
PointType
Definition: constants.h:78
index
uint8_t index
Definition: EtherCATFrame.h:59
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
GfxTL::AACube::Center
void Center(Point *c) const
Definition: AACube.hpp:52
GfxTL::AAKdCell
Definition: AAKdTree.h:17
GfxTL::AABox::Split
void Split(unsigned int axis, ScalarType s, AABox< Point > *left, AABox< Point > *right) const
Definition: AABox.hpp:22
GfxTL::AAKdTree::KNearestNeighbors
void KNearestNeighbors(const PointType &p, unsigned int k, std::vector< NN > *neighbors, ScalarType *dist) const
Definition: AAKdTree.hpp:240
armarx::navigation::core::NavigationFrame::Absolute
@ Absolute
GfxTL::AABox::Infinite
void Infinite()
Definition: AABox.hpp:11
Point
Definition: PointCloud.h:21
GfxTL::AAKdCell::ScalarType
Point::ScalarType ScalarType
Definition: AAKdTree.h:23
GfxTL
Definition: AABox.h:9
GfxTL::AAKdCell::AAKdCell
AAKdCell()
Definition: AAKdTree.hpp:7
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
GfxTL::AAKdCell::Split
ScalarType Split() const
Definition: AAKdTree.hpp:67
GfxTL::AAKdTree::ScalarType
CellType::ScalarType ScalarType
Definition: AAKdTree.h:58
GfxTL::FindFurthestNearestNeighbor
void FindFurthestNearestNeighbor(const NNs &n, size_t size, size_t *worst, ScalarType *worstDist)
Definition: NearestNeighbors.h:124
GfxTL::NearestNeighbor
Definition: NearestNeighbors.h:10
armarx::view_selection::skills::direction::state::center
state::Type center(state::Type previous)
Definition: LookDirection.cpp:233
GfxTL::AAKdTree::RefreshWithNewTreeData
void RefreshWithNewTreeData(const AACube< PointType > &bc)
Definition: AAKdTree.hpp:171
GfxTL::AAKdCell::Child
void Child(unsigned int index, ThisType *child)
Definition: AAKdTree.hpp:60
GfxTL::AAKdTree::PointsInSphere
void PointsInSphere(const PointType &center, ScalarType radius, std::vector< size_t > *points) const
Definition: AAKdTree.hpp:128
GfxTL::AAKdCell::operator[]
const ThisType * operator[](unsigned int index) const
Definition: AAKdTree.hpp:46
GfxTL::AACube::Width
ScalarType Width() const
Definition: AACube.hpp:132
armarx::view_selection::skills::direction::state::left
state::Type left(state::Type previous)
Definition: LookDirection.cpp:258
GfxTL::AAKdCell::Axis
unsigned int Axis() const
Definition: AAKdTree.hpp:81
GfxTL::AAKdTree::NearestNeighbor
void NearestNeighbor(const PointType &p, size_t *neighbor, ScalarType *dist) const
Definition: AAKdTree.hpp:220
GfxTL::AAKdCell::~AAKdCell
~AAKdCell()
Definition: AAKdTree.hpp:35
GfxTL::AABox
Definition: AABox.h:19
GfxTL::AAKdTree::PointsInAACube
void PointsInAACube(const AACube< PointType > &cube, std::vector< size_t > *points) const
Definition: AAKdTree.hpp:142
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:38