SyB3R - Synthetic Benchmark for 3D Reconstruction
KNearestNeighbors.hpp
1 #ifndef KNEARESTNEIGHBORS_HPP_
2 #define KNEARESTNEIGHBORS_HPP_
3 
4 #include <vector>
5 #include <memory>
6 
7 namespace syb3r {
8 namespace math {
9 
10 
11 namespace kNearestNeighbors {
12 
13 template<class FeatureSpace, typename Fact>
14 struct QueueEntry
15 {
16  typename FeatureSpace::Metric::Type metric;
17  typename Fact::Handle handle;
18  inline bool operator<(const QueueEntry &rhs) const {
19  return metric < rhs.metric;
20  }
21 };
22 
23 template<class FeatureSpace, typename Fact>
24 class Node
25 {
26  public:
27  Node(const Fact &fact);
28  void addFact(const Fact &fact);
29  void getkNearestNeighbors(const typename FeatureSpace::Features &features, std::vector<QueueEntry<FeatureSpace, Fact>> &facts, unsigned k) const;
30  void getNearestNeighbor(const typename FeatureSpace::Features &features, QueueEntry<FeatureSpace, Fact> &fact) const;
31 
32  inline const Fact &getFact() const { return m_fact; }
33  private:
35  Fact m_fact;
36  std::unique_ptr<NodeType> m_subNodes[2];
37  typename FeatureSpace::SplitType m_splittinFeature;
38 };
39 
40 template<class FeatureSpace, typename Fact>
41 class Dataset
42 {
43  public:
45  void addFact(const Fact &fact);
46  void getkNearestNeighbors(const typename FeatureSpace::Features &features, std::vector<QueueEntry<FeatureSpace, Fact>> &facts, unsigned k) const;
47  void getNearestNeighbor(const typename FeatureSpace::Features &features, QueueEntry<FeatureSpace, Fact> &facts) const;
48  private:
49  std::unique_ptr<NodeType> m_rootNode;
50 };
51 
52 
53 template<class FeatureSpace, class Fact>
54 Node<FeatureSpace, Fact>::Node(const Fact &fact)
55 {
56  m_fact = fact;
57  m_splittinFeature = FeatureSpace::NO_SPLIT;
58 }
59 
60 template<class FeatureSpace, class Fact>
61 void Node<FeatureSpace, Fact>::addFact(const Fact &fact)
62 {
63  if (m_splittinFeature == FeatureSpace::NO_SPLIT)
64  m_splittinFeature = FeatureSpace::chooseSplittingFeature(m_fact, fact);
65 
66  unsigned index = FeatureSpace::compare(m_fact, fact, m_splittinFeature);
67 
68  if (m_subNodes[index] == nullptr)
69  m_subNodes[index].reset(new Node<FeatureSpace, Fact>(fact));
70  else
71  m_subNodes[index]->addFact(fact);
72 }
73 
74 template<class FeatureSpace, typename Fact>
75 void insertSorted(std::vector<QueueEntry<FeatureSpace, Fact>> &facts,
76  const QueueEntry<FeatureSpace, Fact> &newFact)
77 {
78  unsigned start = 0;
79  unsigned end = facts.size()-1;
80 
81  while (start != end) {
82  unsigned center = (start + end) / 2;
83  if (facts[center] < newFact) {
84  start = center+1;
85  } else {
86  end = center;
87  }
88  }
89 
90  unsigned index = start;
91  for (unsigned i = facts.size()-1; i > index; i--) {
92  facts[i] = facts[i-1];
93  }
94  facts[index] = newFact;
95 }
96 
97 
98 template<class FeatureSpace, class Fact>
99 void Node<FeatureSpace, Fact>::getkNearestNeighbors(const typename FeatureSpace::Features &features,
100  std::vector<QueueEntry<FeatureSpace, Fact>> &facts,
101  unsigned k) const
102 {
103  typename FeatureSpace::Metric::Type metric = FeatureSpace::Metric::compute(features, m_fact);
104 
105  if (facts.size() < k) {
106  facts.push_back(QueueEntry<FeatureSpace, Fact>{metric, m_fact.getHandle()});
107 
108  if (facts.size() == k)
109  std::sort(facts.begin(), facts.end());
110  } else {
111  if (metric < facts[k-1].metric)
112  insertSorted<FeatureSpace, Fact>(facts, QueueEntry<FeatureSpace, Fact>{metric, m_fact.getHandle()});
113  }
114 
115  if (m_splittinFeature != FeatureSpace::NO_SPLIT) {
116  unsigned first = FeatureSpace::compare(m_fact, features, m_splittinFeature);
117 
118  if (m_subNodes[first] != nullptr)
119  m_subNodes[first]->getkNearestNeighbors(features, facts, k);
120 
121  if ((facts.size() < k) || FeatureSpace::Metric::stillInRange(m_fact, features, facts[k-1].metric, m_splittinFeature)) {
122  unsigned second = first ^ 1;
123  if (m_subNodes[second] != nullptr)
124  m_subNodes[second]->getkNearestNeighbors(features, facts, k);
125  }
126  }
127 }
128 
129 
130 template<class FeatureSpace, class Fact>
131 void Node<FeatureSpace, Fact>::getNearestNeighbor(const typename FeatureSpace::Features &features,
132  QueueEntry<FeatureSpace, Fact> &fact) const
133 {
134  typename FeatureSpace::Metric::Type metric = FeatureSpace::Metric::compute(features, m_fact);
135 
136  if (metric < fact.metric)
137  fact = QueueEntry<FeatureSpace, Fact>{metric, m_fact.getHandle()};
138 
139  if (m_splittinFeature != FeatureSpace::NO_SPLIT) {
140  unsigned first = FeatureSpace::compare(m_fact, features, m_splittinFeature);
141 
142  if (m_subNodes[first] != nullptr)
143  m_subNodes[first]->getNearestNeighbor(features, fact);
144 
145  if (FeatureSpace::Metric::stillInRange(m_fact, features, fact.metric, m_splittinFeature)) {
146  unsigned second = first ^ 1;
147  if (m_subNodes[second] != nullptr)
148  m_subNodes[second]->getNearestNeighbor(features, fact);
149  }
150  }
151 }
152 
153 
154 
155 template<class FeatureSpace, typename Fact>
156 void Dataset<FeatureSpace, Fact>::addFact(const Fact &fact)
157 {
158  if (m_rootNode == nullptr)
159  m_rootNode.reset(new Node<FeatureSpace, Fact>(fact));
160  else
161  m_rootNode->addFact(fact);
162 }
163 
164 template<class FeatureSpace, typename Fact>
165 void Dataset<FeatureSpace, Fact>::getkNearestNeighbors(const typename FeatureSpace::Features &features,
166  std::vector<QueueEntry<FeatureSpace, Fact>> &facts,
167  unsigned k) const
168 {
169  facts.clear();
170  facts.reserve(k);
171 
172  if (m_rootNode != nullptr)
173  m_rootNode->getkNearestNeighbors(features, facts, k);
174 }
175 
176 template<class FeatureSpace, typename Fact>
177 void Dataset<FeatureSpace, Fact>::getNearestNeighbor(const typename FeatureSpace::Features &features,
178  QueueEntry<FeatureSpace, Fact> &fact) const
179 {
180  fact.metric = 1e30f;
181 
182  if (m_rootNode != nullptr)
183  m_rootNode->getNearestNeighbor(features, fact);
184 }
185 
186 }
187 }
188 }
189 
190 #endif
Definition: CameraPathEvaluation.cpp:10
Definition: KNearestNeighbors.hpp:24
Definition: KNearestNeighbors.hpp:14
Definition: KNearestNeighbors.hpp:41