text: /***********************************************************************
   * Software License Agreement (BSD License)
   *
   * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
   * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
   * Copyright 2011 Jose Luis Blanco (joseluisblancoc@gmail.com).
   *   All rights reserved.
   *
   * THE BSD LICENSE
   *
   * Redistribution and use in source and binary forms, with or without
   * modification, are permitted provided that the following conditions
   * are met:
   *
   * 1. Redistributions of source code must retain the above copyright
   *    notice, this list of conditions and the following disclaimer.
   * 2. Redistributions in binary form must reproduce the above copyright
   *    notice, this list of conditions and the following disclaimer in the
   *    documentation and/or other materials provided with the distribution.
   *
   * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
   * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
   * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
   * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
   * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
   * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
   * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
   * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
   * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
   * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   *************************************************************************/
  
  #ifndef  NANOFLANN_HPP_
  #define  NANOFLANN_HPP_
  
  #include <vector>
  #include <cassert>
  #include <algorithm>
  #include <stdexcept>
  #include <cstdio>  // for fwrite()
  #include <cmath>   // for fabs(),...
  #include <limits>
  
  // Avoid conflicting declaration of min/max macros in windows headers
  #if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_)  || defined(WIN32) || defined(_WIN64))
  # define NOMINMAX
  # ifdef max
  #  undef   max
  #  undef   min
  # endif
  #endif
  
  namespace nanoflann
  {
  /** @addtogroup nanoflann_grp nanoflann C++ library for ANN
    *  @{ */
  
    	/** Library version: 0xMmP (M=Major,m=minor,P=path) */
  	#define NANOFLANN_VERSION 0x113
  
  	/** @addtogroup result_sets_grp Result set classes
  	  *  @{ */
  	template <typename DistanceType, typename IndexType = size_t, typename CountType = size_t>
  	class KNNResultSet
  	{
  		IndexType * indices;
  		DistanceType* dists;
  		CountType capacity;
  		CountType count;
  
  	public:
  		inline KNNResultSet(CountType capacity_) : capacity(capacity_), count(0)
  		{
  		}
  
  		inline void init(IndexType* indices_, DistanceType* dists_)
  		{
  			indices = indices_;
  			dists = dists_;
  			count = 0;
  			dists[capacity-1] = (std::numeric_limits<DistanceType>::max)();
  		}
  
  		inline CountType size() const
  		{
  			return count;
  		}
  
  		inline bool full() const
  		{
  			return count == capacity;
  		}
  
  
  		inline void addPoint(DistanceType dist, IndexType index)
  		{
  			CountType i;
  			for (i=count; i>0; --i) {
  #ifdef NANOFLANN_FIRST_MATCH   // If defined and two poins have the same distance, the one with the lowest-index will be returned first.
  				if ( (dists[i-1]>dist) || ((dist==dists[i-1])&&(indices[i-1]>index)) ) {
  #else
  				if (dists[i-1]>dist) {
  #endif
  					if (i<capacity) {
  						dists[i] = dists[i-1];
  						indices[i] = indices[i-1];
  					}
  				}
  				else break;
  			}
  			if (i<capacity) {
  				dists[i] = dist;
  				indices[i] = index;
  			}
  			if (count<capacity) count++;
  		}
  
  		inline DistanceType worstDist() const
  		{
  			return dists[capacity-1];
  		}
  	};
  
  
  	/**
  	 * A result-set class used when performing a radius based search.
  	 */
  	template <typename DistanceType, typename IndexType = size_t>
  	class RadiusResultSet
  	{
  	public:
  		const DistanceType radius;
  
  		std::vector<std::pair<IndexType,DistanceType> >& m_indices_dists;
  
  		inline RadiusResultSet(DistanceType radius_, std::vector<std::pair<IndexType,DistanceType> >& indices_dists) : radius(radius_), m_indices_dists(indices_dists)
  		{
  			init();
  		}
  
  		inline ~RadiusResultSet() { }
  
  		inline void init() { clear(); }
  		inline void clear() { m_indices_dists.clear(); }
  
  		inline size_t size() const { return m_indices_dists.size(); }
  
  		inline bool full() const { return true; }
  
  		inline void addPoint(DistanceType dist, IndexType index)
  		{
  			if (dist<radius)
  				m_indices_dists.push_back(std::make_pair<IndexType,DistanceType>(index,dist));
  		}
  
  		inline DistanceType worstDist() const { return radius; }
  
  		/** Clears the result set and adjusts the search radius. */
  		inline void set_radius_and_clear( const DistanceType r )
  		{
  			radius = r;
  			clear();
  		}
  
  		/**
  		 * Find the worst result (furtherest neighbor) without copying or sorting
  		 * Pre-conditions: size() > 0
  		 */
  		std::pair<IndexType,DistanceType> worst_item() const
  		{
  		   if (m_indices_dists.empty()) throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
  		   typedef typename std::vector<std::pair<IndexType,DistanceType> >::const_iterator DistIt;
  		   DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end());
  		   return *it;
  		}
  	};
  
  	/** operator "<" for std::sort() */
  	struct IndexDist_Sorter
  	{
  		/** PairType will be typically: std::pair<IndexType,DistanceType> */
  		template <typename PairType>
  		inline bool operator()(const PairType &p1, const PairType &p2) const {
  			return p1.second < p2.second;
  		}
  	};
  
  	/** @} */
  
  
  	/** @addtogroup loadsave_grp Load/save auxiliary functions
  	  * @{ */
  	template<typename T>
  	void save_value(FILE* stream, const T& value, size_t count = 1)
  	{
  		fwrite(&value, sizeof(value),count, stream);
  	}
  
  	template<typename T>
  	void save_value(FILE* stream, const std::vector<T>& value)
  	{
  		size_t size = value.size();
  		fwrite(&size, sizeof(size_t), 1, stream);
  		fwrite(&value[0], sizeof(T), size, stream);
  	}
  
  	template<typename T>
  	void load_value(FILE* stream, T& value, size_t count = 1)
  	{
  		size_t read_cnt = fread(&value, sizeof(value), count, stream);
  		if (read_cnt != count) {
  			throw std::runtime_error("Cannot read from file");
  		}
  	}
  
  
  	template<typename T>
  	void load_value(FILE* stream, std::vector<T>& value)
  	{
  		size_t size;
  		size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
  		if (read_cnt!=1) {
  			throw std::runtime_error("Cannot read from file");
  		}
  		value.resize(size);
  		read_cnt = fread(&value[0], sizeof(T), size, stream);
  		if (read_cnt!=size) {
  			throw std::runtime_error("Cannot read from file");
  		}
  	}
  	/** @} */
  
  
  	/** @addtogroup metric_grp Metric (distance) classes
  	  * @{ */
  
  	template<typename T> inline T abs(T x) { return (x<0) ? -x : x; }
  	template<> inline int abs<int>(int x) { return ::abs(x); }
  	template<> inline float abs<float>(float x) { return fabsf(x); }
  	template<> inline double abs<double>(double x) { return fabs(x); }
  	template<> inline long double abs<long double>(long double x) { return fabsl(x); }
  
  	/** Manhattan distance functor (generic version, optimized for high-dimensionality data sets).
  	  *  Corresponding distance traits: nanoflann::metric_L1
  	  * \tparam T Type of the elements (e.g. double, float, uint8_t)
  	  * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
  	  */
  	template<class T, class DataSource, typename _DistanceType = T>
  	struct L1_Adaptor
  	{
  		typedef T ElementType;
  		typedef _DistanceType DistanceType;
  
  		const DataSource &data_source;
  
  		L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
  
  		inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
  		{
  			DistanceType result = DistanceType();
  			const T* last = a + size;
  			const T* lastgroup = last - 3;
  			size_t d = 0;
  
  			/* Process 4 items with each loop for efficiency. */
  			while (a < lastgroup) {
  				const DistanceType diff0 = nanoflann::abs(a[0] - data_source.kdtree_get_pt(b_idx,d++));
  				const DistanceType diff1 = nanoflann::abs(a[1] - data_source.kdtree_get_pt(b_idx,d++));
  				const DistanceType diff2 = nanoflann::abs(a[2] - data_source.kdtree_get_pt(b_idx,d++));
  				const DistanceType diff3 = nanoflann::abs(a[3] - data_source.kdtree_get_pt(b_idx,d++));
  				result += diff0 + diff1 + diff2 + diff3;
  				a += 4;
  				if ((worst_dist>0)&&(result>worst_dist)) {
  					return result;
  				}
  			}
  			/* Process last 0-3 components.  Not needed for standard vector lengths. */
  			while (a < last) {
  				result += nanoflann::abs( *a++ - data_source.kdtree_get_pt(b_idx,d++) );
  			}
  			return result;
  		}
  
  		template <typename U, typename V>
  		inline DistanceType accum_dist(const U a, const V b, int dim) const
  		{
  			return (a-b)*(a-b);
  		}
  	};
  
  	/** Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets).
  	  *  Corresponding distance traits: nanoflann::metric_L2
  	  * \tparam T Type of the elements (e.g. double, float, uint8_t)
  	  * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
  	  */
  	template<class T, class DataSource, typename _DistanceType = T>
  	struct L2_Adaptor
  	{
  		typedef T ElementType;
  		typedef _DistanceType DistanceType;
  
  		const DataSource &data_source;
  
  		L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
  
  		inline DistanceType operator()(const T* a, const size_t b_idx, size_t size, DistanceType worst_dist = -1) const
  		{
  			DistanceType result = DistanceType();
  			const T* last = a + size;
  			const T* lastgroup = last - 3;
  			size_t d = 0;
  
  			/* Process 4 items with each loop for efficiency. */
  			while (a < lastgroup) {
  				const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx,d++);
  				const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx,d++);
  				const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx,d++);
  				const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx,d++);
  				result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
  				a += 4;
  				if ((worst_dist>0)&&(result>worst_dist)) {
  					return result;
  				}
  			}
  			/* Process last 0-3 components.  Not needed for standard vector lengths. */
  			while (a < last) {
  				const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx,d++);
  				result += diff0 * diff0;
  			}
  			return result;
  		}
  
  		template <typename U, typename V>
  		inline DistanceType accum_dist(const U a, const V b, int dim) const
  		{
  			return (a-b)*(a-b);
  		}
  	};
  
  	/** Squared Euclidean distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds)
  	  *  Corresponding distance traits: nanoflann::metric_L2_Simple
  	  * \tparam T Type of the elements (e.g. double, float, uint8_t)
  	  * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
  	  */
  	template<class T, class DataSource, typename _DistanceType = T>
  	struct L2_Simple_Adaptor
  	{
  		typedef T ElementType;
  		typedef _DistanceType DistanceType;
  
  		const DataSource &data_source;
  
  		L2_Simple_Adaptor(const DataSource &_data_source) : data_source(_data_source) { }
  
  		inline DistanceType operator()(const T* a, const size_t b_idx, size_t size) const {
  			return data_source.kdtree_distance(a,b_idx,size);
  		}
  
  		template <typename U, typename V>
  		inline DistanceType accum_dist(const U a, const V b, int dim) const
  		{
              dim = dim;
  			return (a-b)*(a-b);
  		}
  	};
  
  	/** Metaprogramming helper traits class for the L1 (Manhattan) metric */
  	struct metric_L1 {
  		template<class T, class DataSource>
  		struct traits {
  			typedef L1_Adaptor<T,DataSource> distance_t;
  		};
  	};
  	/** Metaprogramming helper traits class for the L2 (Euclidean) metric */
  	struct metric_L2 {
  		template<class T, class DataSource>
  		struct traits {
  			typedef L2_Adaptor<T,DataSource> distance_t;
  		};
  	};
  	/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */
  	struct metric_L2_Simple {
  		template<class T, class DataSource>
  		struct traits {
  			typedef L2_Simple_Adaptor<T,DataSource> distance_t;
  		};
  	};
  
  	/** @} */
  
  
  
  	/** @addtogroup param_grp Parameter structs
  	  * @{ */
  
  	/**  Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
  	  */
  	struct KDTreeSingleIndexAdaptorParams
  	{
  		KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10, int dim_ = -1) :
  			leaf_max_size(_leaf_max_size), dim(dim_)
  		{}
  
  		size_t leaf_max_size;
  		int dim;
  	};
  
  	/** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */
  	struct SearchParams
  	{
  		/** Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN interface */
  		SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true ) :
              eps(eps_), sorted(sorted_) {
              checks_IGNORED_ = checks_IGNORED_;
          }
  
  		int   checks;  //!< Ignored parameter (Kept for compatibility with the FLANN interface).
  		float eps;  //!< search for eps-approximate neighbours (default: 0)
  		bool sorted; //!< only for radius search, require neighbours sorted by distance (default: true)
  	};
  	/** @} */
  
  
  	/** @addtogroup memalloc_grp Memory allocation
  	  * @{ */
  
  	/**
  	 * Allocates (using C's malloc) a generic type T.
  	 *
  	 * Params:
  	 *     count = number of instances to allocate.
  	 * Returns: pointer (of type T*) to memory buffer
  	 */
  	template <typename T>
  	inline T* allocate(size_t count = 1)
  	{
  		T* mem = (T*) ::malloc(sizeof(T)*count);
  		return mem;
  	}
  
  
  	/**
  	 * Pooled storage allocator
  	 *
  	 * The following routines allow for the efficient allocation of storage in
  	 * small chunks from a specified pool.  Rather than allowing each structure
  	 * to be freed individually, an entire pool of storage is freed at once.
  	 * This method has two advantages over just using malloc() and free().  First,
  	 * it is far more efficient for allocating small objects, as there is
  	 * no overhead for remembering all the information needed to free each
  	 * object or consolidating fragmented memory.  Second, the decision about
  	 * how long to keep an object is made at the time of allocation, and there
  	 * is no need to track down all the objects to free them.
  	 *
  	 */
  
  	const size_t     WORDSIZE=16;
  	const size_t     BLOCKSIZE=8192;
  
  	class PooledAllocator
  	{
  		/* We maintain memory alignment to word boundaries by requiring that all
  		    allocations be in multiples of the machine wordsize.  */
  		/* Size of machine word in bytes.  Must be power of 2. */
  		/* Minimum number of bytes requested at a time from	the system.  Must be multiple of WORDSIZE. */
  
  
  		size_t  remaining;  /* Number of bytes left in current block of storage. */
  		void*   base;     /* Pointer to base of current block of storage. */
  		void*   loc;      /* Current location in block to next allocate memory. */
  		size_t  blocksize;
  
  
  	public:
  		size_t  usedMemory;
  		size_t  wastedMemory;
  
  		/**
  		    Default constructor. Initializes a new pool.
  		 */
  		PooledAllocator(const size_t blocksize = BLOCKSIZE)
  		{
  			this->blocksize = blocksize;
  			remaining = 0;
  			base = NULL;
  
  			usedMemory = 0;
  			wastedMemory = 0;
  		}
  
  		/**
  		 * Destructor. Frees all the memory allocated in this pool.
  		 */
  		~PooledAllocator()
  		{
  			while (base != NULL) {
  				void *prev = *((void**) base); /* Get pointer to prev block. */
  				::free(base);
  				base = prev;
  			}
  		}
  
  		/**
  		 * Returns a pointer to a piece of new memory of the given size in bytes
  		 * allocated from the pool.
  		 */
  		void* malloc(const size_t req_size)
  		{
  			/* Round size up to a multiple of wordsize.  The following expression
  			    only works for WORDSIZE that is a power of 2, by masking last bits of
  			    incremented size to zero.
  			 */
  			const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
  
  			/* Check whether a new block must be allocated.  Note that the first word
  			    of a block is reserved for a pointer to the previous block.
  			 */
  			if (size > remaining) {
  
  				wastedMemory += remaining;
  
  				/* Allocate new storage. */
  				const size_t blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ?
  							size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE;
  
  				// use the standard C malloc to allocate memory
  				void* m = ::malloc(blocksize);
  				if (!m) {
  					fprintf(stderr,"Failed to allocate memory.\n");
  					return NULL;
  				}
  
  				/* Fill first word of new block with pointer to previous block. */
  				((void**) m)[0] = base;
  				base = m;
  
  				size_t shift = 0;
  				//int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);
  
  				remaining = blocksize - sizeof(void*) - shift;
  				loc = ((char*)m + sizeof(void*) + shift);
  			}
  			void* rloc = loc;
  			loc = (char*)loc + size;
  			remaining -= size;
  
  			usedMemory += size;
  
  			return rloc;
  		}
  
  		/**
  		 * Allocates (using this pool) a generic type T.
  		 *
  		 * Params:
  		 *     count = number of instances to allocate.
  		 * Returns: pointer (of type T*) to memory buffer
  		 */
  		template <typename T>
  		T* allocate(const size_t count = 1)
  		{
  			T* mem = (T*) this->malloc(sizeof(T)*count);
  			return mem;
  		}
  
  	};
  	/** @} */
  
  
  	/** @addtogroup kdtrees_grp KD-tree classes and adaptors
  	  * @{ */
  
  	/** kd-tree index
  	 *
  	 * Contains the k-d trees and other information for indexing a set of points
  	 * for nearest-neighbor matching.
  	 *
  	 *  The class "DatasetAdaptor" must provide the following interface (can be non-virtual, inlined methods):
  	 *
  	 *  \code
  	 *   // Must return the number of data points
  	 *   inline size_t kdtree_get_point_count() const { ... }
  	 *
  	 *   // Must return the Euclidean (L2) distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
  	 *   inline DistanceType kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const { ... }
  	 *
  	 *   // Must return the dim'th component of the idx'th point in the class:
  	 *   inline T kdtree_get_pt(const size_t idx, int dim) const { ... }
  	 *
  	 *   // Optional bounding-box computation: return false to default to a standard bbox computation loop.
  	 *   //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
  	 *   //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
  	 *   template <class BBOX>
  	 *   bool kdtree_get_bbox(BBOX &bb) const
  	 *   {
  	 *      bb[0].low = ...; bb[0].high = ...;  // 0th dimension limits
  	 *      bb[1].low = ...; bb[1].high = ...;  // 1st dimension limits
  	 *      ...
  	 *      return true;
  	 *   }
  	 *
  	 *  \endcode
  	 *
  	 * \tparam IndexType Will be typically size_t or int
  	 */
  	template <typename Distance, class DatasetAdaptor,int DIM = -1, typename IndexType = size_t>
  	class KDTreeSingleIndexAdaptor
  	{
  	public:
  		typedef typename Distance::ElementType  ElementType;
  		typedef typename Distance::DistanceType DistanceType;
  	protected:
  
  		/**
  		 *  Array of indices to vectors in the dataset.
  		 */
  		std::vector<IndexType> vind;
  
  		size_t m_leaf_max_size;
  
  
  		/**
  		 * The dataset used by this index
  		 */
  		const DatasetAdaptor &dataset; //!< The source of our data
  
  		const KDTreeSingleIndexAdaptorParams index_params;
  
  		size_t m_size;
  		int dim;  //!< Dimensionality of each data point
  
  
  		/*--------------------- Internal Data Structures --------------------------*/
  		struct Node
  		{
  			union {
  				struct
  				{
  					/**
  					 * Indices of points in leaf node
  					 */
  					IndexType left, right;
  				} lr;
  				struct
  				{
  					/**
  					 * Dimension used for subdivision.
  					 */
  					int divfeat;
  					/**
  					 * The values used for subdivision.
  					 */
  					DistanceType divlow, divhigh;
  				} sub;
  			};
  			/**
  			 * The child nodes.
  			 */
  			Node* child1, * child2;
  		};
  		typedef Node* NodePtr;
  
  
  		struct Interval
  		{
  			ElementType low, high;
  		};
  
  		typedef std::vector<Interval> BoundingBox;
  
  
  		/** This record represents a branch point when finding neighbors in
  			the tree.  It contains a record of the minimum distance to the query
  			point, as well as the node at which the search resumes.
  		 */
  		template <typename T, typename DistanceType>
  		struct BranchStruct
  		{
  			T node;           /* Tree node at which search resumes */
  			DistanceType mindist;     /* Minimum distance to query for all nodes below. */
  
  			BranchStruct() {}
  			BranchStruct(const T& aNode, DistanceType dist) : node(aNode), mindist(dist) {}
  
  			inline bool operator<(const BranchStruct<T, DistanceType>& rhs) const
  			{
  				return mindist<rhs.mindist;
  			}
  		};
  
  		/**
  		 * Array of k-d trees used to find neighbours.
  		 */
  		NodePtr root_node;
  		typedef BranchStruct<NodePtr, DistanceType> BranchSt;
  		typedef BranchSt* Branch;
  
  		BoundingBox root_bbox;
  
  		/**
  		 * Pooled memory allocator.
  		 *
  		 * Using a pooled memory allocator is more efficient
  		 * than allocating memory directly when there is a large
  		 * number small of memory allocations.
  		 */
  		PooledAllocator pool;
  
  	public:
  
  		Distance distance;
  
  		/**
  		 * KDTree constructor
  		 *
  		 * Params:
  		 *          inputData = dataset with the input features
  		 *          params = parameters passed to the kdtree algorithm (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
  		 */
  		KDTreeSingleIndexAdaptor(const int dimensionality, const DatasetAdaptor& inputData, const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams() ) :
  			dataset(inputData), index_params(params), distance(inputData)
  		{
  			m_size = dataset.kdtree_get_point_count();
  			dim = dimensionality;
  			if (DIM>0) dim=DIM;
  			else {
  				if (params.dim>0) dim = params.dim;
  			}
  			m_leaf_max_size = params.leaf_max_size;
  
  			// Create a permutable array of indices to the input vectors.
  			init_vind();
  		}
  
  		/**
  		 * Standard destructor
  		 */
  		~KDTreeSingleIndexAdaptor()
  		{
  		}
  
  		/**
  		 * Builds the index
  		 */
  		void buildIndex()
  		{
  			init_vind();
  			computeBoundingBox(root_bbox);
  			root_node = divideTree(0, m_size, root_bbox );   // construct the tree
  		}
  
  		/**
  		 *  Returns size of index.
  		 */
  		size_t size() const
  		{
  			return m_size;
  		}
  
  		/**
  		 * Returns the length of an index feature.
  		 */
  		size_t veclen() const
  		{
  			return static_cast<size_t>(DIM>0 ? DIM : dim);
  		}
  
  		/**
  		 * Computes the inde memory usage
  		 * Returns: memory used by the index
  		 */
  		size_t usedMemory() const
  		{
  			return pool.usedMemory+pool.wastedMemory+dataset.kdtree_get_point_count()*sizeof(IndexType);  // pool memory and vind array memory
  		}
  
  		/** \name Query methods
  		  * @{ */
  
  		/**
  		 * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored inside
  		 * the result object.
  		 *
  		 * Params:
  		 *     result = the result object in which the indices of the nearest-neighbors are stored
  		 *     vec = the vector for which to search the nearest neighbors
  		 *
  		 * \tparam RESULTSET Should be any ResultSet<DistanceType>
  		 * \sa knnSearch, radiusSearch
  		 */
  		template <typename RESULTSET>
  		void findNeighbors(RESULTSET& result, const ElementType* vec, const SearchParams& searchParams) const
  		{
  			assert(vec);
  			float epsError = 1+searchParams.eps;
  
  			std::vector<DistanceType> dists( (DIM>0 ? DIM : dim) ,0);
  			DistanceType distsq = computeInitialDistances(vec, dists);
  			searchLevel(result, vec, root_node, distsq, dists, epsError);  // "count_leaf" parameter removed since was neither used nor returned to the user.
  		}
  
  		/**
  		 * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. Their indices are stored inside
  		 * the result object.
  		 *  \sa radiusSearch, findNeighbors
  		 * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
  		 */
          inline void knnSearch(const ElementType *query_point, const size_t num_closest, IndexType *out_indices, DistanceType *out_distances_sq) const
  		{
  			nanoflann::KNNResultSet<DistanceType,IndexType> resultSet(num_closest);
  			resultSet.init(out_indices, out_distances_sq);
  			this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
  		}
  
  		/**
  		 * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
  		 *  The output is given as a vector of pairs, of which the first element is a point index and the second the corresponding distance.
  		 *  Previous contents of \a IndicesDists are cleared.
  		 *
  		 *  If searchParams.sorted==true, the output list is sorted by ascending distances.
  		 *
  		 *  For a better performance, it is advisable to do a .reserve() on the vector if you have any wild guess about the number of expected matches.
  		 *
  		 *  \sa knnSearch, findNeighbors
  		 * \return The number of points within the given radius (i.e. indices.size() or dists.size() )
  		 */
  		size_t radiusSearch(const ElementType *query_point,const DistanceType radius, std::vector<std::pair<IndexType,DistanceType> >& IndicesDists, const SearchParams& searchParams) const
  		{
  			RadiusResultSet<DistanceType,IndexType> resultSet(radius,IndicesDists);
  			this->findNeighbors(resultSet, query_point, searchParams);
  
  			if (searchParams.sorted)
  				std::sort(IndicesDists.begin(),IndicesDists.end(), IndexDist_Sorter() );
  
  			return resultSet.size();
  		}
  
  		/** @} */
  
  	private:
  		/** Make sure the auxiliary list \a vind has the same size than the current dataset, and re-generate if size has changed. */
  		void init_vind()
  		{
  			// Create a permutable array of indices to the input vectors.
  			m_size = dataset.kdtree_get_point_count();
  			if (vind.size()!=m_size)
  			{
  				vind.resize(m_size);
  				for (size_t i = 0; i < m_size; i++) vind[i] = i;
  			}
  		}
  
  		/// Helper accessor to the dataset points:
  		inline ElementType dataset_get(size_t idx, int component) const {
  			return dataset.kdtree_get_pt(idx,component);
  		}
  
  
  		void save_tree(FILE* stream, NodePtr tree)
  		{
  			save_value(stream, *tree);
  			if (tree->child1!=NULL) {
  				save_tree(stream, tree->child1);
  			}
  			if (tree->child2!=NULL) {
  				save_tree(stream, tree->child2);
  			}
  		}
  
  
  		void load_tree(FILE* stream, NodePtr& tree)
  		{
  			tree = pool.allocate<Node>();
  			load_value(stream, *tree);
  			if (tree->child1!=NULL) {
  				load_tree(stream, tree->child1);
  			}
  			if (tree->child2!=NULL) {
  				load_tree(stream, tree->child2);
  			}
  		}
  
  
  		void computeBoundingBox(BoundingBox& bbox)
  		{
  			bbox.resize((DIM>0 ? DIM : dim));
  			if (dataset.kdtree_get_bbox(bbox))
  			{
  				// Done! It was implemented in derived class
  			}
  			else
  			{
  				for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
  					bbox[i].low =
  						bbox[i].high = dataset_get(0,i);
  				}
  				const size_t N = dataset.kdtree_get_point_count();
  				for (size_t k=1; k<N; ++k) {
  					for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
  						if (dataset_get(k,i)<bbox[i].low) bbox[i].low = dataset_get(k,i);
  						if (dataset_get(k,i)>bbox[i].high) bbox[i].high = dataset_get(k,i);
  					}
  				}
  			}
  		}
  
  
  		/**
  		 * Create a tree node that subdivides the list of vecs from vind[first]
  		 * to vind[last].  The routine is called recursively on each sublist.
  		 * Place a pointer to this new tree node in the location pTree.
  		 *
  		 * Params: pTree = the new node to create
  		 *                  first = index of the first vector
  		 *                  last = index of the last vector
  		 */
  		NodePtr divideTree(const IndexType left, const IndexType right, BoundingBox& bbox)
  		{
  			NodePtr node = pool.allocate<Node>(); // allocate memory
  
  			/* If too few exemplars remain, then make this a leaf node. */
  			if ( (right-left) <= m_leaf_max_size) {
  				node->child1 = node->child2 = NULL;    /* Mark as leaf node. */
  				node->lr.left = left;
  				node->lr.right = right;
  
  				// compute bounding-box of leaf points
  				for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
  					bbox[i].low = dataset_get(vind[left],i);
  					bbox[i].high = dataset_get(vind[left],i);
  				}
  				for (IndexType k=left+1; k<right; ++k) {
  					for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
  						if (bbox[i].low>dataset_get(vind[k],i)) bbox[i].low=dataset_get(vind[k],i);
  						if (bbox[i].high<dataset_get(vind[k],i)) bbox[i].high=dataset_get(vind[k],i);
  					}
  				}
  			}
  			else {
  				IndexType idx;
  				int cutfeat;
  				DistanceType cutval;
  				middleSplit_(&vind[0]+left, right-left, idx, cutfeat, cutval, bbox);
  
  				node->sub.divfeat = cutfeat;
  
  				BoundingBox left_bbox(bbox);
  				left_bbox[cutfeat].high = cutval;
  				node->child1 = divideTree(left, left+idx, left_bbox);
  
  				BoundingBox right_bbox(bbox);
  				right_bbox[cutfeat].low = cutval;
  				node->child2 = divideTree(left+idx, right, right_bbox);
  
  				node->sub.divlow = left_bbox[cutfeat].high;
  				node->sub.divhigh = right_bbox[cutfeat].low;
  
  				for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
  					bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
  					bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
  				}
  			}
  
  			return node;
  		}
  
  		void computeMinMax(IndexType* ind, IndexType count, int element, ElementType& min_elem, ElementType& max_elem)
  		{
  			min_elem = dataset_get(ind[0],element);
  			max_elem = dataset_get(ind[0],element);
  			for (IndexType i=1; i<count; ++i) {
  				ElementType val = dataset_get(ind[i],element);
  				if (val<min_elem) min_elem = val;
  				if (val>max_elem) max_elem = val;
  			}
  		}
  
  		void middleSplit(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
  		{
  			// find the largest span from the approximate bounding box
  			ElementType max_span = bbox[0].high-bbox[0].low;
  			cutfeat = 0;
  			cutval = (bbox[0].high+bbox[0].low)/2;
  			for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
  				ElementType span = bbox[i].low-bbox[i].low;
  				if (span>max_span) {
  					max_span = span;
  					cutfeat = i;
  					cutval = (bbox[i].high+bbox[i].low)/2;
  				}
  			}
  
  			// compute exact span on the found dimension
  			ElementType min_elem, max_elem;
  			computeMinMax(ind, count, cutfeat, min_elem, max_elem);
  			cutval = (min_elem+max_elem)/2;
  			max_span = max_elem - min_elem;
  
  			// check if a dimension of a largest span exists
  			size_t k = cutfeat;
  			for (size_t i=0; i<(DIM>0 ? DIM : dim); ++i) {
  				if (i==k) continue;
  				ElementType span = bbox[i].high-bbox[i].low;
  				if (span>max_span) {
  					computeMinMax(ind, count, i, min_elem, max_elem);
  					span = max_elem - min_elem;
  					if (span>max_span) {
  						max_span = span;
  						cutfeat = i;
  						cutval = (min_elem+max_elem)/2;
  					}
  				}
  			}
  			IndexType lim1, lim2;
  			planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
  
  			if (lim1>count/2) index = lim1;
  			else if (lim2<count/2) index = lim2;
  			else index = count/2;
  		}
  
  
  		void middleSplit_(IndexType* ind, IndexType count, IndexType& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
  		{
  			const DistanceType EPS=static_cast<DistanceType>(0.00001);
  			ElementType max_span = bbox[0].high-bbox[0].low;
  			for (int i=1; i<(DIM>0 ? DIM : dim); ++i) {
  				ElementType span = bbox[i].high-bbox[i].low;
  				if (span>max_span) {
  					max_span = span;
  				}
  			}
  			ElementType max_spread = -1;
  			cutfeat = 0;
  			for (int i=0; i<(DIM>0 ? DIM : dim); ++i) {
  				ElementType span = bbox[i].high-bbox[i].low;
  				if (span>(1-EPS)*max_span) {
  					ElementType min_elem, max_elem;
  					computeMinMax(ind, count, cutfeat, min_elem, max_elem);
  					ElementType spread = max_elem-min_elem;;
  					if (spread>max_spread) {
  						cutfeat = i;
  						max_spread = spread;
  					}
  				}
  			}
  			// split in the middle
  			DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
  			ElementType min_elem, max_elem;
  			computeMinMax(ind, count, cutfeat, min_elem, max_elem);
  
  			if (split_val<min_elem) cutval = min_elem;
  			else if (split_val>max_elem) cutval = max_elem;
  			else cutval = split_val;
  
  			IndexType lim1, lim2;
  			planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
  
  			if (lim1>count/2) index = lim1;
  			else if (lim2<count/2) index = lim2;
  			else index = count/2;
  		}
  
  
  		/**
  		 *  Subdivide the list of points by a plane perpendicular on axe corresponding
  		 *  to the 'cutfeat' dimension at 'cutval' position.
  		 *
  		 *  On return:
  		 *  dataset[ind[0..lim1-1]][cutfeat]<cutval
  		 *  dataset[ind[lim1..lim2-1]][cutfeat]==cutval
  		 *  dataset[ind[lim2..count]][cutfeat]>cutval
  		 */
  		void planeSplit(IndexType* ind, const IndexType count, int cutfeat, DistanceType cutval, IndexType& lim1, IndexType& lim2)
  		{
  			/* Move vector indices for left subtree to front of list. */
  			IndexType left = 0;
  			IndexType right = count-1;
  			for (;; ) {
  				while (left<=right && dataset_get(ind[left],cutfeat)<cutval) ++left;
  				while (right && left<=right && dataset_get(ind[right],cutfeat)>=cutval) --right;
  				if (left>right || !right) break;  // "!right" was added to support unsigned Index types
  				std::swap(ind[left], ind[right]);
  				++left;
  				--right;
  			}
  			/* If either list is empty, it means that all remaining features
  			 * are identical. Split in the middle to maintain a balanced tree.
  			 */
  			lim1 = left;
  			right = count-1;
  			for (;; ) {
  				while (left<=right && dataset_get(ind[left],cutfeat)<=cutval) ++left;
  				while (right && left<=right && dataset_get(ind[right],cutfeat)>cutval) --right;
  				if (left>right || !right) break;  // "!right" was added to support unsigned Index types
  				std::swap(ind[left], ind[right]);
  				++left;
  				--right;
  			}
  			lim2 = left;
  		}
  
  		DistanceType computeInitialDistances(const ElementType* vec, std::vector<DistanceType>& dists) const
  		{
  			assert(vec);
  			DistanceType distsq = 0.0;
  
  			for (int i = 0; i < (DIM>0 ? DIM : dim); ++i) {
  				if (vec[i] < root_bbox[i].low) {
  					dists[i] = distance.accum_dist(vec[i], root_bbox[i].low, i);
  					distsq += dists[i];
  				}
  				if (vec[i] > root_bbox[i].high) {
  					dists[i] = distance.accum_dist(vec[i], root_bbox[i].high, i);
  					distsq += dists[i];
  				}
  			}
  
  			return distsq;
  		}
  
  		/**
  		 * Performs an exact search in the tree starting from a node.
  		 * \tparam RESULTSET Should be any ResultSet<DistanceType>
  		 */
  		template <class RESULTSET>
  		void searchLevel(RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
  						 std::vector<DistanceType>& dists, const float epsError) const
  		{
  			/* If this is a leaf node, then do check and return. */
  			if ((node->child1 == NULL)&&(node->child2 == NULL)) {
  				//count_leaf += (node->lr.right-node->lr.left);  // Removed since was neither used nor returned to the user.
  				DistanceType worst_dist = result_set.worstDist();
  				for (IndexType i=node->lr.left; i<node->lr.right; ++i) {
  					const IndexType index = vind[i];// reorder... : i;
  					DistanceType dist = distance(vec, index, (DIM>0 ? DIM : dim));
  					if (dist<worst_dist) {
  						result_set.addPoint(dist,vind[i]);
  					}
  				}
  				return;
  			}
  
  			/* Which child branch should be taken first? */
  			int idx = node->sub.divfeat;
  			ElementType val = vec[idx];
  			DistanceType diff1 = val - node->sub.divlow;
  			DistanceType diff2 = val - node->sub.divhigh;
  
  			NodePtr bestChild;
  			NodePtr otherChild;
  			DistanceType cut_dist;
  			if ((diff1+diff2)<0) {
  				bestChild = node->child1;
  				otherChild = node->child2;
  				cut_dist = distance.accum_dist(val, node->sub.divhigh, idx);
  			}
  			else {
  				bestChild = node->child2;
  				otherChild = node->child1;
  				cut_dist = distance.accum_dist( val, node->sub.divlow, idx);
  			}
  
  			/* Call recursively to search next level down. */
  			searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
  
  			DistanceType dst = dists[idx];
  			mindistsq = mindistsq + cut_dist - dst;
  			dists[idx] = cut_dist;
  			if (mindistsq*epsError<=result_set.worstDist()) {
  				searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
  			}
  			dists[idx] = dst;
  		}
  
  
  		void saveIndex(FILE* stream)
  		{
  			save_value(stream, m_size);
  			save_value(stream, dim);
  			save_value(stream, root_bbox);
  			save_value(stream, m_leaf_max_size);
  			save_value(stream, vind);
  			save_tree(stream, root_node);
  		}
  
  		void loadIndex(FILE* stream)
  		{
  			load_value(stream, m_size);
  			load_value(stream, dim);
  			load_value(stream, root_bbox);
  			load_value(stream, m_leaf_max_size);
  			load_value(stream, vind);
  			load_tree(stream, root_node);
  		}
  
  	};   // class KDTree
  
  
  	/** A simple KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
  	  *  Each row in the matrix represents a point in the state space.
  	  *
  	  *  Example of usage:
  	  * \code
  	  * 	Eigen::Matrix<num_t,Dynamic,Dynamic>  mat;
  	  * 	// Fill out "mat"...
  	  *
  	  * 	typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic> >  my_kd_tree_t;
  	  * 	const int max_leaf = 10;
  	  * 	my_kd_tree_t   mat_index(dimdim, mat, max_leaf );
  	  * 	mat_index.index->buildIndex();
  	  * 	mat_index.index->...
  	  * \endcode
  	  *
  	  *  \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
  	  *  \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
  	  *  \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
  	  */
  	template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = size_t>
  	struct KDTreeEigenMatrixAdaptor
  	{
  		typedef KDTreeEigenMatrixAdaptor<MatrixType,DIM,Distance> self_t;
  		typedef typename MatrixType::Scalar              num_t;
  		typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
  		typedef KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType>  index_t;
  
  		index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
  
  		/// Constructor: takes a const ref to the matrix object with the data points
  		KDTreeEigenMatrixAdaptor(const int dimensionality, const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat)
  		{
  			const size_t dims = mat.cols();
  			if (DIM>0 && static_cast<int>(dims)!=DIM)
  				throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
  			index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
  			index->buildIndex();
  		}
  
  		~KDTreeEigenMatrixAdaptor() {
  			delete index;
  		}
  
  		const MatrixType &m_data_matrix;
  
  		/** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
  		  *  Note that this is a short-cut method for index->findNeighbors().
  		  *  The user can also call index->... methods as desired.
  		  * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
  		  */
  		inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const
  		{
  			nanoflann::KNNResultSet<typename MatrixType::Scalar,IndexType> resultSet(num_closest);
  			resultSet.init(out_indices, out_distances_sq);
  			index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
  		}
  
  		/** @name Interface expected by KDTreeSingleIndexAdaptor
  		  * @{ */
  
  		const self_t & derived() const {
  			return *this;
  		}
  		self_t & derived()       {
  			return *this;
  		}
  
  		// Must return the number of data points
  		inline size_t kdtree_get_point_count() const {
  			return m_data_matrix.rows();
  		}
  
  		// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
  		inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2,size_t size) const
  		{
  			num_t s=0;
  			for (size_t i=0; i<size; i++) {
  				const num_t d= p1[i]-m_data_matrix.coeff(idx_p2,i);
  				s+=d*d;
  			}
  			return s;
  		}
  
  		// Returns the dim'th component of the idx'th point in the class:
  		inline num_t kdtree_get_pt(const size_t idx, int dim) const {
  			return m_data_matrix.coeff(idx,dim);
  		}
  
  		// Optional bounding-box computation: return false to default to a standard bbox computation loop.
  		//   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
  		//   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
  		template <class BBOX>
  		bool kdtree_get_bbox(BBOX &bb) const {
  			return false;
  		}
  
  		/** @} */
  
  	}; // end of KDTreeEigenMatrixAdaptor
  	/** @} */
  
  /** @} */ // end of grouping
  } // end of NS
  
  
  #endif /* NANOFLANN_HPP_ */
  