60 #define NANOFLANN_VERSION 0x142
63 #if !defined(NOMINMAX) && \
64 (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
81 return static_cast<T
>(3.14159265358979323846);
88 template <
typename T,
typename =
int>
99 template <
typename T,
typename =
int>
104 template <
typename T>
113 template <
typename Container>
114 inline typename std::enable_if<has_resize<Container>::value,
void>::type
resize(
115 Container& c,
const size_t nElements)
124 template <
typename Container>
125 inline typename std::enable_if<!has_resize<Container>::value,
void>::type
126 resize(Container& c,
const size_t nElements)
128 if (nElements != c.size())
129 throw std::logic_error(
"Try to change the size of a std::array.");
135 template <
typename Container,
typename T>
136 inline typename std::enable_if<has_assign<Container>::value,
void>::type
assign(
137 Container& c,
const size_t nElements,
const T& value)
139 c.assign(nElements, value);
145 template <
typename Container,
typename T>
146 inline typename std::enable_if<!has_assign<Container>::value,
void>::type
147 assign(Container& c,
const size_t nElements,
const T& value)
149 for (
size_t i = 0; i < nElements; i++) c[i] = value;
155 typename _DistanceType,
typename _IndexType = size_t,
156 typename _CountType =
size_t>
160 using DistanceType = _DistanceType;
161 using IndexType = _IndexType;
162 using CountType = _CountType;
172 : indices(0), dists(0), capacity(capacity_), count(0)
176 inline void init(IndexType* indices_, DistanceType* dists_)
182 dists[capacity - 1] = (std::numeric_limits<DistanceType>::max)();
185 inline CountType size()
const {
return count; }
187 inline bool full()
const {
return count == capacity; }
194 inline bool addPoint(DistanceType dist, IndexType index)
197 for (i = count; i > 0; --i)
199 #ifdef NANOFLANN_FIRST_MATCH
202 if ((dists[i - 1] > dist) ||
203 ((dist == dists[i - 1]) && (indices[i - 1] > index)))
206 if (dists[i - 1] > dist)
211 dists[i] = dists[i - 1];
212 indices[i] = indices[i - 1];
223 if (count < capacity) count++;
229 inline DistanceType worstDist()
const {
return dists[capacity - 1]; }
236 template <
typename PairType>
237 inline bool operator()(
const PairType& p1,
const PairType& p2)
const
239 return p1.second < p2.second;
246 template <
typename _DistanceType,
typename _IndexType =
size_t>
250 using DistanceType = _DistanceType;
251 using IndexType = _IndexType;
254 const DistanceType radius;
256 std::vector<std::pair<IndexType, DistanceType>>& m_indices_dists;
259 DistanceType radius_,
260 std::vector<std::pair<IndexType, DistanceType>>& indices_dists)
261 : radius(radius_), m_indices_dists(indices_dists)
266 inline void init() { clear(); }
267 inline void clear() { m_indices_dists.clear(); }
269 inline size_t size()
const {
return m_indices_dists.size(); }
271 inline bool full()
const {
return true; }
278 inline bool addPoint(DistanceType dist, IndexType index)
281 m_indices_dists.push_back(std::make_pair(index, dist));
285 inline DistanceType worstDist()
const {
return radius; }
293 if (m_indices_dists.empty())
294 throw std::runtime_error(
295 "Cannot invoke RadiusResultSet::worst_item() on "
296 "an empty list of results.");
297 using DistIt =
typename std::vector<
298 std::pair<IndexType, DistanceType>>::const_iterator;
299 DistIt it = std::max_element(
309 template <
typename T>
310 void save_value(std::ostream& stream,
const T& value)
312 stream.write(
reinterpret_cast<const char*
>(&value),
sizeof(T));
315 template <
typename T>
316 void save_value(std::ostream& stream,
const std::vector<T>& value)
318 size_t size = value.size();
319 stream.write(
reinterpret_cast<const char*
>(&size),
sizeof(
size_t));
320 stream.write(
reinterpret_cast<const char*
>(value.data()),
sizeof(T) * size);
323 template <
typename T>
324 void load_value(std::istream& stream, T& value)
326 stream.read(
reinterpret_cast<char*
>(&value),
sizeof(T));
329 template <
typename T>
330 void load_value(std::istream& stream, std::vector<T>& value)
333 stream.read(
reinterpret_cast<char*
>(&size),
sizeof(
size_t));
335 stream.read(
reinterpret_cast<char*
>(value.data()),
sizeof(T) * size);
357 class T,
class DataSource,
typename _DistanceType = T,
358 typename AccessorType = uint32_t>
361 using ElementType = T;
362 using DistanceType = _DistanceType;
364 const DataSource& data_source;
366 L1_Adaptor(
const DataSource& _data_source) : data_source(_data_source) {}
368 inline DistanceType evalMetric(
369 const T* a,
const AccessorType b_idx,
size_t size,
370 DistanceType worst_dist = -1)
const
372 DistanceType result = DistanceType();
373 const T* last = a + size;
374 const T* lastgroup = last - 3;
378 while (a < lastgroup)
380 const DistanceType diff0 =
381 std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++));
382 const DistanceType diff1 =
383 std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++));
384 const DistanceType diff2 =
385 std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++));
386 const DistanceType diff3 =
387 std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++));
388 result += diff0 + diff1 + diff2 + diff3;
390 if ((worst_dist > 0) && (result > worst_dist)) {
return result; }
395 { result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); }
399 template <
typename U,
typename V>
400 inline DistanceType accum_dist(
const U a,
const V b,
const size_t)
const
402 return std::abs(a - b);
417 class T,
class DataSource,
typename _DistanceType = T,
418 typename AccessorType = uint32_t>
421 using ElementType = T;
422 using DistanceType = _DistanceType;
424 const DataSource& data_source;
426 L2_Adaptor(
const DataSource& _data_source) : data_source(_data_source) {}
428 inline DistanceType evalMetric(
429 const T* a,
const AccessorType b_idx,
size_t size,
430 DistanceType worst_dist = -1)
const
432 DistanceType result = DistanceType();
433 const T* last = a + size;
434 const T* lastgroup = last - 3;
438 while (a < lastgroup)
440 const DistanceType diff0 =
441 a[0] - data_source.kdtree_get_pt(b_idx, d++);
442 const DistanceType diff1 =
443 a[1] - data_source.kdtree_get_pt(b_idx, d++);
444 const DistanceType diff2 =
445 a[2] - data_source.kdtree_get_pt(b_idx, d++);
446 const DistanceType diff3 =
447 a[3] - data_source.kdtree_get_pt(b_idx, d++);
449 diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
451 if ((worst_dist > 0) && (result > worst_dist)) {
return result; }
457 const DistanceType diff0 =
458 *a++ - data_source.kdtree_get_pt(b_idx, d++);
459 result += diff0 * diff0;
464 template <
typename U,
typename V>
465 inline DistanceType accum_dist(
const U a,
const V b,
const size_t)
const
467 return (a - b) * (a - b);
482 class T,
class DataSource,
typename _DistanceType = T,
483 typename AccessorType = uint32_t>
486 using ElementType = T;
487 using DistanceType = _DistanceType;
489 const DataSource& data_source;
492 : data_source(_data_source)
496 inline DistanceType evalMetric(
497 const T* a,
const AccessorType b_idx,
size_t size)
const
499 DistanceType result = DistanceType();
500 for (
size_t i = 0; i < size; ++i)
502 const DistanceType diff =
503 a[i] - data_source.kdtree_get_pt(b_idx, i);
504 result += diff * diff;
509 template <
typename U,
typename V>
510 inline DistanceType accum_dist(
const U a,
const V b,
const size_t)
const
512 return (a - b) * (a - b);
527 class T,
class DataSource,
typename _DistanceType = T,
528 typename AccessorType = uint32_t>
531 using ElementType = T;
532 using DistanceType = _DistanceType;
534 const DataSource& data_source;
536 SO2_Adaptor(
const DataSource& _data_source) : data_source(_data_source) {}
538 inline DistanceType evalMetric(
539 const T* a,
const AccessorType b_idx,
size_t size)
const
542 a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), size - 1);
547 template <
typename U,
typename V>
548 inline DistanceType
accum_dist(
const U a,
const V b,
const size_t)
const
550 DistanceType result = DistanceType();
551 DistanceType PI = pi_const<DistanceType>();
555 else if (result < -PI)
572 class T,
class DataSource,
typename _DistanceType = T,
573 typename AccessorType = uint32_t>
576 using ElementType = T;
577 using DistanceType = _DistanceType;
583 : distance_L2_Simple(_data_source)
587 inline DistanceType evalMetric(
588 const T* a,
const AccessorType b_idx,
size_t size)
const
590 return distance_L2_Simple.evalMetric(a, b_idx, size);
593 template <
typename U,
typename V>
594 inline DistanceType accum_dist(
const U a,
const V b,
const size_t idx)
const
596 return distance_L2_Simple.accum_dist(a, b, idx);
603 template <
class T,
class DataSource,
typename AccessorType = u
int32_t>
612 template <
class T,
class DataSource,
typename AccessorType = u
int32_t>
621 template <
class T,
class DataSource,
typename AccessorType = u
int32_t>
630 template <
class T,
class DataSource,
typename AccessorType = u
int32_t>
639 template <
class T,
class DataSource,
typename AccessorType = u
int32_t>
655 : leaf_max_size(_leaf_max_size)
659 size_t leaf_max_size;
667 SearchParams(
int checks_IGNORED_ = 32,
float eps_ = 0,
bool sorted_ =
true)
668 : checks(checks_IGNORED_), eps(eps_), sorted(sorted_)
690 template <
typename T>
693 T* mem =
static_cast<T*
>(::malloc(
sizeof(T) * count));
713 const size_t BLOCKSIZE = 8192;
723 using Offset = uint32_t;
724 using Size = uint32_t;
725 using Dimension = int32_t;
756 while (base !=
nullptr)
759 *(
static_cast<void**
>(base));
781 if (size > remaining)
783 wastedMemory += remaining;
786 const Size blocksize =
787 (size +
sizeof(
void*) + (
WORDSIZE - 1) > BLOCKSIZE)
788 ? size +
sizeof(
void*) + (
WORDSIZE - 1)
792 void* m = ::malloc(blocksize);
795 fprintf(stderr,
"Failed to allocate memory.\n");
796 throw std::bad_alloc();
800 static_cast<void**
>(m)[0] = base;
807 remaining = blocksize -
sizeof(
void*) - shift;
808 loc = (
static_cast<char*
>(m) +
sizeof(
void*) + shift);
811 loc =
static_cast<char*
>(loc) + size;
826 template <
typename T>
829 T* mem =
static_cast<T*
>(this->malloc(
sizeof(T) * count));
841 template <
int32_t DIM,
typename T>
844 using container_t = std::array<T, DIM>;
847 template <
typename T>
850 using container_t = std::vector<T>;
868 class Derived,
typename Distance,
class DatasetAdaptor, int32_t DIM = -1,
869 typename AccessorType = uint32_t>
878 obj.root_node =
nullptr;
879 obj.m_size_at_index_build = 0;
882 using ElementType =
typename Distance::ElementType;
883 using DistanceType =
typename Distance::DistanceType;
888 std::vector<AccessorType>
vAcc;
890 using Offset =
typename decltype(vAcc)::size_type;
891 using Size =
typename decltype(vAcc)::size_type;
892 using Dimension = int32_t;
921 ElementType low, high;
926 Size m_leaf_max_size;
941 typename array_or_vector_selector<DIM, DistanceType>::container_t;
957 Size
size(
const Derived& obj)
const {
return obj.m_size; }
960 Size
veclen(
const Derived& obj) {
return DIM > 0 ? DIM : obj.dim; }
964 const Derived& obj, AccessorType element, Dimension component)
const
966 return obj.dataset.kdtree_get_pt(element, component);
975 return obj.pool.usedMemory + obj.pool.wastedMemory +
976 obj.dataset.kdtree_get_point_count() *
977 sizeof(AccessorType);
981 const Derived& obj, Offset ind, Size count, Dimension element,
982 ElementType& min_elem, ElementType& max_elem)
984 min_elem = dataset_get(obj, vAcc[ind], element);
986 for (Offset i = 1; i < count; ++i)
988 ElementType val = dataset_get(obj, vAcc[ind + i], element);
989 if (val < min_elem) min_elem = val;
990 if (val > max_elem) max_elem = val;
1002 Derived& obj,
const Offset left,
const Offset right,
BoundingBox& bbox)
1004 NodePtr node = obj.pool.template allocate<Node>();
1007 if ((right - left) <=
static_cast<Offset
>(obj.m_leaf_max_size))
1009 node->
child1 = node->child2 =
nullptr;
1014 for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1016 bbox[i].low = dataset_get(obj, obj.vAcc[left], i);
1017 bbox[i].high = dataset_get(obj, obj.vAcc[left], i);
1019 for (Offset k = left + 1; k < right; ++k)
1021 for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1023 if (bbox[i].low > dataset_get(obj, obj.vAcc[k], i))
1024 bbox[i].low = dataset_get(obj, obj.vAcc[k], i);
1025 if (bbox[i].high < dataset_get(obj, obj.vAcc[k], i))
1026 bbox[i].high = dataset_get(obj, obj.vAcc[k], i);
1034 DistanceType cutval;
1035 middleSplit_(obj, left, right - left, idx, cutfeat, cutval, bbox);
1040 left_bbox[cutfeat].high = cutval;
1041 node->
child1 = divideTree(obj, left, left + idx, left_bbox);
1044 right_bbox[cutfeat].low = cutval;
1045 node->child2 = divideTree(obj, left + idx, right, right_bbox);
1047 node->
node_type.sub.divlow = left_bbox[cutfeat].high;
1048 node->
node_type.sub.divhigh = right_bbox[cutfeat].low;
1050 for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1052 bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
1053 bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
1061 Derived& obj, Offset ind, Size count, Offset& index, Dimension& cutfeat,
1062 DistanceType& cutval,
const BoundingBox& bbox)
1064 const auto EPS =
static_cast<DistanceType
>(0.00001);
1065 ElementType max_span = bbox[0].high - bbox[0].low;
1066 for (Dimension i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i)
1068 ElementType span = bbox[i].high - bbox[i].low;
1069 if (span > max_span) { max_span = span; }
1071 ElementType max_spread = -1;
1073 for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1075 ElementType span = bbox[i].high - bbox[i].low;
1076 if (span > (1 - EPS) * max_span)
1078 ElementType min_elem, max_elem;
1079 computeMinMax(obj, ind, count, i, min_elem, max_elem);
1080 ElementType spread = max_elem - min_elem;
1081 if (spread > max_spread)
1084 max_spread = spread;
1089 DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2;
1090 ElementType min_elem, max_elem;
1091 computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem);
1093 if (split_val < min_elem)
1095 else if (split_val > max_elem)
1101 planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2);
1103 if (lim1 > count / 2)
1105 else if (lim2 < count / 2)
1121 Derived& obj, Offset ind,
const Size count, Dimension cutfeat,
1122 DistanceType& cutval, Offset& lim1, Offset& lim2)
1126 Offset right = count - 1;
1129 while (left <= right &&
1130 dataset_get(obj, vAcc[ind + left], cutfeat) < cutval)
1132 while (right && left <= right &&
1133 dataset_get(obj, vAcc[ind + right], cutfeat) >= cutval)
1135 if (left > right || !right)
1137 std::swap(vAcc[ind + left], vAcc[ind + right]);
1148 while (left <= right &&
1149 dataset_get(obj, vAcc[ind + left], cutfeat) <= cutval)
1151 while (right && left <= right &&
1152 dataset_get(obj, vAcc[ind + right], cutfeat) > cutval)
1154 if (left > right || !right)
1156 std::swap(vAcc[ind + left], vAcc[ind + right]);
1163 DistanceType computeInitialDistances(
1164 const Derived& obj,
const ElementType* vec,
1165 distance_vector_t& dists)
const
1168 DistanceType distsq = DistanceType();
1170 for (Dimension i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i)
1172 if (vec[i] < obj.root_bbox[i].low)
1175 obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i);
1178 if (vec[i] > obj.root_bbox[i].high)
1181 obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i);
1188 void save_tree(Derived& obj, std::ostream& stream, NodePtr tree)
1190 save_value(stream, *tree);
1191 if (tree->child1 !=
nullptr) { save_tree(obj, stream, tree->child1); }
1192 if (tree->child2 !=
nullptr) { save_tree(obj, stream, tree->child2); }
1195 void load_tree(Derived& obj, std::istream& stream, NodePtr& tree)
1197 tree = obj.pool.template allocate<Node>();
1198 load_value(stream, *tree);
1199 if (tree->child1 !=
nullptr) { load_tree(obj, stream, tree->child1); }
1200 if (tree->child2 !=
nullptr) { load_tree(obj, stream, tree->child2); }
1210 save_value(stream, obj.m_size);
1211 save_value(stream, obj.dim);
1212 save_value(stream, obj.root_bbox);
1213 save_value(stream, obj.m_leaf_max_size);
1214 save_value(stream, obj.vAcc);
1215 save_tree(obj, stream, obj.root_node);
1225 load_value(stream, obj.m_size);
1226 load_value(stream, obj.dim);
1227 load_value(stream, obj.root_bbox);
1228 load_value(stream, obj.m_leaf_max_size);
1229 load_value(stream, obj.vAcc);
1230 load_tree(obj, stream, obj.root_node);
1275 typename Distance,
class DatasetAdaptor, int32_t DIM = -1,
1276 typename AccessorType = uint32_t>
1279 KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, AccessorType>,
1280 Distance, DatasetAdaptor, DIM, AccessorType>
1285 Distance, DatasetAdaptor, DIM, AccessorType>&) =
1299 Distance, DatasetAdaptor, DIM, AccessorType>,
1300 Distance, DatasetAdaptor, DIM, AccessorType>;
1302 using Offset =
typename BaseClassRef::Offset;
1303 using Size =
typename BaseClassRef::Size;
1304 using Dimension =
typename BaseClassRef::Dimension;
1306 using ElementType =
typename BaseClassRef::ElementType;
1307 using DistanceType =
typename BaseClassRef::DistanceType;
1309 using Node =
typename BaseClassRef::Node;
1310 using NodePtr = Node*;
1312 using Interval =
typename BaseClassRef::Interval;
1341 template <
class... Args>
1343 const Dimension dimensionality,
const DatasetAdaptor& inputData,
1345 : dataset(inputData),
1346 index_params(params),
1347 distance(inputData, std::forward<Args>(args)...)
1349 BaseClassRef::root_node =
nullptr;
1350 BaseClassRef::m_size = dataset.kdtree_get_point_count();
1351 BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1352 BaseClassRef::dim = dimensionality;
1353 if (DIM > 0) BaseClassRef::dim = DIM;
1354 BaseClassRef::m_leaf_max_size = params.leaf_max_size;
1364 BaseClassRef::m_size = dataset.kdtree_get_point_count();
1365 BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1367 this->freeIndex(*
this);
1368 BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1369 if (BaseClassRef::m_size == 0)
return;
1370 computeBoundingBox(BaseClassRef::root_bbox);
1371 BaseClassRef::root_node = this->divideTree(
1372 *
this, 0, BaseClassRef::m_size,
1373 BaseClassRef::root_bbox);
1392 template <
typename RESULTSET>
1394 RESULTSET& result,
const ElementType* vec,
1398 if (this->size(*
this) == 0)
return false;
1399 if (!BaseClassRef::root_node)
1400 throw std::runtime_error(
1401 "[nanoflann] findNeighbors() called before building the "
1403 float epsError = 1 + searchParams.
eps;
1407 auto zero =
static_cast<decltype(result.worstDist())
>(0);
1409 dists, (DIM > 0 ? DIM : BaseClassRef::dim),
1411 DistanceType distsq = this->computeInitialDistances(*
this, vec, dists);
1413 result, vec, BaseClassRef::root_node, distsq, dists,
1416 return result.full();
1429 const ElementType* query_point,
const Size num_closest,
1430 AccessorType* out_indices, DistanceType* out_distances_sq,
1431 const int = 10)
const
1435 resultSet.init(out_indices, out_distances_sq);
1437 return resultSet.size();
1457 const ElementType* query_point,
const DistanceType& radius,
1458 std::vector<std::pair<AccessorType, DistanceType>>& IndicesDists,
1462 radius, IndicesDists);
1464 radiusSearchCustomCallback(query_point, resultSet, searchParams);
1476 template <
class SEARCH_CALLBACK>
1478 const ElementType* query_point, SEARCH_CALLBACK& resultSet,
1481 this->findNeighbors(resultSet, query_point, searchParams);
1482 return resultSet.size();
1493 BaseClassRef::m_size = dataset.kdtree_get_point_count();
1494 if (BaseClassRef::vAcc.size() != BaseClassRef::m_size)
1495 BaseClassRef::vAcc.resize(BaseClassRef::m_size);
1496 for (Size i = 0; i < BaseClassRef::m_size; i++)
1497 BaseClassRef::vAcc[i] = i;
1500 void computeBoundingBox(BoundingBox& bbox)
1502 resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
1503 if (dataset.kdtree_get_bbox(bbox))
1509 const Size N = dataset.kdtree_get_point_count();
1511 throw std::runtime_error(
1512 "[nanoflann] computeBoundingBox() called but "
1513 "no data points found.");
1514 for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i)
1516 bbox[i].low = bbox[i].high =
1517 this->dataset_get(*
this, BaseClassRef::vAcc[0], i);
1519 for (Offset k = 1; k < N; ++k)
1521 for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim);
1524 if (this->dataset_get(*
this, BaseClassRef::vAcc[k], i) <
1527 this->dataset_get(*
this, BaseClassRef::vAcc[k], i);
1528 if (this->dataset_get(*
this, BaseClassRef::vAcc[k], i) >
1531 this->dataset_get(*
this, BaseClassRef::vAcc[k], i);
1543 template <
class RESULTSET>
1545 RESULTSET& result_set,
const ElementType* vec,
const NodePtr node,
1547 const float epsError)
const
1550 if ((node->child1 ==
nullptr) && (node->child2 ==
nullptr))
1554 DistanceType worst_dist = result_set.worstDist();
1555 for (Offset i = node->node_type.lr.left;
1556 i < node->node_type.lr.right; ++i)
1558 const AccessorType accessor =
1559 BaseClassRef::vAcc[i];
1560 DistanceType dist = distance.evalMetric(
1561 vec, accessor, (DIM > 0 ? DIM : BaseClassRef::dim));
1562 if (dist < worst_dist)
1564 if (!result_set.addPoint(dist, BaseClassRef::vAcc[i]))
1576 Dimension idx = node->node_type.sub.divfeat;
1577 ElementType val = vec[idx];
1578 DistanceType diff1 = val - node->node_type.sub.divlow;
1579 DistanceType diff2 = val - node->node_type.sub.divhigh;
1583 DistanceType cut_dist;
1584 if ((diff1 + diff2) < 0)
1586 bestChild = node->child1;
1587 otherChild = node->child2;
1589 distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1593 bestChild = node->child2;
1594 otherChild = node->child1;
1596 distance.accum_dist(val, node->node_type.sub.divlow, idx);
1601 result_set, vec, bestChild, mindistsq, dists, epsError))
1608 DistanceType dst = dists[idx];
1609 mindistsq = mindistsq + cut_dist - dst;
1610 dists[idx] = cut_dist;
1611 if (mindistsq * epsError <= result_set.worstDist())
1614 result_set, vec, otherChild, mindistsq, dists, epsError))
1631 void saveIndex(std::ostream& stream) { this->saveIndex_(*
this, stream); }
1638 void loadIndex(std::istream& stream) { this->loadIndex_(*
this, stream); }
1679 typename Distance,
class DatasetAdaptor, int32_t DIM = -1,
1680 typename AccessorType = uint32_t>
1683 KDTreeSingleIndexDynamicAdaptor_<
1684 Distance, DatasetAdaptor, DIM, AccessorType>,
1685 Distance, DatasetAdaptor, DIM, AccessorType>
1695 std::vector<int>& treeIndex;
1701 Distance, DatasetAdaptor, DIM, AccessorType>,
1702 Distance, DatasetAdaptor, DIM, AccessorType>;
1704 using ElementType =
typename BaseClassRef::ElementType;
1705 using DistanceType =
typename BaseClassRef::DistanceType;
1707 using Offset =
typename BaseClassRef::Offset;
1708 using Size =
typename BaseClassRef::Size;
1709 using Dimension =
typename BaseClassRef::Dimension;
1711 using Node =
typename BaseClassRef::Node;
1712 using NodePtr = Node*;
1714 using Interval =
typename BaseClassRef::Interval;
1738 const Dimension dimensionality,
const DatasetAdaptor& inputData,
1739 std::vector<int>& treeIndex_,
1742 : dataset(inputData),
1743 index_params(params),
1744 treeIndex(treeIndex_),
1747 BaseClassRef::root_node =
nullptr;
1748 BaseClassRef::m_size = 0;
1749 BaseClassRef::m_size_at_index_build = 0;
1750 BaseClassRef::dim = dimensionality;
1751 if (DIM > 0) BaseClassRef::dim = DIM;
1752 BaseClassRef::m_leaf_max_size = params.leaf_max_size;
1764 std::swap(BaseClassRef::vAcc, tmp.BaseClassRef::vAcc);
1766 BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size);
1767 std::swap(index_params, tmp.index_params);
1768 std::swap(treeIndex, tmp.treeIndex);
1769 std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size);
1771 BaseClassRef::m_size_at_index_build,
1772 tmp.BaseClassRef::m_size_at_index_build);
1773 std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node);
1774 std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox);
1775 std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool);
1784 BaseClassRef::m_size = BaseClassRef::vAcc.size();
1785 this->freeIndex(*
this);
1786 BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1787 if (BaseClassRef::m_size == 0)
return;
1788 computeBoundingBox(BaseClassRef::root_bbox);
1789 BaseClassRef::root_node = this->divideTree(
1790 *
this, 0, BaseClassRef::m_size,
1791 BaseClassRef::root_bbox);
1810 template <
typename RESULTSET>
1812 RESULTSET& result,
const ElementType* vec,
1816 if (this->size(*
this) == 0)
return false;
1817 if (!BaseClassRef::root_node)
return false;
1818 float epsError = 1 + searchParams.
eps;
1824 dists, (DIM > 0 ? DIM : BaseClassRef::dim),
1825 static_cast<typename distance_vector_t::value_type
>(0));
1826 DistanceType distsq = this->computeInitialDistances(*
this, vec, dists);
1828 result, vec, BaseClassRef::root_node, distsq, dists,
1831 return result.full();
1844 const ElementType* query_point,
const Size num_closest,
1845 AccessorType* out_indices, DistanceType* out_distances_sq,
1846 const int = 10)
const
1850 resultSet.init(out_indices, out_distances_sq);
1852 return resultSet.size();
1872 const ElementType* query_point,
const DistanceType& radius,
1873 std::vector<std::pair<AccessorType, DistanceType>>& IndicesDists,
1877 radius, IndicesDists);
1878 const size_t nFound =
1879 radiusSearchCustomCallback(query_point, resultSet, searchParams);
1891 template <
class SEARCH_CALLBACK>
1893 const ElementType* query_point, SEARCH_CALLBACK& resultSet,
1896 this->findNeighbors(resultSet, query_point, searchParams);
1897 return resultSet.size();
1903 void computeBoundingBox(BoundingBox& bbox)
1905 resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
1907 if (dataset.kdtree_get_bbox(bbox))
1913 const Size N = BaseClassRef::m_size;
1915 throw std::runtime_error(
1916 "[nanoflann] computeBoundingBox() called but "
1917 "no data points found.");
1918 for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i)
1920 bbox[i].low = bbox[i].high =
1921 this->dataset_get(*
this, BaseClassRef::vAcc[0], i);
1923 for (Offset k = 1; k < N; ++k)
1925 for (Dimension i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim);
1928 if (this->dataset_get(*
this, BaseClassRef::vAcc[k], i) <
1931 this->dataset_get(*
this, BaseClassRef::vAcc[k], i);
1932 if (this->dataset_get(*
this, BaseClassRef::vAcc[k], i) >
1935 this->dataset_get(*
this, BaseClassRef::vAcc[k], i);
1945 template <
class RESULTSET>
1947 RESULTSET& result_set,
const ElementType* vec,
const NodePtr node,
1949 const float epsError)
const
1952 if ((node->child1 ==
nullptr) && (node->child2 ==
nullptr))
1956 DistanceType worst_dist = result_set.worstDist();
1957 for (Offset i = node->node_type.lr.left;
1958 i < node->node_type.lr.right; ++i)
1960 const AccessorType index =
1961 BaseClassRef::vAcc[i];
1962 if (treeIndex[index] == -1)
continue;
1963 DistanceType dist = distance.evalMetric(
1964 vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
1965 if (dist < worst_dist)
1967 if (!result_set.addPoint(
1968 static_cast<typename RESULTSET::DistanceType
>(dist),
1969 static_cast<typename RESULTSET::IndexType
>(
1970 BaseClassRef::vAcc[i])))
1982 Dimension idx = node->node_type.sub.divfeat;
1983 ElementType val = vec[idx];
1984 DistanceType diff1 = val - node->node_type.sub.divlow;
1985 DistanceType diff2 = val - node->node_type.sub.divhigh;
1989 DistanceType cut_dist;
1990 if ((diff1 + diff2) < 0)
1992 bestChild = node->child1;
1993 otherChild = node->child2;
1995 distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1999 bestChild = node->child2;
2000 otherChild = node->child1;
2002 distance.accum_dist(val, node->node_type.sub.divlow, idx);
2006 searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
2008 DistanceType dst = dists[idx];
2009 mindistsq = mindistsq + cut_dist - dst;
2010 dists[idx] = cut_dist;
2011 if (mindistsq * epsError <= result_set.worstDist())
2014 result_set, vec, otherChild, mindistsq, dists, epsError);
2025 void saveIndex(std::ostream& stream) { this->saveIndex_(*
this, stream); }
2032 void loadIndex(std::istream& stream) { this->loadIndex_(*
this, stream); }
2050 typename Distance,
class DatasetAdaptor, int32_t DIM = -1,
2051 typename AccessorType = uint32_t>
2055 using ElementType =
typename Distance::ElementType;
2056 using DistanceType =
typename Distance::DistanceType;
2059 Distance, DatasetAdaptor, DIM>::Offset;
2061 Distance, DatasetAdaptor, DIM>::Size;
2063 Distance, DatasetAdaptor, DIM>::Dimension;
2066 Size m_leaf_max_size;
2086 std::vector<index_container_t> index;
2098 int First0Bit(AccessorType num)
2112 using my_kd_tree_t =
2113 KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM>;
2114 std::vector<my_kd_tree_t> index_(
2116 my_kd_tree_t(dim , dataset, treeIndex, index_params));
2138 const int dimensionality,
const DatasetAdaptor& inputData,
2141 const size_t maximumPointCount = 1000000000U)
2142 : dataset(inputData), index_params(params), distance(inputData)
2144 treeCount =
static_cast<size_t>(std::log2(maximumPointCount));
2146 dim = dimensionality;
2148 if (DIM > 0) dim = DIM;
2149 m_leaf_max_size = params.leaf_max_size;
2151 const size_t num_initial_points = dataset.kdtree_get_point_count();
2152 if (num_initial_points > 0) { addPoints(0, num_initial_points - 1); }
2158 Distance, DatasetAdaptor, DIM, AccessorType>&) =
delete;
2163 Size count = end - start + 1;
2164 treeIndex.resize(treeIndex.size() + count);
2165 for (AccessorType idx = start; idx <= end; idx++)
2167 int pos = First0Bit(pointCount);
2168 index[pos].vAcc.clear();
2169 treeIndex[pointCount] = pos;
2170 for (
int i = 0; i < pos; i++)
2172 for (
int j = 0; j < static_cast<int>(index[i].vAcc.size()); j++)
2174 index[pos].vAcc.push_back(index[i].vAcc[j]);
2175 if (treeIndex[index[i].vAcc[j]] != -1)
2176 treeIndex[index[i].vAcc[j]] = pos;
2178 index[i].vAcc.clear();
2179 index[i].freeIndex(index[i]);
2181 index[pos].vAcc.push_back(idx);
2182 index[pos].buildIndex();
2190 if (idx >= pointCount)
return;
2191 treeIndex[idx] = -1;
2207 template <
typename RESULTSET>
2209 RESULTSET& result,
const ElementType* vec,
2212 for (
size_t i = 0; i < treeCount; i++)
2213 { index[i].findNeighbors(result, &vec[0], searchParams); }
2214 return result.full();
2245 bool row_major =
true>
2250 using num_t =
typename MatrixType::Scalar;
2251 using IndexType =
typename MatrixType::Index;
2252 using metric_t =
typename Distance::template traits<
2253 num_t,
self_t, IndexType>::distance_t;
2257 row_major ? MatrixType::ColsAtCompileTime
2258 : MatrixType::RowsAtCompileTime,
2265 using Size =
typename index_t::Size;
2266 using Dimension =
typename index_t::Dimension;
2270 const Dimension dimensionality,
2271 const std::reference_wrapper<const MatrixType>& mat,
2272 const int leaf_max_size = 10)
2273 : m_data_matrix(mat)
2275 const auto dims = row_major ? mat.get().cols() : mat.get().rows();
2276 if (
static_cast<Dimension
>(dims) != dimensionality)
2277 throw std::runtime_error(
2278 "Error: 'dimensionality' must match column count in data "
2280 if (DIM > 0 &&
static_cast<int32_t
>(dims) != DIM)
2281 throw std::runtime_error(
2282 "Data set dimensionality does not match the 'DIM' template "
2296 const std::reference_wrapper<const MatrixType> m_data_matrix;
2305 const num_t* query_point,
const Size num_closest,
2306 IndexType* out_indices, num_t* out_distances_sq,
2307 const int = 10)
const
2310 resultSet.init(out_indices, out_distances_sq);
2317 const self_t& derived()
const {
return *
this; }
2318 self_t& derived() {
return *
this; }
2321 inline Size kdtree_get_point_count()
const
2324 return m_data_matrix.get().rows();
2326 return m_data_matrix.get().cols();
2330 inline num_t kdtree_get_pt(
const IndexType idx,
size_t dim)
const
2333 return m_data_matrix.get().coeff(idx, IndexType(dim));
2335 return m_data_matrix.get().coeff(IndexType(dim), idx);
2343 template <
class BBOX>
2344 bool kdtree_get_bbox(BBOX& )
const
Definition: nanoflann.hpp:871
Size veclen(const Derived &obj)
Definition: nanoflann.hpp:960
NodePtr divideTree(Derived &obj, const Offset left, const Offset right, BoundingBox &bbox)
Definition: nanoflann.hpp:1001
PooledAllocator pool
Definition: nanoflann.hpp:954
Dimension dim
Dimensionality of each data point.
Definition: nanoflann.hpp:931
Size m_size_at_index_build
Definition: nanoflann.hpp:929
ElementType dataset_get(const Derived &obj, AccessorType element, Dimension component) const
Helper accessor to the dataset points:
Definition: nanoflann.hpp:963
Size size(const Derived &obj) const
Definition: nanoflann.hpp:957
BoundingBox root_bbox
Definition: nanoflann.hpp:945
Size usedMemory(Derived &obj)
Definition: nanoflann.hpp:973
void saveIndex_(Derived &obj, std::ostream &stream)
Definition: nanoflann.hpp:1208
std::vector< AccessorType > vAcc
Definition: nanoflann.hpp:888
void planeSplit(Derived &obj, Offset ind, const Size count, Dimension cutfeat, DistanceType &cutval, Offset &lim1, Offset &lim2)
Definition: nanoflann.hpp:1120
void freeIndex(Derived &obj)
Definition: nanoflann.hpp:875
void loadIndex_(Derived &obj, std::istream &stream)
Definition: nanoflann.hpp:1223
typename array_or_vector_selector< DIM, Interval >::container_t BoundingBox
Definition: nanoflann.hpp:936
Size m_size
Number of current points in the dataset.
Definition: nanoflann.hpp:928
typename array_or_vector_selector< DIM, DistanceType >::container_t distance_vector_t
Definition: nanoflann.hpp:941
Definition: nanoflann.hpp:1281
void saveIndex(std::ostream &stream)
Definition: nanoflann.hpp:1631
Size radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector< std::pair< AccessorType, DistanceType >> &IndicesDists, const SearchParams &searchParams) const
Definition: nanoflann.hpp:1456
void buildIndex()
Definition: nanoflann.hpp:1362
bool searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const
Definition: nanoflann.hpp:1544
KDTreeSingleIndexAdaptor(const KDTreeSingleIndexAdaptor< Distance, DatasetAdaptor, DIM, AccessorType > &)=delete
Size radiusSearchCustomCallback(const ElementType *query_point, SEARCH_CALLBACK &resultSet, const SearchParams &searchParams=SearchParams()) const
Definition: nanoflann.hpp:1477
KDTreeSingleIndexAdaptor(const Dimension dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams ¶ms={}, Args &&... args)
Definition: nanoflann.hpp:1342
bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Definition: nanoflann.hpp:1393
typename BaseClassRef::distance_vector_t distance_vector_t
Definition: nanoflann.hpp:1320
Size knnSearch(const ElementType *query_point, const Size num_closest, AccessorType *out_indices, DistanceType *out_distances_sq, const int=10) const
Definition: nanoflann.hpp:1428
void init_vind()
Definition: nanoflann.hpp:1490
void loadIndex(std::istream &stream)
Definition: nanoflann.hpp:1638
const DatasetAdaptor & dataset
The source of our data.
Definition: nanoflann.hpp:1291
typename BaseClassRef::BoundingBox BoundingBox
Definition: nanoflann.hpp:1316
Definition: nanoflann.hpp:1686
const DatasetAdaptor & dataset
The source of our data.
Definition: nanoflann.hpp:1691
void searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const
Definition: nanoflann.hpp:1946
Size radiusSearchCustomCallback(const ElementType *query_point, SEARCH_CALLBACK &resultSet, const SearchParams &searchParams=SearchParams()) const
Definition: nanoflann.hpp:1892
typename BaseClassRef::distance_vector_t distance_vector_t
Definition: nanoflann.hpp:1721
Size radiusSearch(const ElementType *query_point, const DistanceType &radius, std::vector< std::pair< AccessorType, DistanceType >> &IndicesDists, const SearchParams &searchParams) const
Definition: nanoflann.hpp:1871
void buildIndex()
Definition: nanoflann.hpp:1782
void loadIndex(std::istream &stream)
Definition: nanoflann.hpp:2032
KDTreeSingleIndexDynamicAdaptor_(const KDTreeSingleIndexDynamicAdaptor_ &rhs)=default
typename BaseClassRef::BoundingBox BoundingBox
Definition: nanoflann.hpp:1717
KDTreeSingleIndexDynamicAdaptor_ operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs)
Definition: nanoflann.hpp:1760
bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Definition: nanoflann.hpp:1811
KDTreeSingleIndexDynamicAdaptor_(const Dimension dimensionality, const DatasetAdaptor &inputData, std::vector< int > &treeIndex_, const KDTreeSingleIndexAdaptorParams ¶ms=KDTreeSingleIndexAdaptorParams())
Definition: nanoflann.hpp:1737
Size knnSearch(const ElementType *query_point, const Size num_closest, AccessorType *out_indices, DistanceType *out_distances_sq, const int=10) const
Definition: nanoflann.hpp:1843
void saveIndex(std::ostream &stream)
Definition: nanoflann.hpp:2025
Definition: nanoflann.hpp:2053
void removePoint(size_t idx)
Definition: nanoflann.hpp:2188
const DatasetAdaptor & dataset
The source of our data.
Definition: nanoflann.hpp:2073
std::vector< int > treeIndex
Definition: nanoflann.hpp:2076
void addPoints(AccessorType start, AccessorType end)
Definition: nanoflann.hpp:2161
Dimension dim
Dimensionality of each data point.
Definition: nanoflann.hpp:2082
KDTreeSingleIndexDynamicAdaptor(const KDTreeSingleIndexDynamicAdaptor< Distance, DatasetAdaptor, DIM, AccessorType > &)=delete
const std::vector< index_container_t > & getAllIndices() const
Definition: nanoflann.hpp:2091
KDTreeSingleIndexDynamicAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams ¶ms=KDTreeSingleIndexAdaptorParams(), const size_t maximumPointCount=1000000000U)
Definition: nanoflann.hpp:2137
bool findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Definition: nanoflann.hpp:2208
Definition: nanoflann.hpp:158
bool addPoint(DistanceType dist, IndexType index)
Definition: nanoflann.hpp:194
Definition: nanoflann.hpp:716
T * allocate(const size_t count=1)
Definition: nanoflann.hpp:827
~PooledAllocator()
Definition: nanoflann.hpp:751
void free_all()
Definition: nanoflann.hpp:754
PooledAllocator()
Definition: nanoflann.hpp:746
void * malloc(const size_t req_size)
Definition: nanoflann.hpp:770
Definition: nanoflann.hpp:248
std::pair< IndexType, DistanceType > worst_item() const
Definition: nanoflann.hpp:291
bool addPoint(DistanceType dist, IndexType index)
Definition: nanoflann.hpp:278
const size_t WORDSIZE
Definition: nanoflann.hpp:712
T * allocate(size_t count=1)
Definition: nanoflann.hpp:691
T pi_const()
Definition: nanoflann.hpp:79
std::enable_if< has_assign< Container >::value, void >::type assign(Container &c, const size_t nElements, const T &value)
Definition: nanoflann.hpp:136
std::enable_if< has_resize< Container >::value, void >::type resize(Container &c, const size_t nElements)
Definition: nanoflann.hpp:114
Definition: nanoflann.hpp:234
bool operator()(const PairType &p1, const PairType &p2) const
Definition: nanoflann.hpp:237
Definition: nanoflann.hpp:920
Definition: nanoflann.hpp:897
DistanceType divhigh
The values used for subdivision.
Definition: nanoflann.hpp:910
Dimension divfeat
Dimension used for subdivision.
Definition: nanoflann.hpp:908
Offset right
Indices of points in leaf node.
Definition: nanoflann.hpp:904
union nanoflann::KDTreeBaseClass::Node::@0 node_type
Node * child1
Definition: nanoflann.hpp:914
Definition: nanoflann.hpp:2247
void query(const num_t *query_point, const Size num_closest, IndexType *out_indices, num_t *out_distances_sq, const int=10) const
Definition: nanoflann.hpp:2304
KDTreeEigenMatrixAdaptor(const Dimension dimensionality, const std::reference_wrapper< const MatrixType > &mat, const int leaf_max_size=10)
Constructor: takes a const ref to the matrix object with the data points.
Definition: nanoflann.hpp:2269
KDTreeEigenMatrixAdaptor(const self_t &)=delete
typename index_t::Offset Offset
Definition: nanoflann.hpp:2264
Definition: nanoflann.hpp:653
Definition: nanoflann.hpp:360
Definition: nanoflann.hpp:420
Definition: nanoflann.hpp:485
Definition: nanoflann.hpp:343
Definition: nanoflann.hpp:530
DistanceType accum_dist(const U a, const V b, const size_t) const
Definition: nanoflann.hpp:548
Definition: nanoflann.hpp:575
Definition: nanoflann.hpp:664
bool sorted
distance (default: true)
Definition: nanoflann.hpp:675
SearchParams(int checks_IGNORED_=32, float eps_=0, bool sorted_=true)
Definition: nanoflann.hpp:667
float eps
search for eps-approximate neighbours (default: 0)
Definition: nanoflann.hpp:674
int checks
Definition: nanoflann.hpp:672
Definition: nanoflann.hpp:843
Definition: nanoflann.hpp:101
Definition: nanoflann.hpp:90
Definition: nanoflann.hpp:605
Definition: nanoflann.hpp:602
Definition: nanoflann.hpp:614
Definition: nanoflann.hpp:623
Definition: nanoflann.hpp:620
Definition: nanoflann.hpp:611
Definition: nanoflann.hpp:632
Definition: nanoflann.hpp:629
Definition: nanoflann.hpp:641
Definition: nanoflann.hpp:638