46 #ifndef NANOFLANN_HPP_
47 #define NANOFLANN_HPP_
59 #if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
73 #define NANOFLANN_VERSION 0x123
77 template <
typename DistanceType,
typename IndexType =
size_t,
typename CountType =
size_t>
94 inline void init(IndexType* indices_, DistanceType* dists_)
113 inline void addPoint(DistanceType dist, IndexType index)
116 for (i =
count; i > 0; --i)
118 #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same distance, the one with the lowest-index will be returned first.
119 if ((
dists[i - 1] > dist) || ((dist ==
dists[i - 1]) && (
indices[i - 1] > index)))
122 if (
dists[i - 1] > dist)
151 template <
typename DistanceType,
typename IndexType =
size_t>
159 inline RadiusResultSet(DistanceType radius_, std::vector<std::pair<IndexType, DistanceType> >& indices_dists)
161 , m_indices_dists(indices_dists)
169 inline void clear() { m_indices_dists.clear(); }
171 inline size_t size()
const {
return m_indices_dists.size(); }
173 inline bool full()
const {
return true; }
175 inline void addPoint(DistanceType dist, IndexType index)
178 m_indices_dists.push_back(std::make_pair(index, dist));
184 inline void set_radius_and_clear(
const DistanceType
r)
194 std::pair<IndexType, DistanceType> worst_item()
const
196 if (m_indices_dists.empty())
throw std::runtime_error(
"Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
197 typedef typename std::vector<std::pair<IndexType, DistanceType> >::const_iterator DistIt;
198 DistIt
it = std::max_element(m_indices_dists.begin(), m_indices_dists.end());
207 template <
typename PairType>
208 inline bool operator()(
const PairType& p1,
const PairType& p2)
const
210 return p1.second < p2.second;
218 template <
typename T>
221 fwrite(&value,
sizeof(value),
count, stream);
224 template <
typename T>
227 size_t size = value.size();
228 fwrite(&size,
sizeof(
size_t), 1, stream);
229 fwrite(&value[0],
sizeof(
T), size, stream);
232 template <
typename T>
235 size_t read_cnt = fread(&value,
sizeof(value),
count, stream);
236 if (read_cnt !=
count)
238 throw std::runtime_error(
"Cannot read from file");
242 template <
typename T>
246 size_t read_cnt = fread(&size,
sizeof(
size_t), 1, stream);
249 throw std::runtime_error(
"Cannot read from file");
252 read_cnt = fread(&value[0],
sizeof(
T), size, stream);
253 if (read_cnt != size)
255 throw std::runtime_error(
"Cannot read from file");
268 template <
class T,
class DataSource,
typename _DistanceType = T>
277 : data_source(_data_source)
284 const T* last = a +
size;
285 const T* lastgroup = last - 3;
289 while (a < lastgroup)
295 result += diff0 + diff1 + diff2 + diff3;
297 if ((worst_dist > 0) && (result > worst_dist))
305 result +=
std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++));
310 template <
typename U,
typename V>
322 template <
class T,
class DataSource,
typename _DistanceType = T>
331 : data_source(_data_source)
338 const T* last = a +
size;
339 const T* lastgroup = last - 3;
343 while (a < lastgroup)
345 const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++);
346 const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++);
347 const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++);
348 const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++);
349 result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
351 if ((worst_dist > 0) && (result > worst_dist))
359 const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++);
360 result += diff0 * diff0;
365 template <
typename U,
typename V>
368 return (a - b) * (a -
b);
377 template <
class T,
class DataSource,
typename _DistanceType = T>
386 : data_source(_data_source)
392 return data_source.kdtree_distance(a, b_idx, size);
395 template <
typename U,
typename V>
398 return (a - b) * (a -
b);
405 template <
class T,
class DataSource>
414 template <
class T,
class DataSource>
423 template <
class T,
class DataSource>
439 : leaf_max_size(_leaf_max_size)
450 SearchParams(
int checks_IGNORED_ = 32,
float eps_ = 0,
bool sorted_ =
true)
451 : checks(checks_IGNORED_)
473 template <
typename T>
476 T* mem =
static_cast<T*
>(::malloc(
sizeof(
T) *
count));
543 void* prev = *(
static_cast<void**
>(
base));
554 void* malloc(
const size_t req_size)
565 if (size > remaining)
567 wastedMemory += remaining;
573 void*
m = ::malloc(blocksize);
576 fprintf(stderr,
"Failed to allocate memory.\n");
581 static_cast<void**
>(
m)[0] =
base;
587 remaining = blocksize -
sizeof(
void*) - shift;
588 loc = (
static_cast<char*
>(
m) +
sizeof(
void*) + shift);
591 loc =
static_cast<char*
>(loc) + size;
606 template <
typename T>
609 T* mem =
static_cast<T*
>(this->malloc(
sizeof(
T) *
count));
644 template <
typename T, std::
size_t N>
667 #if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS)
670 #elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310)
712 static bool empty() {
return false; }
719 inline void resize(
const size_t nElements)
721 if (nElements !=
N)
throw std::logic_error(
"Try to change the size of a CArray.");
726 const T*
data()
const {
return elems; }
730 template <
typename T2>
740 for (
size_t i = 0; i <
N; i++) elems[i] = value;
746 for (
size_t i = 0; i <
N; i++) elems[i] = value;
755 throw std::out_of_range(
"CArray<>: index out of range");
763 template <
int DIM,
typename T>
769 template <
typename T>
815 template <
typename Distance,
class DatasetAdaptor,
int DIM = -1,
typename IndexType =
size_t>
906 , index_params(params)
908 , distance(inputData)
910 m_size = dataset.kdtree_get_point_count();
911 m_size_at_index_build =
m_size;
912 dim = dimensionality;
913 if (DIM > 0)
dim = DIM;
914 m_leaf_max_size = params.leaf_max_size;
928 m_size_at_index_build = 0;
938 m_size_at_index_build =
m_size;
940 computeBoundingBox(root_bbox);
941 root_node = divideTree(0,
m_size, root_bbox);
948 size_t veclen()
const
950 return static_cast<size_t>(DIM > 0 ? DIM :
dim);
957 size_t usedMemory()
const
959 return pool.usedMemory + pool.wastedMemory + dataset.kdtree_get_point_count() *
sizeof(IndexType);
977 template <
typename RESULTSET>
984 throw std::runtime_error(
"[nanoflann] findNeighbors() called before building the index.");
985 float epsError = 1 + searchParams.
eps;
989 DistanceType distsq = computeInitialDistances(vec, dists);
990 searchLevel(result, vec, root_node, distsq, dists, epsError);
991 return result.full();
1002 size_t knnSearch(
const ElementType* query_point,
const size_t num_closest, IndexType* out_indices,
DistanceType* out_distances_sq,
const int = 10)
const
1005 resultSet.
init(out_indices, out_distances_sq);
1007 return resultSet.
size();
1025 const size_t nFound = radiusSearchCustomCallback(query_point, resultSet, searchParams);
1036 template <
class SEARCH_CALLBACK>
1039 this->findNeighbors(resultSet, query_point, searchParams);
1040 return resultSet.size();
1050 m_size = dataset.kdtree_get_point_count();
1052 for (
size_t i = 0; i <
m_size; i++) vind[i] = i;
1058 return dataset.kdtree_get_pt(idx, component);
1064 if (tree->child1 != NULL)
1066 save_tree(stream, tree->child1);
1068 if (tree->child2 != NULL)
1070 save_tree(stream, tree->child2);
1076 tree = pool.allocate<Node>();
1078 if (tree->child1 != NULL)
1080 load_tree(stream, tree->child1);
1082 if (tree->child2 != NULL)
1084 load_tree(stream, tree->child2);
1090 bbox.resize((DIM > 0 ? DIM :
dim));
1091 if (dataset.kdtree_get_bbox(bbox))
1097 const size_t N = dataset.kdtree_get_point_count();
1098 if (!N)
throw std::runtime_error(
"[nanoflann] computeBoundingBox() called but no data points found.");
1099 for (
int i = 0; i < (DIM > 0 ? DIM :
dim); ++i)
1102 bbox[i].high = dataset_get(0, i);
1104 for (
size_t k = 1;
k <
N; ++
k)
1106 for (
int i = 0; i < (DIM > 0 ? DIM :
dim); ++i)
1108 if (dataset_get(
k, i) < bbox[i].low) bbox[i].low = dataset_get(
k, i);
1109 if (dataset_get(
k, i) > bbox[i].high) bbox[i].high = dataset_get(
k, i);
1124 NodePtr node = pool.allocate<Node>();
1127 if ((right - left) <=
static_cast<IndexType
>(m_leaf_max_size))
1129 node->child1 = node->child2 = NULL;
1130 node->node_type.lr.left =
left;
1131 node->node_type.lr.right =
right;
1134 for (
int i = 0; i < (DIM > 0 ? DIM :
dim); ++i)
1136 bbox[i].low = dataset_get(vind[left], i);
1137 bbox[i].high = dataset_get(vind[left], i);
1139 for (IndexType
k = left + 1;
k <
right; ++
k)
1141 for (
int i = 0; i < (DIM > 0 ? DIM :
dim); ++i)
1143 if (bbox[i].low > dataset_get(vind[
k], i)) bbox[i].low = dataset_get(vind[k], i);
1144 if (bbox[i].high < dataset_get(vind[k], i)) bbox[i].high = dataset_get(vind[k], i);
1153 middleSplit_(&vind[0] + left, right - left, idx, cutfeat, cutval, bbox);
1155 node->node_type.sub.divfeat = cutfeat;
1158 left_bbox[cutfeat].high = cutval;
1159 node->child1 = divideTree(left, left + idx, left_bbox);
1162 right_bbox[cutfeat].low = cutval;
1163 node->child2 = divideTree(left + idx, right, right_bbox);
1165 node->node_type.sub.divlow = left_bbox[cutfeat].high;
1166 node->node_type.sub.divhigh = right_bbox[cutfeat].low;
1168 for (
int i = 0; i < (DIM > 0 ? DIM :
dim); ++i)
1170 bbox[i].low =
std::min(left_bbox[i].low, right_bbox[i].low);
1171 bbox[i].high =
std::max(left_bbox[i].high, right_bbox[i].high);
1180 min_elem = dataset_get(ind[0], element);
1181 max_elem = dataset_get(ind[0], element);
1182 for (IndexType i = 1; i <
count; ++i)
1185 if (val < min_elem) min_elem = val;
1186 if (val > max_elem) max_elem = val;
1193 ElementType max_span = bbox[0].high - bbox[0].low;
1194 for (
int i = 1; i < (DIM > 0 ? DIM :
dim); ++i)
1197 if (span > max_span)
1204 for (
int i = 0; i < (DIM > 0 ? DIM :
dim); ++i)
1207 if (span > (1 - EPS) * max_span)
1210 computeMinMax(ind, count, i, min_elem, max_elem);
1213 if (spread > max_spread)
1216 max_spread = spread;
1221 DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2;
1223 computeMinMax(ind, count, cutfeat, min_elem, max_elem);
1225 if (split_val < min_elem)
1227 else if (split_val > max_elem)
1232 IndexType lim1, lim2;
1233 planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
1235 if (lim1 > count / 2)
1237 else if (lim2 < count / 2)
1252 void planeSplit(IndexType* ind,
const IndexType
count,
int cutfeat,
DistanceType& cutval, IndexType& lim1, IndexType& lim2)
1256 IndexType
right = count - 1;
1259 while (left <= right && dataset_get(ind[left], cutfeat) < cutval) ++
left;
1260 while (right && left <= right && dataset_get(ind[right], cutfeat) >= cutval) --
right;
1261 if (left > right || !right)
break;
1273 while (left <= right && dataset_get(ind[left], cutfeat) <= cutval) ++
left;
1274 while (right && left <= right && dataset_get(ind[right], cutfeat) > cutval) --
right;
1275 if (left > right || !right)
break;
1288 for (
int i = 0; i < (DIM > 0 ? DIM :
dim); ++i)
1290 if (vec[i] < root_bbox[i].low)
1292 dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i);
1295 if (vec[i] > root_bbox[i].high)
1297 dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i);
1309 template <
class RESULTSET>
1314 if ((node->child1 == NULL) && (node->child2 == NULL))
1318 for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; ++i)
1320 const IndexType index = vind[i];
1322 if (dist < worst_dist)
1324 result_set.addPoint(dist, vind[i]);
1331 int idx = node->node_type.sub.divfeat;
1334 DistanceType diff2 = val - node->node_type.sub.divhigh;
1339 if ((diff1 + diff2) < 0)
1341 bestChild = node->child1;
1342 otherChild = node->child2;
1343 cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1347 bestChild = node->child2;
1348 otherChild = node->child1;
1349 cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
1353 searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
1356 mindistsq = mindistsq + cut_dist - dst;
1357 dists[
idx] = cut_dist;
1358 if (mindistsq * epsError <= result_set.worstDist())
1360 searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
1370 void saveIndex(FILE* stream)
1377 save_tree(stream, root_node);
1384 void loadIndex(FILE* stream)
1391 load_tree(stream, root_node);
1418 typedef typename MatrixType::Scalar
num_t;
1420 typedef typename Distance::template traits<num_t, self_t>::distance_t
metric_t;
1427 : m_data_matrix(mat)
1430 if (dims != dimensionality)
throw std::runtime_error(
"Error: 'dimensionality' must match column count in data matrix");
1431 if (DIM > 0 && static_cast<int>(dims) != DIM)
1432 throw std::runtime_error(
"Data set dimensionality does not match the 'DIM' template argument");
1434 index->buildIndex();
1454 inline void query(
const num_t* query_point,
const size_t num_closest,
IndexType* out_indices,
num_t* out_distances_sq,
const int = 10)
const
1457 resultSet.
init(out_indices, out_distances_sq);
1474 inline size_t kdtree_get_point_count()
const
1476 return m_data_matrix.rows();
1485 const num_t d = p1[i] - m_data_matrix.coeff(idx_p2, i);
1494 return m_data_matrix.coeff(idx,
IndexType(dim));
1500 template <
class BBOX>
1501 bool kdtree_get_bbox(BBOX& )
const