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
95 inline void addPoint(DistanceType dist, IndexType index)
98 for (i=
count; i>0; --i) {
99#ifdef NANOFLANN_FIRST_MATCH
102 if (
dists[i-1]>dist) {
128 template <
typename DistanceType,
typename IndexType =
size_t>
136 inline RadiusResultSet(DistanceType radius_, std::vector<std::pair<IndexType,DistanceType> >& indices_dists) : radius(radius_), m_indices_dists(indices_dists)
143 inline void init() { clear(); }
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; }
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);
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");
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; }
238 template<>
inline int abs<int>(
int x) { return ::abs(x); }
239 template<>
inline float abs<float>(
float x) {
return fabsf(x); }
240 template<>
inline double abs<double>(
double x) {
return fabs(x); }
248 template<
class T,
class DataSource,
typename _DistanceType = T>
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>
296 template<
class T,
class DataSource,
typename _DistanceType = T>
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>
345 template<
class T,
class DataSource,
typename _DistanceType = T>
356 return data_source.kdtree_distance(a,b_idx,size);
359 template <
typename U,
typename V>
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_)
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);
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>
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)
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; }
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>
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 );
908 return static_cast<size_t>(DIM>0 ? DIM : dim);
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);
943 DistanceType distsq = computeInitialDistances(vec, dists);
944 searchLevel(result, vec, root_node, distsq, dists, epsError);
956 resultSet.
init(out_indices, out_distances_sq);
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;
997 return dataset.kdtree_get_pt(idx,component);
1004 if (tree->
child1!=NULL) {
1005 save_tree(stream, tree->
child1);
1007 if (tree->
child2!=NULL) {
1008 save_tree(stream, tree->
child2);
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) {
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);
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;
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;
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;
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>
1233 const IndexType index = vind[i];
1234 DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim));
1235 if (dist<worst_dist) {
1236 result_set.addPoint(dist,vind[i]);
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);
1286 save_tree(stream, root_node);
1300 load_tree(stream, root_node);
1325 template <
class MatrixType,
int DIM = -1,
class Distance =
nanoflann::metric_L2,
typename IndexType =
size_t>
1329 typedef typename MatrixType::Scalar
num_t;
1330 typedef typename Distance::template traits<num_t,self_t>::distance_t
metric_t;
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");
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);
1379 return m_data_matrix.rows();
1386 for (
size_t i=0; i<size; i++) {
1387 const num_t d= p1[i]-m_data_matrix.coeff(idx_p2,i);
1395 return m_data_matrix.coeff(idx,dim);
1401 template <
class BBOX>
A STL container (as wrapper) for arrays of constant size defined at compile time (class imported from...
static void rangecheck(size_type i)
void assign(const size_t n, const T &value)
const_reference front() const
const_reference operator[](size_type i) const
void swap(CArray< T, N > &y)
reference operator[](size_type i)
const_reference at(size_type i) const
reference at(size_type i)
std::ptrdiff_t difference_type
void resize(const size_t nElements)
This method has no effects in this class, but raises an exception if the expected size does not match...
const_reference back() const
static size_type max_size()
const_reverse_iterator rend() const
std::reverse_iterator< iterator > reverse_iterator
const_reverse_iterator rbegin() const
const T & const_reference
const_iterator begin() const
void assign(const T &value)
reverse_iterator rbegin()
std::reverse_iterator< const_iterator > const_reverse_iterator
const_iterator end() const
size_t size() const
Returns size of index.
const KDTreeSingleIndexAdaptorParams index_params
KDTreeSingleIndexAdaptor(const KDTreeSingleIndexAdaptor< Distance, DatasetAdaptor, DIM, IndexType > &)
Hidden copy constructor, to disallow copying indices (Not implemented)
~KDTreeSingleIndexAdaptor()
Standard destructor.
int dim
Dimensionality of each data point.
NodePtr root_node
Array of k-d trees used to find neighbours.
void saveIndex(FILE *stream)
Stores the index in a binary file.
Distance::ElementType ElementType
void knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq, const int=10) const
Find the "num_closest" nearest neighbors to the query_point[0:dim-1].
void planeSplit(IndexType *ind, const IndexType count, int cutfeat, DistanceType cutval, IndexType &lim1, IndexType &lim2)
Subdivide the list of points by a plane perpendicular on axe corresponding to the 'cutfeat' dimension...
size_t usedMemory() const
Computes the inde memory usage Returns: memory used by the index.
void save_tree(FILE *stream, NodePtr tree)
size_t veclen() const
Returns the length of an index feature.
KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor &inputData, const KDTreeSingleIndexAdaptorParams ¶ms=KDTreeSingleIndexAdaptorParams())
KDTree constructor.
void computeMinMax(IndexType *ind, IndexType count, int element, ElementType &min_elem, ElementType &max_elem)
void searchLevel(RESULTSET &result_set, const ElementType *vec, const NodePtr node, DistanceType mindistsq, distance_vector_t &dists, const float epsError) const
Performs an exact search in the tree starting from a node.
void middleSplit_(IndexType *ind, IndexType count, IndexType &index, int &cutfeat, DistanceType &cutval, const BoundingBox &bbox)
std::vector< IndexType > vind
Array of indices to vectors in the dataset.
void freeIndex()
Frees the previously-built index.
NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox &bbox)
Create a tree node that subdivides the list of vecs from vind[first] to vind[last].
void init_vind()
Make sure the auxiliary list vind has the same size than the current dataset, and re-generate if size...
const DatasetAdaptor & dataset
The dataset used by this index.
size_t radiusSearch(const ElementType *query_point, const DistanceType radius, std::vector< std::pair< IndexType, DistanceType > > &IndicesDists, const SearchParams &searchParams) const
Find all the neighbors to query_point[0:dim-1] within a maximum radius.
ElementType dataset_get(size_t idx, int component) const
Helper accessor to the dataset points:
DistanceType computeInitialDistances(const ElementType *vec, distance_vector_t &dists) const
void loadIndex(FILE *stream)
Loads a previous index from a binary file.
BranchStruct< NodePtr, DistanceType > BranchSt
Distance::DistanceType DistanceType
void buildIndex()
Builds the index.
PooledAllocator pool
Pooled memory allocator.
void computeBoundingBox(BoundingBox &bbox)
array_or_vector_selector< DIM, Interval >::container_t BoundingBox
Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM".
void findNeighbors(RESULTSET &result, const ElementType *vec, const SearchParams &searchParams) const
Find set of nearest neighbors to vec[0:dim-1].
array_or_vector_selector< DIM, DistanceType >::container_t distance_vector_t
Define "distance_vector_t" as a fixed-size or variable-size container depending on "DIM".
void load_tree(FILE *stream, NodePtr &tree)
void init(IndexType *indices_, DistanceType *dists_)
void addPoint(DistanceType dist, IndexType index)
KNNResultSet(CountType capacity_)
DistanceType worstDist() const
PooledAllocator(const size_t blocksize_=BLOCKSIZE)
Default constructor.
~PooledAllocator()
Destructor.
void free_all()
Frees all allocated memory chunks.
void * malloc(const size_t req_size)
Returns a pointer to a piece of new memory of the given size in bytes allocated from the pool.
T * allocate(const size_t count=1)
Allocates (using this pool) a generic type T.
A result-set class used when performing a radius based search.
std::pair< IndexType, DistanceType > worst_item() const
Find the worst result (furtherest neighbor) without copying or sorting Pre-conditions: size() > 0.
void addPoint(DistanceType dist, IndexType index)
const DistanceType radius
DistanceType worstDist() const
RadiusResultSet(DistanceType radius_, std::vector< std::pair< IndexType, DistanceType > > &indices_dists)
std::vector< std::pair< IndexType, DistanceType > > & m_indices_dists
void set_radius_and_clear(const DistanceType r)
Clears the result set and adjusts the search radius.
const Scalar * const_iterator
EIGEN_STRONG_INLINE iterator begin()
EIGEN_STRONG_INLINE iterator end()
void load_value(FILE *stream, T &value, size_t count=1)
void save_value(FILE *stream, const T &value, size_t count=1)
const size_t WORDSIZE
Pooled storage allocator.
T * allocate(size_t count=1)
Allocates (using C's malloc) a generic type T.
long double abs< long double >(long double x)
float abs< float >(float x)
double abs< double >(double x)
operator "<" for std::sort()
bool operator()(const PairType &p1, const PairType &p2) const
PairType will be typically: std::pair<IndexType,DistanceType>
An L2-metric KD-tree adaptor for working with data directly stored in an Eigen Matrix,...
const MatrixType & m_data_matrix
void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int=10) const
Query for the num_closest closest points to a given point (entered as query_point[0:dim-1]).
const self_t & derived() const
bool kdtree_get_bbox(BBOX &bb) const
num_t kdtree_get_pt(const size_t idx, int dim) const
KDTreeEigenMatrixAdaptor(const self_t &)
Hidden copy constructor, to disallow copying this class (Not implemented)
KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size=10)
The kd-tree index for the user to call its methods as usual with any other FLANN index.
Distance::template traits< num_t, self_t >::distance_t metric_t
~KDTreeEigenMatrixAdaptor()
size_t kdtree_get_point_count() const
KDTreeSingleIndexAdaptor< metric_t, self_t, DIM, IndexType > index_t
KDTreeEigenMatrixAdaptor< MatrixType, DIM, Distance, IndexType > self_t
num_t kdtree_distance(const num_t *p1, const size_t idx_p2, size_t size) const
This record represents a branch point when finding neighbors in the tree.
bool operator<(const BranchStruct< T, DistanceType > &rhs) const
BranchStruct(const T &aNode, DistanceType dist)
IndexType left
Indices of points in leaf node.
DistanceType divlow
The values used for subdivision.
struct nanoflann::KDTreeSingleIndexAdaptor::Node::@10::@13 sub
struct nanoflann::KDTreeSingleIndexAdaptor::Node::@10::@12 lr
Node * child1
The child nodes.
int divfeat
Dimension used for subdivision.
Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size=10, int dim_=-1)
Manhattan distance functor (generic version, optimized for high-dimensionality data sets).
_DistanceType DistanceType
L1_Adaptor(const DataSource &_data_source)
DistanceType operator()(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist=-1) const
DistanceType accum_dist(const U a, const V b, int) const
const DataSource & data_source
Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets).
_DistanceType DistanceType
DistanceType operator()(const T *a, const size_t b_idx, size_t size, DistanceType worst_dist=-1) const
L2_Adaptor(const DataSource &_data_source)
DistanceType accum_dist(const U a, const V b, int) const
const DataSource & data_source
Squared Euclidean (L2) distance functor (suitable for low-dimensionality datasets,...
DistanceType accum_dist(const U a, const V b, int) const
_DistanceType DistanceType
const DataSource & data_source
L2_Simple_Adaptor(const DataSource &_data_source)
DistanceType operator()(const T *a, const size_t b_idx, size_t size) const
Search options for KDTreeSingleIndexAdaptor::findNeighbors()
bool sorted
only for radius search, require neighbours sorted by distance (default: true)
SearchParams(int checks_IGNORED_=32, float eps_=0, bool sorted_=true)
Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN inte...
float eps
search for eps-approximate neighbours (default: 0)
int checks
Ignored parameter (Kept for compatibility with the FLANN interface).
std::vector< T > container_t
Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors when DIM=-1.
CArray< T, DIM > container_t
L1_Adaptor< T, DataSource > distance_t
Metaprogramming helper traits class for the L1 (Manhattan) metric.
L2_Adaptor< T, DataSource > distance_t
L2_Simple_Adaptor< T, DataSource > distance_t
Metaprogramming helper traits class for the L2_simple (Euclidean) metric.
Metaprogramming helper traits class for the L2 (Euclidean) metric.