MauiKit Image Tools

modules/geolocation/kdtree.cpp
1/*
2 * file: KDTree.hpp
3 * author: J. Frederico Carvalho
4 *
5 * This is an adaptation of the KD-tree implementation in rosetta code
6 * https://rosettacode.org/wiki/K-d_tree
7 *
8 * It is a reimplementation of the C code using C++. It also includes a few
9 * more queries than the original, namely finding all points at a distance
10 * smaller than some given distance to a point.
11 *
12 */
13
14#include <algorithm>
15#include <cmath>
16#include <functional>
17#include <iterator>
18#include <limits>
19#include <memory>
20#include <vector>
21
22#include "kdtree.hpp"
23
24KDNode::KDNode() = default;
25
26KDNode::KDNode(const point_t &pt, const size_t &idx_, const KDNodePtr &left_,
27 const KDNodePtr &right_) {
28 x = pt;
29 index = idx_;
30 left = left_;
31 right = right_;
32}
33
34KDNode::KDNode(const pointIndex &pi, const KDNodePtr &left_,
35 const KDNodePtr &right_) {
36 x = pi.first;
37 index = pi.second;
38 left = left_;
39 right = right_;
40}
41
42KDNode::~KDNode() = default;
43
44double KDNode::coord(const size_t &idx) { return x.at(idx); }
45KDNode::operator bool() { return (!x.empty()); }
46KDNode::operator point_t() { return x; }
47KDNode::operator size_t() { return index; }
48KDNode::operator pointIndex() { return pointIndex(x, index); }
49
50KDNodePtr NewKDNodePtr() {
51 KDNodePtr mynode = std::make_shared< KDNode >();
52 return mynode;
53}
54
55inline double dist2(const point_t &a, const point_t &b) {
56 double distc = 0;
57 for (size_t i = 0; i < a.size(); i++) {
58 double di = a.at(i) - b.at(i);
59 distc += di * di;
60 }
61 return distc;
62}
63
64inline double dist2(const KDNodePtr &a, const KDNodePtr &b) {
65 return dist2(a->x, b->x);
66}
67
68inline double dist(const point_t &a, const point_t &b) {
69 return std::sqrt(dist2(a, b));
70}
71
72inline double dist(const KDNodePtr &a, const KDNodePtr &b) {
73 return std::sqrt(dist2(a, b));
74}
75
76comparer::comparer(size_t idx_) : idx{idx_} {};
77
78inline bool comparer::compare_idx(const pointIndex &a, //
79 const pointIndex &b //
80) {
81 return (a.first.at(idx) < b.first.at(idx)); //
82}
83
84inline void sort_on_idx(const pointIndexArr::iterator &begin, //
85 const pointIndexArr::iterator &end, //
86 size_t idx) {
87 comparer comp(idx);
88 comp.idx = idx;
89
90 using std::placeholders::_1;
91 using std::placeholders::_2;
92
93 std::nth_element(begin, begin + std::distance(begin, end) / 2,
94 end, std::bind(&comparer::compare_idx, comp, _1, _2));
95}
96
97using pointVec = std::vector< point_t >;
98
99KDNodePtr KDTree::make_tree(const pointIndexArr::iterator &begin, //
100 const pointIndexArr::iterator &end, //
101 const size_t &length, //
102 const size_t &level //
103) {
104 if (begin == end) {
105 return NewKDNodePtr(); // empty tree
106 }
107
108 size_t dim = begin->first.size();
109
110 if (length > 1) {
111 sort_on_idx(begin, end, level);
112 }
113
114 auto middle = begin + (length / 2);
115
116 auto l_begin = begin;
117 auto l_end = middle;
118 auto r_begin = middle + 1;
119 auto r_end = end;
120
121 size_t l_len = length / 2;
122 size_t r_len = length - l_len - 1;
123
124 KDNodePtr left;
125 if (l_len > 0 && dim > 0) {
126 left = make_tree(l_begin, l_end, l_len, (level + 1) % dim);
127 } else {
128 left = leaf;
129 }
130 KDNodePtr right;
131 if (r_len > 0 && dim > 0) {
132 right = make_tree(r_begin, r_end, r_len, (level + 1) % dim);
133 } else {
134 right = leaf;
135 }
136
137 // KDNode result = KDNode();
138 return std::make_shared< KDNode >(*middle, left, right);
139}
140
141KDTree::KDTree(pointVec point_array) {
142 leaf = std::make_shared< KDNode >();
143 // iterators
144 pointIndexArr arr;
145 for (size_t i = 0; i < point_array.size(); i++) {
146 arr.push_back(pointIndex(point_array.at(i), i));
147 }
148
149 auto begin = arr.begin();
150 auto end = arr.end();
151
152 size_t length = arr.size();
153 size_t level = 0; // starting
154
155 root = KDTree::make_tree(begin, end, length, level);
156
157 m_empty = point_array.size();
158}
159
160KDNodePtr KDTree::nearest_( //
161 const KDNodePtr &branch, //
162 const point_t &pt, //
163 const size_t &level, //
164 const KDNodePtr &best, //
165 const double &best_dist //
166) {
167 double d, dx, dx2;
168
169 if (!bool(*branch)) {
170 return NewKDNodePtr(); // basically, null
171 }
172
173 point_t branch_pt(*branch);
174 size_t dim = branch_pt.size();
175
176 d = dist2(branch_pt, pt);
177 dx = branch_pt.at(level) - pt.at(level);
178 dx2 = dx * dx;
179
180 KDNodePtr best_l = best;
181 double best_dist_l = best_dist;
182
183 if (d < best_dist) {
184 best_dist_l = d;
185 best_l = branch;
186 }
187
188 size_t next_lv = (level + 1) % dim;
189 KDNodePtr section;
190 KDNodePtr other;
191
192 // select which branch makes sense to check
193 if (dx > 0) {
194 section = branch->left;
195 other = branch->right;
196 } else {
197 section = branch->right;
198 other = branch->left;
199 }
200
201 // keep nearest neighbor from further down the tree
202 KDNodePtr further = nearest_(section, pt, next_lv, best_l, best_dist_l);
203 if (!further->x.empty()) {
204 double dl = dist2(further->x, pt);
205 if (dl < best_dist_l) {
206 best_dist_l = dl;
207 best_l = further;
208 }
209 }
210 // only check the other branch if it makes sense to do so
211 if (dx2 < best_dist_l) {
212 further = nearest_(other, pt, next_lv, best_l, best_dist_l);
213 if (!further->x.empty()) {
214 double dl = dist2(further->x, pt);
215 if (dl < best_dist_l) {
216 best_dist_l = dl;
217 best_l = further;
218 }
219 }
220 }
221
222 return best_l;
223};
224
225// default caller
226KDNodePtr KDTree::nearest_(const point_t &pt) {
227 size_t level = 0;
228 // KDNodePtr best = branch;
229 double branch_dist = dist2(point_t(*root), pt);
230 return nearest_(root, // beginning of tree
231 pt, // point we are querying
232 level, // start from level 0
233 root, // best is the root
234 branch_dist); // best_dist = branch_dist
235};
236
237point_t KDTree::nearest_point(const point_t &pt) {
238 return point_t(*nearest_(pt));
239};
240size_t KDTree::nearest_index(const point_t &pt) {
241 return size_t(*nearest_(pt));
242};
243
244pointIndex KDTree::nearest_pointIndex(const point_t &pt) {
245 KDNodePtr Nearest = nearest_(pt);
246 return pointIndex(point_t(*Nearest), size_t(*Nearest));
247}
248
249bool KDTree::empty()
250{
251 return m_empty;
252}
253
254pointIndexArr KDTree::neighborhood_( //
255 const KDNodePtr &branch, //
256 const point_t &pt, //
257 const double &rad, //
258 const size_t &level //
259) {
260 double d, dx, dx2;
261
262 if (!bool(*branch)) {
263 // branch has no point, means it is a leaf,
264 // no points to add
265 return pointIndexArr();
266 }
267
268 size_t dim = pt.size();
269
270 double r2 = rad * rad;
271
272 d = dist2(point_t(*branch), pt);
273 dx = point_t(*branch).at(level) - pt.at(level);
274 dx2 = dx * dx;
275
276 pointIndexArr nbh, nbh_s, nbh_o;
277 if (d <= r2) {
278 nbh.push_back(pointIndex(*branch));
279 }
280
281 //
282 KDNodePtr section;
283 KDNodePtr other;
284 if (dx > 0) {
285 section = branch->left;
286 other = branch->right;
287 } else {
288 section = branch->right;
289 other = branch->left;
290 }
291
292 nbh_s = neighborhood_(section, pt, rad, (level + 1) % dim);
293 nbh.insert(nbh.end(), nbh_s.begin(), nbh_s.end());
294 if (dx2 < r2) {
295 nbh_o = neighborhood_(other, pt, rad, (level + 1) % dim);
296 nbh.insert(nbh.end(), nbh_o.begin(), nbh_o.end());
297 }
298
299 return nbh;
300};
301
302pointIndexArr KDTree::neighborhood( //
303 const point_t &pt, //
304 const double &rad) {
305 size_t level = 0;
306 return neighborhood_(root, pt, rad, level);
307}
308
309pointVec KDTree::neighborhood_points( //
310 const point_t &pt, //
311 const double &rad) {
312 size_t level = 0;
313 pointIndexArr nbh = neighborhood_(root, pt, rad, level);
314 pointVec nbhp;
315 nbhp.resize(nbh.size());
316 std::transform(nbh.begin(), nbh.end(), nbhp.begin(),
317 [](pointIndex x) { return x.first; });
318 return nbhp;
319}
320
321indexArr KDTree::neighborhood_indices( //
322 const point_t &pt, //
323 const double &rad) {
324 size_t level = 0;
325 pointIndexArr nbh = neighborhood_(root, pt, rad, level);
326 indexArr nbhi;
327 nbhi.resize(nbh.size());
328 std::transform(nbh.begin(), nbh.end(), nbhi.begin(),
329 [](pointIndex x) { return x.second; });
330 return nbhi;
331}
QStringView level(QStringView ifopt)
const QList< QKeySequence > & begin()
const QList< QKeySequence > & end()
QTextStream & left(QTextStream &stream)
QTextStream & right(QTextStream &stream)
This file is part of the KDE documentation.
Documentation copyright © 1996-2024 The KDE developers.
Generated on Sat Dec 21 2024 16:59:01 by doxygen 1.12.0 written by Dimitri van Heesch, © 1997-2006

KDE's Doxygen guidelines are available online.