33 #ifndef NANOFLANN_HPP_
34 #define NANOFLANN_HPP_
45 #if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
59 #define NANOFLANN_VERSION 0x119
63 template <
typename DistanceType,
typename IndexType =
size_t,
typename CountType =
size_t>
76 inline void init(IndexType* indices_, DistanceType* dists_)
81 dists[
capacity-1] = (std::numeric_limits<DistanceType>::max)();
84 inline CountType
size()
const
89 inline bool full()
const
95 inline void addPoint(DistanceType dist, IndexType index)
98 for (i=
count; i>0; --i) {
99 #ifdef NANOFLANN_FIRST_MATCH // If defined and two poins have the same distance, the one with the lowest-index will be returned first.
102 if (
dists[i-1]>dist) {
128 template <
typename DistanceType,
typename IndexType =
size_t>
132 const DistanceType radius;
134 std::vector<std::pair<IndexType,DistanceType> >& m_indices_dists;
136 inline RadiusResultSet(DistanceType radius_, std::vector<std::pair<IndexType,DistanceType> >& indices_dists) : radius(radius_), m_indices_dists(indices_dists)
144 inline void clear() { m_indices_dists.clear(); }
146 inline size_t size()
const {
return m_indices_dists.size(); }
148 inline bool full()
const {
return true; }
150 inline void addPoint(DistanceType dist, IndexType index)
153 m_indices_dists.push_back(std::make_pair(index,dist));
156 inline DistanceType
worstDist()
const {
return radius; }
159 inline void set_radius_and_clear(
const DistanceType r )
169 std::pair<IndexType,DistanceType> worst_item()
const
171 if (m_indices_dists.empty())
throw std::runtime_error(
"Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
172 typedef typename std::vector<std::pair<IndexType,DistanceType> >
::const_iterator DistIt;
173 DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end());
182 template <
typename PairType>
183 inline bool operator()(
const PairType &p1,
const PairType &p2)
const {
184 return p1.second < p2.second;
196 fwrite(&value,
sizeof(value),
count, stream);
200 void save_value(FILE* stream,
const std::vector<T>& value)
202 size_t size = value.size();
203 fwrite(&
size,
sizeof(
size_t), 1, stream);
204 fwrite(&value[0],
sizeof(T),
size, stream);
210 size_t read_cnt = fread(&value,
sizeof(value),
count, stream);
211 if (read_cnt !=
count) {
212 throw std::runtime_error(
"Cannot read from file");
218 void load_value(FILE* stream, std::vector<T>& value)
221 size_t read_cnt = fread(&
size,
sizeof(
size_t), 1, stream);
223 throw std::runtime_error(
"Cannot read from file");
226 read_cnt = fread(&value[0],
sizeof(T),
size, stream);
227 if (read_cnt!=
size) {
228 throw std::runtime_error(
"Cannot read from file");
237 template<
typename T>
inline T
abs(T x) {
return (x<0) ? -x : x; }
239 template<>
inline float abs<float>(
float x) {
return fabsf(x); }
240 template<>
inline double abs<double>(
double x) {
return fabs(x); }
241 template<>
inline long double abs<long double>(
long double x) {
return fabsl(x); }
248 template<
class T,
class DataSource,
typename _DistanceType = T>
254 const DataSource &data_source;
256 L1_Adaptor(
const DataSource &_data_source) : data_source(_data_source) { }
261 const T* last = a +
size;
262 const T* lastgroup = last - 3;
266 while (a < lastgroup) {
271 result += diff0 + diff1 + diff2 + diff3;
273 if ((worst_dist>0)&&(result>worst_dist)) {
279 result +=
nanoflann::abs( *a++ - data_source.kdtree_get_pt(b_idx,d++) );
284 template <
typename U,
typename V>
285 inline DistanceType accum_dist(
const U a,
const V b,
int )
const
296 template<
class T,
class DataSource,
typename _DistanceType = T>
302 const DataSource &data_source;
304 L2_Adaptor(
const DataSource &_data_source) : data_source(_data_source) { }
309 const T* last = a +
size;
310 const T* lastgroup = last - 3;
314 while (a < lastgroup) {
315 const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx,d++);
316 const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx,d++);
317 const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx,d++);
318 const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx,d++);
319 result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
321 if ((worst_dist>0)&&(result>worst_dist)) {
327 const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx,d++);
328 result += diff0 * diff0;
333 template <
typename U,
typename V>
334 inline DistanceType accum_dist(
const U a,
const V b,
int )
const
345 template<
class T,
class DataSource,
typename _DistanceType = T>
351 const DataSource &data_source;
353 L2_Simple_Adaptor(
const DataSource &_data_source) : data_source(_data_source) { }
355 inline DistanceType operator()(
const T* a,
const size_t b_idx,
size_t size)
const {
356 return data_source.kdtree_distance(a,b_idx,
size);
359 template <
typename U,
typename V>
360 inline DistanceType accum_dist(
const U a,
const V b,
int )
const
368 template<
class T,
class DataSource>
375 template<
class T,
class DataSource>
382 template<
class T,
class DataSource>
400 leaf_max_size(_leaf_max_size), dim(dim_)
403 size_t leaf_max_size;
411 SearchParams(
int checks_IGNORED_ = 32,
float eps_ = 0,
bool sorted_ =
true ) :
412 checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {}
431 template <
typename T>
434 T* mem = (T*) ::malloc(
sizeof(T)*
count);
499 while (base != NULL) {
500 void *prev = *((
void**) base);
511 void* malloc(
const size_t req_size)
522 if (
size > remaining) {
524 wastedMemory += remaining;
531 void* m = ::malloc(blocksize);
533 fprintf(stderr,
"Failed to allocate memory.\n");
538 ((
void**) m)[0] = base;
544 remaining = blocksize -
sizeof(
void*) - shift;
545 loc = ((
char*)m +
sizeof(
void*) + shift);
548 loc = (
char*)loc +
size;
563 template <
typename T>
566 T* mem = (T*) this->malloc(
sizeof(T)*
count);
602 template <
typename T, std::
size_t N>
613 typedef const T& const_reference;
624 #if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS)
627 #elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310)
642 const_reverse_iterator rend()
const {
return const_reverse_iterator(
begin()); }
645 inline const_reference operator[](size_type i)
const {
return elems[i]; }
648 const_reference at(size_type i)
const { rangecheck(i);
return elems[i]; }
651 const_reference front()
const {
return elems[0]; }
653 const_reference back()
const {
return elems[N-1]; }
656 static bool empty() {
return false; }
660 inline void resize(
const size_t nElements) {
if (nElements!=N)
throw std::logic_error(
"Try to change the size of a CArray."); }
664 const T* data()
const {
return elems; }
666 T* data() {
return elems; }
673 inline void assign (
const T& value) {
for (
size_t i=0;i<N;i++) elems[i]=value; }
675 void assign (
const size_t n,
const T& value) { assert(N==n);
for (
size_t i=0;i<N;i++) elems[i]=value; }
678 static void rangecheck (
size_type i) {
if (i >=
size()) {
throw std::out_of_range(
"CArray<>: index out of range"); } }
684 template <
int DIM,
typename T>
690 template <
typename T>
734 template <
typename Distance,
class DatasetAdaptor,
int DIM = -1,
typename IndexType =
size_t>
748 std::vector<IndexType> vind;
750 size_t m_leaf_max_size;
756 const DatasetAdaptor &dataset;
773 IndexType left, right;
790 Node* child1, * child2;
792 typedef Node* NodePtr;
810 template <
typename T,
typename DistanceType>
855 dataset(inputData), index_params(params), root_node(NULL),
distance(inputData)
857 m_size = dataset.kdtree_get_point_count();
858 dim = dimensionality;
861 if (params.dim>0) dim = params.dim;
863 m_leaf_max_size = params.leaf_max_size;
890 if(m_size == 0)
return;
891 computeBoundingBox(root_bbox);
892 root_node = divideTree(0, m_size, root_bbox );
906 size_t veclen()
const
908 return static_cast<size_t>(DIM>0 ? DIM : dim);
915 size_t usedMemory()
const
934 template <
typename RESULTSET>
938 if (!root_node)
throw std::runtime_error(
"[nanoflann] findNeighbors() called before building the index or no data points.");
939 float epsError = 1+searchParams.
eps;
942 dists.assign((DIM>0 ? DIM : dim) ,0);
944 searchLevel(result, vec, root_node, distsq,
dists, epsError);
953 inline void knnSearch(
const ElementType *query_point,
const size_t num_closest, IndexType *out_indices,
DistanceType *out_distances_sq,
const int = 10)
const
956 resultSet.
init(out_indices, out_distances_sq);
972 size_t radiusSearch(
const ElementType *query_point,
const DistanceType radius, std::vector<std::pair<IndexType,DistanceType> >& IndicesDists,
const SearchParams& searchParams)
const
975 this->findNeighbors(resultSet, query_point, searchParams);
980 return resultSet.
size();
990 m_size = dataset.kdtree_get_point_count();
991 if (vind.size()!=m_size) vind.resize(m_size);
992 for (
size_t i = 0; i < m_size; i++) vind[i] = i;
996 inline ElementType dataset_get(
size_t idx,
int component)
const {
997 return dataset.kdtree_get_pt(idx,component);
1001 void save_tree(FILE* stream,
NodePtr tree)
1004 if (tree->
child1!=NULL) {
1005 save_tree(stream, tree->
child1);
1007 if (tree->
child2!=NULL) {
1008 save_tree(stream, tree->
child2);
1013 void load_tree(FILE* stream,
NodePtr& tree)
1017 if (tree->
child1!=NULL) {
1018 load_tree(stream, tree->
child1);
1020 if (tree->
child2!=NULL) {
1021 load_tree(stream, tree->
child2);
1028 bbox.resize((DIM>0 ? DIM : dim));
1029 if (dataset.kdtree_get_bbox(bbox))
1035 const size_t N = dataset.kdtree_get_point_count();
1036 if (!N)
throw std::runtime_error(
"[nanoflann] computeBoundingBox() called but no data points found.");
1037 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1039 bbox[i].high = dataset_get(0,i);
1041 for (
size_t k=1; k<N; ++k) {
1042 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1043 if (dataset_get(k,i)<bbox[i].low) bbox[i].low = dataset_get(k,i);
1044 if (dataset_get(k,i)>bbox[i].high) bbox[i].high = dataset_get(k,i);
1065 if ( (right-left) <= m_leaf_max_size) {
1067 node->
lr.left = left;
1068 node->
lr.right = right;
1071 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1072 bbox[i].low = dataset_get(vind[left],i);
1073 bbox[i].high = dataset_get(vind[left],i);
1075 for (IndexType k=left+1; k<right; ++k) {
1076 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1077 if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i);
1078 if (bbox[i].high<dataset_get(vind[k],i)) bbox[i].high=dataset_get(vind[k],i);
1086 middleSplit_(&vind[0]+left, right-left, idx, cutfeat, cutval, bbox);
1088 node->
sub.divfeat = cutfeat;
1091 left_bbox[cutfeat].high = cutval;
1092 node->
child1 = divideTree(left, left+idx, left_bbox);
1095 right_bbox[cutfeat].low = cutval;
1096 node->
child2 = divideTree(left+idx, right, right_bbox);
1098 node->
sub.divlow = left_bbox[cutfeat].high;
1099 node->
sub.divhigh = right_bbox[cutfeat].low;
1101 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1102 bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
1103 bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
1112 min_elem = dataset_get(ind[0],element);
1113 max_elem = dataset_get(ind[0],element);
1114 for (IndexType i=1; i<
count; ++i) {
1116 if (val<min_elem) min_elem = val;
1117 if (val>max_elem) max_elem = val;
1123 const DistanceType EPS=static_cast<DistanceType>(0.00001);
1125 for (
int i=1; i<(DIM>0 ? DIM : dim); ++i) {
1127 if (span>max_span) {
1133 for (
int i=0; i<(DIM>0 ? DIM : dim); ++i) {
1135 if (span>(1-EPS)*max_span) {
1137 computeMinMax(ind,
count, cutfeat, min_elem, max_elem);
1139 if (spread>max_spread) {
1141 max_spread = spread;
1146 DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
1148 computeMinMax(ind,
count, cutfeat, min_elem, max_elem);
1150 if (split_val<min_elem) cutval = min_elem;
1151 else if (split_val>max_elem) cutval = max_elem;
1152 else cutval = split_val;
1154 IndexType lim1, lim2;
1155 planeSplit(ind,
count, cutfeat, cutval, lim1, lim2);
1157 if (lim1>
count/2) index = lim1;
1158 else if (lim2<
count/2) index = lim2;
1159 else index =
count/2;
1172 void planeSplit(IndexType* ind,
const IndexType
count,
int cutfeat,
DistanceType cutval, IndexType& lim1, IndexType& lim2)
1176 IndexType right =
count-1;
1178 while (left<=right && dataset_get(ind[left],cutfeat)<cutval) ++left;
1179 while (right && left<=right && dataset_get(ind[right],cutfeat)>=cutval) --right;
1180 if (left>right || !right)
break;
1181 std::swap(ind[left], ind[right]);
1191 while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left;
1192 while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right;
1193 if (left>right || !right)
break;
1194 std::swap(ind[left], ind[right]);
1206 for (
int i = 0; i < (DIM>0 ? DIM : dim); ++i) {
1207 if (vec[i] < root_bbox[i].low) {
1208 dists[i] =
distance.accum_dist(vec[i], root_bbox[i].low, i);
1211 if (vec[i] > root_bbox[i].high) {
1212 dists[i] =
distance.accum_dist(vec[i], root_bbox[i].high, i);
1224 template <
class RESULTSET>
1232 for (IndexType i=node->
lr.left; i<node->lr.right; ++i) {
1233 const IndexType index = vind[i];
1235 if (dist<worst_dist) {
1236 result_set.addPoint(dist,vind[i]);
1243 int idx = node->
sub.divfeat;
1251 if ((diff1+diff2)<0) {
1252 bestChild = node->
child1;
1253 otherChild = node->
child2;
1254 cut_dist =
distance.accum_dist(val, node->
sub.divhigh, idx);
1257 bestChild = node->
child2;
1258 otherChild = node->
child1;
1259 cut_dist =
distance.accum_dist( val, node->
sub.divlow, idx);
1263 searchLevel(result_set, vec, bestChild, mindistsq,
dists, epsError);
1266 mindistsq = mindistsq + cut_dist - dst;
1267 dists[idx] = cut_dist;
1268 if (mindistsq*epsError<=result_set.worstDist()) {
1269 searchLevel(result_set, vec, otherChild, mindistsq,
dists, epsError);
1279 void saveIndex(FILE* stream)
1286 save_tree(stream, root_node);
1293 void loadIndex(FILE* stream)
1300 load_tree(stream, root_node);
1329 typedef typename MatrixType::Scalar
num_t;
1330 typedef typename Distance::template traits<num_t,self_t>::distance_t
metric_t;
1336 KDTreeEigenMatrixAdaptor(
const int dimensionality,
const MatrixType &mat,
const int leaf_max_size = 10) : m_data_matrix(mat)
1338 const size_t dims = mat.cols();
1339 if (DIM>0 && static_cast<int>(dims)!=DIM)
1340 throw std::runtime_error(
"Data set dimensionality does not match the 'DIM' template argument");
1353 const MatrixType &m_data_matrix;
1360 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
1363 resultSet.
init(out_indices, out_distances_sq);
1370 const self_t & derived()
const {
1373 self_t & derived() {
1378 inline size_t kdtree_get_point_count()
const {
1379 return m_data_matrix.rows();
1383 inline num_t kdtree_distance(
const num_t *p1,
const size_t idx_p2,
size_t size)
const
1386 for (
size_t i=0; i<
size; i++) {
1387 const num_t d= p1[i]-m_data_matrix.coeff(idx_p2,i);
1394 inline num_t kdtree_get_pt(
const size_t idx,
int dim)
const {
1395 return m_data_matrix.coeff(idx,dim);
1401 template <
class BBOX>
1402 bool kdtree_get_bbox(BBOX &bb)
const {