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>
100 dists[
capacity - 1] = (std::numeric_limits<DistanceType>::max)();
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)
168 inline void init() { clear(); }
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; }
178 m_indices_dists.push_back(std::make_pair(index, dist));
181 inline DistanceType
worstDist()
const {
return radius; }
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)
291 const DistanceType diff0 = std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++));
292 const DistanceType diff1 = std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++));
293 const DistanceType diff2 = std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++));
294 const DistanceType diff3 = std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++));
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>
313 return std::abs(a - b);
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)
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;
939 if (m_size == 0)
return;
940 computeBoundingBox(root_bbox);
941 root_node = divideTree(0, m_size, root_bbox);
945 size_t size()
const {
return m_size; }
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();
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();
1051 if (vind.size() != m_size) vind.resize(m_size);
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)
1082 if (tree->
child2 != NULL)
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);
1127 if ((right - left) <=
static_cast<IndexType>(m_leaf_max_size))
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);
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);
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);
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);
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)
1233 planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
1235 if (lim1 > count / 2)
1237 else if (lim2 < count / 2)
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>
1322 if (dist < worst_dist)
1324 result_set.addPoint(dist, vind[
i]);
1339 if ((diff1 + diff2) < 0)
1341 bestChild = node->
child1;
1342 otherChild = node->
child2;
1347 bestChild = node->
child2;
1348 otherChild = node->
child1;
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)
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");
1435 index->buildIndex();
1455 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
1458 resultSet.
init(out_indices, out_distances_sq);
1475 inline size_t kdtree_get_point_count()
const
1477 return m_data_matrix.rows();
1486 const num_t d = p1[
i] - m_data_matrix.coeff(idx_p2,
i);
1495 return m_data_matrix.coeff(idx,
IndexType(dim));
1501 template <
class BBOX>
1502 bool kdtree_get_bbox(BBOX& )
const