46 #ifndef NANOFLANN_HPP_
47 #define NANOFLANN_HPP_
59 #if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
73 #define NANOFLANN_VERSION 0x122
77 template <
typename DistanceType,
typename IndexType =
size_t,
typename CountType =
size_t>
90 inline void init(IndexType* indices_, DistanceType* dists_)
96 dists[
capacity-1] = (std::numeric_limits<DistanceType>::max)();
99 inline CountType
size()
const
110 inline void addPoint(DistanceType dist, IndexType index)
113 for (i=
count; i>0; --i) {
114 #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same distance, the one with the lowest-index will be returned first.
117 if (
dists[i-1]>dist) {
143 template <
typename DistanceType,
typename IndexType =
size_t>
151 inline RadiusResultSet(DistanceType radius_, std::vector<std::pair<IndexType,DistanceType> >& indices_dists) : radius(radius_), m_indices_dists(indices_dists)
159 inline void clear() { m_indices_dists.clear(); }
161 inline size_t size()
const {
return m_indices_dists.size(); }
163 inline bool full()
const {
return true; }
165 inline void addPoint(DistanceType dist, IndexType index)
168 m_indices_dists.push_back(std::make_pair(index,dist));
171 inline DistanceType
worstDist()
const {
return radius; }
186 if (m_indices_dists.empty())
throw std::runtime_error(
"Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
187 typedef typename std::vector<std::pair<IndexType,DistanceType> >::const_iterator DistIt;
188 DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end());
197 template <
typename PairType>
198 inline bool operator()(
const PairType &p1,
const PairType &p2)
const {
199 return p1.second < p2.second;
211 fwrite(&value,
sizeof(value),
count, stream);
217 size_t size = value.size();
218 fwrite(&
size,
sizeof(
size_t), 1, stream);
219 fwrite(&value[0],
sizeof(T),
size, stream);
225 size_t read_cnt = fread(&value,
sizeof(value),
count, stream);
226 if (read_cnt !=
count) {
227 throw std::runtime_error(
"Cannot read from file");
236 size_t read_cnt = fread(&
size,
sizeof(
size_t), 1, stream);
238 throw std::runtime_error(
"Cannot read from file");
241 read_cnt = fread(&value[0],
sizeof(T),
size, stream);
242 if (read_cnt!=
size) {
243 throw std::runtime_error(
"Cannot read from file");
257 template<
class T,
class DataSource,
typename _DistanceType = T>
265 L1_Adaptor(
const DataSource &_data_source) : data_source(_data_source) { }
270 const T* last = a +
size;
271 const T* lastgroup = last - 3;
275 while (a < lastgroup) {
276 const DistanceType diff0 = std::abs(a[0] - data_source.kdtree_get_pt(b_idx,d++));
277 const DistanceType diff1 = std::abs(a[1] - data_source.kdtree_get_pt(b_idx,d++));
278 const DistanceType diff2 = std::abs(a[2] - data_source.kdtree_get_pt(b_idx,d++));
279 const DistanceType diff3 = std::abs(a[3] - data_source.kdtree_get_pt(b_idx,d++));
280 result += diff0 + diff1 + diff2 + diff3;
282 if ((worst_dist>0)&&(result>worst_dist)) {
288 result += std::abs( *a++ - data_source.kdtree_get_pt(b_idx,d++) );
293 template <
typename U,
typename V>
296 return std::abs(a-b);
305 template<
class T,
class DataSource,
typename _DistanceType = T>
313 L2_Adaptor(
const DataSource &_data_source) : data_source(_data_source) { }
318 const T* last = a +
size;
319 const T* lastgroup = last - 3;
323 while (a < lastgroup) {
324 const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx,d++);
325 const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx,d++);
326 const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx,d++);
327 const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx,d++);
328 result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
330 if ((worst_dist>0)&&(result>worst_dist)) {
336 const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx,d++);
337 result += diff0 * diff0;
342 template <
typename U,
typename V>
354 template<
class T,
class DataSource,
typename _DistanceType = T>
365 return data_source.kdtree_distance(a,b_idx,
size);
368 template <
typename U,
typename V>
377 template<
class T,
class DataSource>
384 template<
class T,
class DataSource>
391 template<
class T,
class DataSource>
406 leaf_max_size(_leaf_max_size)
416 SearchParams(
int checks_IGNORED_ = 32,
float eps_ = 0,
bool sorted_ =
true ) :
417 checks(checks_IGNORED_),
eps(eps_), sorted(sorted_) {}
436 template <
typename T>
439 T* mem =
static_cast<T*
>( ::malloc(
sizeof(T)*
count));
503 while (base !=
nullptr) {
504 void *prev = *(
static_cast<void**
>( base));
526 if (
size > remaining) {
528 wastedMemory += remaining;
535 void* m = ::malloc(blocksize);
537 fprintf(stderr,
"Failed to allocate memory.\n");
542 static_cast<void**
>(m)[0] = base;
548 remaining = blocksize -
sizeof(
void*) - shift;
549 loc = (
static_cast<char*
>(m) +
sizeof(
void*) + shift);
552 loc =
static_cast<char*
>(loc) +
size;
567 template <
typename T>
570 T* mem =
static_cast<T*
>(this->malloc(
sizeof(T)*
count));
606 template <
typename T, std::
size_t N>
623 inline const_iterator
begin()
const {
return elems; }
625 inline const_iterator
end()
const {
return elems+N; }
628 #if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS)
631 #elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310)
652 const_reference
at(
size_type i)
const { rangecheck(i);
return elems[i]; }
655 const_reference
front()
const {
return elems[0]; }
657 const_reference
back()
const {
return elems[N-1]; }
660 static bool empty() {
return false; }
662 enum { static_size = N };
664 inline void resize(
const size_t nElements) {
if (nElements!=N)
throw std::logic_error(
"Try to change the size of a CArray."); }
668 const T*
data()
const {
return elems; }
677 inline void assign (
const T& value) {
for (
size_t i=0;i<N;i++) elems[i]=value; }
679 void assign (
const size_t n,
const T& value) { assert(N==n);
for (
size_t i=0;i<N;i++) elems[i]=value; }
688 template <
int DIM,
typename T>
694 template <
typename T>
739 template <
typename Distance,
class DatasetAdaptor,
int DIM = -1,
typename IndexType =
size_t>
832 dataset(inputData), index_params(
params), root_node(nullptr),
distance(inputData)
834 m_size = dataset.kdtree_get_point_count();
835 m_size_at_index_build =
m_size;
836 dim = dimensionality;
838 m_leaf_max_size =
params.leaf_max_size;
852 m_size_at_index_build = 0;
862 m_size_at_index_build =
m_size;
864 computeBoundingBox(root_bbox);
865 root_node = divideTree(0,
m_size, root_bbox );
873 return static_cast<size_t>(DIM>0 ? DIM : dim);
900 template <
typename RESULTSET>
907 throw std::runtime_error(
"[nanoflann] findNeighbors() called before building the index.");
908 float epsError = 1+searchParams.
eps;
911 dists.assign((DIM>0 ? DIM : dim) ,0);
913 searchLevel(result, vec, root_node, distsq,
dists, epsError);
914 return result.full();
928 resultSet.
init(out_indices, out_distances_sq);
930 return resultSet.
size();
948 const size_t nFound = radiusSearchCustomCallback(query_point,resultSet,searchParams);
959 template <
class SEARCH_CALLBACK>
962 this->findNeighbors(resultSet, query_point, searchParams);
963 return resultSet.size();
973 m_size = dataset.kdtree_get_point_count();
975 for (
size_t i = 0; i <
m_size; i++) vind[i] = i;
980 return dataset.kdtree_get_pt(idx,component);
988 save_tree(stream, tree->
child1);
991 save_tree(stream, tree->
child2);
1000 if (tree->
child1!=NULL) {
1001 load_tree(stream, tree->
child1);
1003 if (tree->
child2!=NULL) {
1004 load_tree(stream, tree->
child2);
1011 bbox.
resize((DIM>0 ? DIM : dim));
1012 if (dataset.kdtree_get_bbox(bbox))
1018 const size_t N = dataset.kdtree_get_point_count();
1019 if (!N)
throw std::runtime_error(
"[nanoflann] computeBoundingBox() called but no data points found.");
1020 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1022 bbox[i].high = dataset_get(0,i);
1024 for (
size_t k=1; k<N; ++k) {
1025 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1026 if (dataset_get(k,i)<bbox[i].low) bbox[i].low = dataset_get(k,i);
1027 if (dataset_get(k,i)>bbox[i].high) bbox[i].high = dataset_get(k,i);
1046 if ( (right-left) <=
static_cast<IndexType
>(m_leaf_max_size) ) {
1049 node->node_type.lr.right = right;
1052 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1053 bbox[i].low = dataset_get(vind[left],i);
1054 bbox[i].high = dataset_get(vind[left],i);
1056 for (IndexType k=left+1; k<right; ++k) {
1057 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1058 if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i);
1059 if (bbox[i].high<dataset_get(vind[k],i)) bbox[i].high=dataset_get(vind[k],i);
1067 middleSplit_(&vind[0]+left, right-left, idx, cutfeat, cutval, bbox);
1069 node->node_type.sub.divfeat = cutfeat;
1072 left_bbox[cutfeat].high = cutval;
1073 node->child1 = divideTree(left, left+idx, left_bbox);
1076 right_bbox[cutfeat].low = cutval;
1077 node->child2 = divideTree(left+idx, right, right_bbox);
1079 node->node_type.sub.divlow = left_bbox[cutfeat].high;
1080 node->node_type.sub.divhigh = right_bbox[cutfeat].low;
1082 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1083 bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
1084 bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
1094 min_elem = dataset_get(ind[0],element);
1095 max_elem = dataset_get(ind[0],element);
1096 for (IndexType i=1; i<
count; ++i) {
1098 if (
val<min_elem) min_elem =
val;
1099 if (
val>max_elem) max_elem =
val;
1107 for (
int i=1; i<(DIM>0 ? DIM : dim); ++i) {
1109 if (span>max_span) {
1115 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1117 if (span>(1-EPS)*max_span) {
1119 computeMinMax(ind,
count, cutfeat, min_elem, max_elem);
1121 if (spread>max_spread) {
1123 max_spread = spread;
1128 DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
1130 computeMinMax(ind,
count, cutfeat, min_elem, max_elem);
1132 if (split_val<min_elem) cutval = min_elem;
1133 else if (split_val>max_elem) cutval = max_elem;
1134 else cutval = split_val;
1136 IndexType lim1, lim2;
1137 planeSplit(ind,
count, cutfeat, cutval, lim1, lim2);
1139 if (lim1>
count/2) index = lim1;
1140 else if (lim2<
count/2) index = lim2;
1141 else index =
count/2;
1158 IndexType right =
count-1;
1160 while (left<=right && dataset_get(ind[left],cutfeat)<cutval) ++left;
1161 while (right && left<=right && dataset_get(ind[right],cutfeat)>=cutval) --right;
1162 if (left>right || !right)
break;
1163 std::swap(ind[left], ind[right]);
1173 while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left;
1174 while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right;
1175 if (left>right || !right)
break;
1176 std::swap(ind[left], ind[right]);
1188 for (
int i = 0; i < (DIM>0 ? DIM : dim); ++i) {
1189 if (vec[i] < root_bbox[i].low) {
1190 dists[i] =
distance.accum_dist(vec[i], root_bbox[i].low, i);
1193 if (vec[i] > root_bbox[i].high) {
1194 dists[i] =
distance.accum_dist(vec[i], root_bbox[i].high, i);
1206 template <
class RESULTSET>
1211 if ((node->
child1 ==
nullptr)&&(node->
child2 ==
nullptr)) {
1214 for (IndexType i=node->
node_type.
lr.left; i<node->node_type.lr.right; ++i) {
1215 const IndexType index = vind[i];
1217 if (dist<worst_dist) {
1218 result_set.addPoint(dist,vind[i]);
1233 if ((diff1+diff2)<0) {
1234 bestChild = node->
child1;
1235 otherChild = node->
child2;
1239 bestChild = node->
child2;
1240 otherChild = node->
child1;
1245 searchLevel(result_set, vec, bestChild, mindistsq,
dists, epsError);
1248 mindistsq = mindistsq + cut_dist - dst;
1249 dists[idx] = cut_dist;
1250 if (mindistsq*epsError<=result_set.worstDist()) {
1251 searchLevel(result_set, vec, otherChild, mindistsq,
dists, epsError);
1268 save_tree(stream, root_node);
1282 load_tree(stream, root_node);
1312 typedef typename Distance::template traits<num_t,self_t>::distance_t
metric_t;
1321 if (dims!=dimensionality)
throw std::runtime_error(
"Error: 'dimensionality' must match column count in data matrix");
1322 if (DIM>0 &&
static_cast<int>(dims)!=DIM)
1323 throw std::runtime_error(
"Data set dimensionality does not match the 'DIM' template argument");
1343 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
1346 resultSet.
init(out_indices, out_distances_sq);
1362 return m_data_matrix.rows();
1370 const num_t d= p1[i]-m_data_matrix.coeff(idx_p2,i);
1378 return m_data_matrix.coeff(idx,
IndexType(dim));
1384 template <
class BBOX>