NearestNeighbors.h
Go to the documentation of this file.
1 #ifndef __GfxTL_NEARESTNEIGHBORS_HEADER__
2 #define __GfxTL_NEARESTNEIGHBORS_HEADER__
3 #include <limits>
4 #include <GfxTL/LimitedHeap.h>
5 
6 namespace GfxTL
7 {
8  template< class Scalar, class HandleT = size_t >
10  {
11  typedef Scalar ScalarType;
12  typedef HandleT HandleType;
13 
15  {}
16 
18  : index(i)
19  , sqrDist(d)
20  {}
21 
22  bool operator<(const NearestNeighbor& nn) const
23  {
24  return sqrDist < nn.sqrDist;
25  }
26 
27  bool operator>(const NearestNeighbor& nn) const
28  {
29  return sqrDist > nn.sqrDist;
30  }
31 
32  bool operator<=(const NearestNeighbor& nn) const
33  {
34  return sqrDist <= nn.sqrDist;
35  }
36 
37  bool operator>=(const NearestNeighbor& nn) const
38  {
39  return sqrDist >= nn.sqrDist;
40  }
41 
42  operator HandleType& ()
43  {
44  return index;
45  }
46 
47  operator const HandleType& () const
48  {
49  return index;
50  }
51 
54  };
55 
56  template< class NN, class NNs >
57  void SortedNearestNeighborInsert(const NN& nn, unsigned int k, NNs* n)
58  {
59  /*typename NNs::iterator low = n->begin(), high = n->end() - 1,
60  index = low, mid;
61  if(n->size() > 0)
62  {
63  while(low <= high)
64  {
65  mid = low + ((high - low) >> 1);
66  if(*mid <= nn)
67  {
68  low = mid + 1;
69  index = low;
70  }
71  else
72  {
73  index = mid;
74  high = mid - 1;
75  }
76  }
77  }
78  n->insert(index, nn);*/
79  intptr_t low = 0, high = n->size() - 1, index = 0, mid;
80  if (n->size() > 0)
81  {
82  while (low <= high)
83  {
84  mid = (low + high) >> 1;
85  if ((*n)[mid] <= nn)
86  {
87  low = mid + 1;
88  index = low;
89  }
90  else
91  {
92  index = mid;
93  high = mid - 1;
94  }
95  }
96  }
97  n->insert(n->begin() + index, nn);
98  if (n->size() > k)
99  {
100  if ((*n)[k] > (*n)[k - 1])
101  {
102  n->resize(k);
103  }
104  }
105  /*
106  // remove all that are too far away
107  typename NNs::iterator begin = n->begin() +
108  Math< typename NNs::size_type >::Min(n->size() - 1, k - 1),
109  nEnd = n->end();
110  while(begin + 1 != nEnd && begin->sqrDist == (begin + 1)->sqrDist)
111  ++begin;
112  if(begin + 1 != nEnd)
113  n->erase(begin + 1, nEnd);*/
114  }
115 
116  template< class NNs, class ScalarType >
117  void FindFurthestNearestNeighbor(const NNs& n, size_t size, size_t* worst,
118  ScalarType* worstDist)
119  {
120  size_t j = 0;
121  *worst = j;
122  *worstDist = n[j].sqrDist;
123  ++j;
124  for (; j < size; ++j)
125  {
126  if (n[j].sqrDist > *worstDist)
127  {
128  *worstDist = n[j].sqrDist;
129  *worst = j;
130  }
131  }
132  }
133 
134  template< class Point, class Points >
135  void NearestNeighbors(unsigned int k, const Point& p,
136  const Points& points, size_t size,
137  std::vector< NearestNeighbor< typename Points::size_type,
138  typename Point::ScalarType > >* n)
139  {
140  typedef Point PointType;
141  typedef typename Point::ScalarType ScalarType;
143  typedef typename Points::size_type size_type;
144  n->reserve(k);
145  size_type worst;
146  ScalarType worstDist = 0;
147  size_type i = 0;
148  for (; i < size && i < k; ++i)
149  {
150  PointType diff = points[i] - p;
151  n->push_back(NN(i, diff * diff));
152  if (n->back().sqrDist > worstDist)
153  {
154  worstDist = n->back().sqrDist;
155  worst = n->size() - 1;
156  }
157  }
158 
159  for (i = k; i < size; ++i)
160  {
161  PointType diff = points[i] - p;
162  ScalarType dist = diff * diff;
163  if (dist < worstDist)
164  {
165  (*n)[worst] = NN(i, dist);
166  FindFurthestNearestNeighbor(*n, n->size(), &worst, &worstDist);
167  }
168  }
169  }
170 
171  template< class Point, class Points, template< class > class ContainerT,
172  class NN >
173  void BruteForceNearestNeighbors(unsigned int k, const Point& p,
174  const Points& points, size_t size,
175  LimitedHeap< NN, std::less<NN>, ContainerT >* neighbors)
176  {
177  neighbors->clear();
178  neighbors->Limit(k);
179  for (size_t i = 0; i < size; ++i)
180  {
181  NN nn(p.SqrDistance(points[i]), i);
182  neighbors->PushHeap(nn);
183  }
184  }
185 
186  template< class Point, class Points, template< class > class ContainerT,
187  class NN >
188  void BruteForceL1NearestNeighbors(unsigned int k, const Point& p,
189  const Points& points, size_t size,
190  LimitedHeap< NN, std::less<NN>, ContainerT >* neighbors)
191  {
192  neighbors->clear();
193  neighbors->Limit(k);
194  for (size_t i = 0; i < size; ++i)
195  {
196  NN nn(p.L1Distance(points[i]), i);
197  neighbors->PushHeap(nn);
198  }
199  }
200 };
201 
202 #endif
GfxTL::NearestNeighbor::HandleType
HandleT HandleType
Definition: NearestNeighbors.h:12
visionx::armem::pointcloud::PointType
PointType
Definition: constants.h:78
GfxTL::NearestNeighbors
void NearestNeighbors(unsigned int k, const Point &p, const Points &points, size_t size, std::vector< NearestNeighbor< typename Points::size_type, typename Point::ScalarType > > *n)
Definition: NearestNeighbors.h:135
GfxTL::NearestNeighbor::ScalarType
Scalar ScalarType
Definition: NearestNeighbors.h:11
index
uint8_t index
Definition: EtherCATFrame.h:59
visionx::Points
std::vector< Point > Points
Definition: ObjectShapeClassification.h:70
GfxTL::NearestNeighbor::NearestNeighbor
NearestNeighbor(HandleType i, ScalarType d)
Definition: NearestNeighbors.h:17
GfxTL::NearestNeighbor::operator>
bool operator>(const NearestNeighbor &nn) const
Definition: NearestNeighbors.h:27
LimitedHeap.h
Point::ScalarType
float ScalarType
Definition: PointCloud.h:24
GfxTL::NearestNeighbor::operator>=
bool operator>=(const NearestNeighbor &nn) const
Definition: NearestNeighbors.h:37
GfxTL::BruteForceNearestNeighbors
void BruteForceNearestNeighbors(unsigned int k, const Point &p, const Points &points, size_t size, LimitedHeap< NN, std::less< NN >, ContainerT > *neighbors)
Definition: NearestNeighbors.h:173
GfxTL::NN
Definition: NearestNeighbor.h:8
GfxTL::NearestNeighbor::sqrDist
ScalarType sqrDist
Definition: NearestNeighbors.h:53
GfxTL::SortedNearestNeighborInsert
void SortedNearestNeighborInsert(const NN &nn, unsigned int k, NNs *n)
Definition: NearestNeighbors.h:57
GfxTL::NearestNeighbor::operator<
bool operator<(const NearestNeighbor &nn) const
Definition: NearestNeighbors.h:22
GfxTL::BruteForceL1NearestNeighbors
void BruteForceL1NearestNeighbors(unsigned int k, const Point &p, const Points &points, size_t size, LimitedHeap< NN, std::less< NN >, ContainerT > *neighbors)
Definition: NearestNeighbors.h:188
Point
Definition: PointCloud.h:21
GfxTL
Definition: AABox.h:8
GfxTL::FindFurthestNearestNeighbor
void FindFurthestNearestNeighbor(const NNs &n, size_t size, size_t *worst, ScalarType *worstDist)
Definition: NearestNeighbors.h:117
GfxTL::NearestNeighbor::operator<=
bool operator<=(const NearestNeighbor &nn) const
Definition: NearestNeighbors.h:32
GfxTL::NearestNeighbor
Definition: NearestNeighbors.h:9
Scalar
float Scalar
Definition: basic.h:14
GfxTL::LimitedHeap
Definition: LimitedHeap.h:14
GfxTL::NearestNeighbor::NearestNeighbor
NearestNeighbor()
Definition: NearestNeighbors.h:14
GfxTL::NearestNeighbor::index
HandleType index
Definition: NearestNeighbors.h:52