Included FLANN headers in rtabmap for convenience (to have commit 23051820b2)

CMakeLists.txt

@@ -132,14 +132,8 @@ option(WITH_GTSAM         "Include GTSAM support"                ON)
 option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 
-find_package(PkgConfig QUIET)
-FIND_PACKAGE(FLANN 1.8 QUIET)
-IF(PKG_CONFIG_FOUND AND FLANN_FOUND)
-    SET(FLANN18_FOUND TRUE)
-ENDIF(PKG_CONFIG_FOUND AND FLANN_FOUND)
-
 FIND_PACKAGE(OpenCV REQUIRED)
-FIND_PACKAGE(PCL 1.7 REQUIRED) # Will find FLANN too
+FIND_PACKAGE(PCL 1.7 REQUIRED)
 FIND_PACKAGE(ZLIB REQUIRED)
 
 IF(WITH_QT)

corelib/src/CMakeLists.txt

@@ -57,6 +57,9 @@ SET(SRC_FILES
 	toro3d/posegraph2.cpp
 	toro3d/treeoptimizer2.cpp
 	
+	flann/ext/lz4.c
+	flann/ext/lz4hc.c
+	
 	sqlite3/sqlite3.c
 )
 
@@ -76,10 +79,6 @@ SET(LIBRARIES
 	${ZLIB_LIBRARIES} 
 )
 
-IF(FLANN18_FOUND)
-	ADD_DEFINITIONS("-DWITH_FLANN18")
-ENDIF(FLANN18_FOUND)
-
 IF(Freenect_FOUND)
 	ADD_DEFINITIONS("-DWITH_FREENECT")
 	IF(Freenect_DASH_INCLUDES)

corelib/src/VWDictionary.cpp

@@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #endif
 
-#include <flann/flann.hpp>
+#include "flann/flann.hpp"
 
 #include <fstream>
 #include <string>
@@ -75,20 +75,7 @@ public:
 		{
 			if(featuresType_ == CV_8UC1)
 			{
-				
+				delete (flann::Index<flann::Hamming<unsigned char> >*)index_;
-#ifdef WITH_FLANN18
-			delete (flann::Index<flann::Hamming<unsigned char> >*)index_;
-#else
-			// issue with 1.7.1: we should explicitly use the corresponding index
-			if(isLSH_)
-			{
-				delete (flann::LshIndex<flann::Hamming<unsigned char> >*)index_;
-			}
-			else
-			{
-				delete (flann::LinearIndex<flann::Hamming<unsigned char> >*)index_;
-			}
-#endif
 			}
 			else
 			{
@@ -108,19 +95,7 @@ public:
 		}
 		if(featuresType_ == CV_8UC1)
 		{
-#ifdef WITH_FLANN18
 			return ((const flann::Index<flann::Hamming<unsigned char> >*)index_)->size();
-#else
-			// issue with 1.7.1: we should explicitly use the corresponding index
-			if(isLSH_)
-			{
-				return ((const flann::LshIndex<flann::Hamming<unsigned char> >*)index_)->size();
-			}
-			else
-			{
-				return ((const flann::LinearIndex<flann::Hamming<unsigned char> >*)index_)->size();
-			}
-#endif
 		}
 		else
 		{
@@ -137,19 +112,7 @@ public:
 		}
 		if(featuresType_ == CV_8UC1)
 		{	
-#ifdef WITH_FLANN18
 			return ((const flann::Index<flann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
-#else
-			// issue with 1.7.1: we should explicitly use the corresponding index
-			if(isLSH_)
-			{
-				return ((const flann::LshIndex<flann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
-			}
-			else
-			{
-				return ((const flann::LinearIndex<flann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
-			}
-#endif
 		}
 		else
 		{
@@ -170,21 +133,7 @@ public:
 		if(featuresType_ == CV_8UC1)
 		{
 			flann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
-#ifdef WITH_FLANN18
 			index_ = new flann::Index<flann::Hamming<unsigned char> >(dataset, params);
-#else
-			// issue with 1.7.1: we should explicitly create the corresponding index
-			flann::flann_algorithm_t algo = params.at("algorithm").cast<flann::flann_algorithm_t>();
-			if(algo == flann::FLANN_INDEX_LSH)
-			{
-				isLSH_ = true;
-				index_ = new flann::LshIndex<flann::Hamming<unsigned char> >(dataset, params);
-			}
-			else
-			{
-				index_ = new flann::LinearIndex<flann::Hamming<unsigned char> >(dataset, params);
-			}
-#endif
 			((flann::Index<flann::Hamming<unsigned char> >*)index_)->buildIndex();
 		}
 		else
@@ -196,15 +145,6 @@ public:
 		nextIndex_ = features.rows;
 	}
 
-	bool isIncremental()
-	{
-#ifdef WITH_FLANN18
-		return true;
-#else
-		return false;
-#endif
-	}
-
 	bool isBuilt()
 	{
 		return index_!=0;
@@ -215,7 +155,6 @@ public:
 
 	unsigned int addPoint(const cv::Mat & feature)
 	{
-#ifdef WITH_FLANN18
 		if(!index_)
 		{
 			UERROR("Flann index not yet created!");
@@ -235,15 +174,10 @@ public:
 			((flann::Index<flann::L2<float> >*)index_)->addPoints(dataset);
 		}
 		return nextIndex_++;
-#else
-		UFATAL("Not built with FLANN 1.8! Only when isIncremental() returns true that you can call this method.");
-		return 0;
-#endif
 	}
 
 	void removePoint(unsigned int index)
 	{
-#ifdef WITH_FLANN18
 		if(!index_)
 		{
 			UERROR("Flann index not yet created!");
@@ -263,9 +197,6 @@ public:
 		{
 			((flann::Index<flann::L2<float> >*)index_)->removePoint(index);
 		}
-#else
-		UFATAL("Not built with FLANN 1.8! Only when isIncremental() returns true that you can call this method.");
-#endif
 	}
 
 	void knnSearch(
@@ -289,19 +220,7 @@ public:
 		{
 			flann::Matrix<unsigned int> distsF((unsigned int*)dists.data, dists.rows, dists.cols);
 			flann::Matrix<unsigned char> queryF(query.data, query.rows, query.cols);
-#ifdef WITH_FLANN18
 			((flann::Index<flann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
-#else
-			// issue with 1.7.1: we should explicitly use the corresponding index
-			if(isLSH_)
-			{
-				((flann::LshIndex<flann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
-			}
-			else
-			{
-				((flann::LinearIndex<flann::Hamming<unsigned char> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
-			}
-#endif
 		}
 		else
 		{
@@ -350,12 +269,6 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
 	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 
-	if(_incrementalFlann && !_flannIndex->isIncremental())
-	{
-		UERROR("TRying to set \"KpIncrementalFlann\"=true but RTAB-Map is not built with FLANN>=1.8. Setting to false.");
-		_incrementalFlann = false;
-	}
-
 	UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
 
 	std::string dictionaryPath = _dictionaryPath;
@@ -583,7 +496,6 @@ void VWDictionary::update()
 	if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
 	{
 		if(_incrementalFlann &&
-		   _flannIndex->isIncremental() &&
 		   _strategy < kNNBruteForce &&
 		   _visualWords.size())
 		{

corelib/src/flann/algorithms/all_indices.h

@@ -0,0 +1,197 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_ALL_INDICES_H_
+#define FLANN_ALL_INDICES_H_
+
+#include "flann/general.h"
+
+#include "flann/algorithms/nn_index.h"
+#include "flann/algorithms/kdtree_index.h"
+#include "flann/algorithms/kdtree_single_index.h"
+#include "flann/algorithms/kmeans_index.h"
+#include "flann/algorithms/composite_index.h"
+#include "flann/algorithms/linear_index.h"
+#include "flann/algorithms/hierarchical_clustering_index.h"
+#include "flann/algorithms/lsh_index.h"
+#include "flann/algorithms/autotuned_index.h"
+#ifdef FLANN_USE_CUDA
+#include "flann/algorithms/kdtree_cuda_3d_index.h"
+#endif
+
+
+namespace flann
+{
+
+/**
+ * enable_if sfinae helper
+ */
+template<bool, typename T = void> struct enable_if{};
+template<typename T> struct enable_if<true,T> { typedef T type; };
+
+/**
+ * disable_if sfinae helper
+ */
+template<bool, typename T> struct disable_if{ typedef T type; };
+template<typename T> struct disable_if<true,T> { };
+
+/**
+ * Check if two type are the same
+ */
+template <typename T, typename U>
+struct same_type
+{
+    enum {value = false};
+};
+
+template<typename T>
+struct same_type<T,T>
+{
+    enum {value = true};
+};
+
+#define HAS_MEMBER(member) \
+    template<typename T> \
+    struct member { \
+        typedef char No; \
+        typedef long Yes; \
+        template<typename C> static Yes test( typename C::member* ); \
+        template<typename C> static No test( ... ); \
+        enum { value = sizeof (test<T>(0))==sizeof(Yes) }; \
+    };
+
+HAS_MEMBER(needs_kdtree_distance)
+HAS_MEMBER(needs_vector_space_distance)
+HAS_MEMBER(is_kdtree_distance)
+HAS_MEMBER(is_vector_space_distance)
+
+struct DummyDistance
+{
+    typedef float ElementType;
+    typedef float ResultType;
+
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        return ResultType(0);
+    }
+
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return ResultType(0);
+    }
+};
+
+/**
+ * Checks if an index and a distance can be used together
+ */
+template<template <typename> class Index, typename Distance, typename ElemType>
+struct valid_combination
+{
+    static const bool value = same_type<ElemType,typename Distance::ElementType>::value &&
+    				(!needs_kdtree_distance<Index<DummyDistance> >::value || is_kdtree_distance<Distance>::value) &&
+    				(!needs_vector_space_distance<Index<DummyDistance> >::value || is_kdtree_distance<Distance>::value || is_vector_space_distance<Distance>::value);
+
+};
+
+
+/*********************************************************
+ * Create index
+ **********************************************************/
+template <template<typename> class Index, typename Distance, typename T>
+inline NNIndex<Distance>* create_index_(flann::Matrix<T> data, const flann::IndexParams& params, const Distance& distance,
+		typename enable_if<valid_combination<Index,Distance,T>::value,void>::type* = 0)
+{
+    return new Index<Distance>(data, params, distance);
+}
+
+template <template<typename> class Index, typename Distance, typename T>
+inline NNIndex<Distance>* create_index_(flann::Matrix<T> data, const flann::IndexParams& params, const Distance& distance,
+		typename disable_if<valid_combination<Index,Distance,T>::value,void>::type* = 0)
+{
+    return NULL;
+}
+
+template<typename Distance>
+inline NNIndex<Distance>*
+  create_index_by_type(const flann_algorithm_t index_type,
+		const Matrix<typename Distance::ElementType>& dataset, const IndexParams& params, const Distance& distance)
+{
+	typedef typename Distance::ElementType ElementType;
+
+	NNIndex<Distance>* nnIndex;
+
+	switch (index_type) {
+
+	case FLANN_INDEX_LINEAR:
+		nnIndex = create_index_<LinearIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_KDTREE_SINGLE:
+		nnIndex = create_index_<KDTreeSingleIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_KDTREE:
+		nnIndex = create_index_<KDTreeIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+		//! #define this symbol before including flann.h to enable GPU search algorithms. But you have
+		//! to link libflann_cuda then!
+#ifdef FLANN_USE_CUDA
+	case FLANN_INDEX_KDTREE_CUDA:
+		nnIndex = create_index_<KDTreeCuda3dIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+#endif
+
+	case FLANN_INDEX_KMEANS:
+		nnIndex = create_index_<KMeansIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_COMPOSITE:
+		nnIndex = create_index_<CompositeIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_AUTOTUNED:
+		nnIndex = create_index_<AutotunedIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_HIERARCHICAL:
+		nnIndex = create_index_<HierarchicalClusteringIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	case FLANN_INDEX_LSH:
+		nnIndex = create_index_<LshIndex,Distance,ElementType>(dataset, params, distance);
+		break;
+	default:
+		throw FLANNException("Unknown index type");
+	}
+
+    if (nnIndex==NULL) {
+    	throw FLANNException("Unsupported index/distance combination");
+    }
+    return nnIndex;
+}
+
+}
+
+#endif /* FLANN_ALL_INDICES_H_ */

corelib/src/flann/algorithms/autotuned_index.h

@@ -0,0 +1,763 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_AUTOTUNED_INDEX_H_
+#define FLANN_AUTOTUNED_INDEX_H_
+
+#include "flann/general.h"
+#include "flann/algorithms/nn_index.h"
+#include "flann/nn/ground_truth.h"
+#include "flann/nn/index_testing.h"
+#include "flann/util/sampling.h"
+#include "flann/algorithms/kdtree_index.h"
+#include "flann/algorithms/kdtree_single_index.h"
+#include "flann/algorithms/kmeans_index.h"
+#include "flann/algorithms/composite_index.h"
+#include "flann/algorithms/linear_index.h"
+#include "flann/util/logger.h"
+
+
+namespace flann
+{
+
+template<typename Distance>
+inline NNIndex<Distance>*
+  create_index_by_type(const flann_algorithm_t index_type,
+        const Matrix<typename Distance::ElementType>& dataset, const IndexParams& params, const Distance& distance = Distance());
+
+
+struct AutotunedIndexParams : public IndexParams
+{
+    AutotunedIndexParams(float target_precision = 0.8, float build_weight = 0.01, float memory_weight = 0, float sample_fraction = 0.1)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_AUTOTUNED;
+        // precision desired (used for autotuning, -1 otherwise)
+        (*this)["target_precision"] = target_precision;
+        // build tree time weighting factor
+        (*this)["build_weight"] = build_weight;
+        // index memory weighting factor
+        (*this)["memory_weight"] = memory_weight;
+        // what fraction of the dataset to use for autotuning
+        (*this)["sample_fraction"] = sample_fraction;
+    }
+};
+
+
+template <typename Distance>
+class AutotunedIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+    
+    typedef NNIndex<Distance> BaseClass;
+
+    typedef AutotunedIndex<Distance> IndexType;
+
+    typedef bool needs_kdtree_distance;
+
+    AutotunedIndex(const Matrix<ElementType>& inputData, const IndexParams& params = AutotunedIndexParams(), Distance d = Distance()) :
+        BaseClass(params, d), bestIndex_(NULL), speedup_(0), dataset_(inputData)
+    {
+        target_precision_ = get_param(params, "target_precision",0.8f);
+        build_weight_ =  get_param(params,"build_weight", 0.01f);
+        memory_weight_ = get_param(params, "memory_weight", 0.0f);
+        sample_fraction_ = get_param(params,"sample_fraction", 0.1f);
+    }
+
+    AutotunedIndex(const IndexParams& params = AutotunedIndexParams(), Distance d = Distance()) :
+        BaseClass(params, d), bestIndex_(NULL), speedup_(0)
+    {
+        target_precision_ = get_param(params, "target_precision",0.8f);
+        build_weight_ =  get_param(params,"build_weight", 0.01f);
+        memory_weight_ = get_param(params, "memory_weight", 0.0f);
+        sample_fraction_ = get_param(params,"sample_fraction", 0.1f);
+    }
+
+    AutotunedIndex(const AutotunedIndex& other) : BaseClass(other),
+    		bestParams_(other.bestParams_),
+    		bestSearchParams_(other.bestSearchParams_),
+    		speedup_(other.speedup_),
+    		dataset_(other.dataset_),
+    		target_precision_(other.target_precision_),
+    		build_weight_(other.build_weight_),
+    		memory_weight_(other.memory_weight_),
+    		sample_fraction_(other.sample_fraction_)
+    {
+    		bestIndex_ = other.bestIndex_->clone();
+    }
+
+    AutotunedIndex& operator=(AutotunedIndex other)
+    {
+    	this->swap(other);
+    	return * this;
+    }
+
+    virtual ~AutotunedIndex()
+    {
+    	delete bestIndex_;
+    }
+
+    BaseClass* clone() const
+    {
+    	return new AutotunedIndex(*this);
+    }
+
+    /**
+     *          Method responsible with building the index.
+     */
+    void buildIndex()
+    {
+        bestParams_ = estimateBuildParams();
+        Logger::info("----------------------------------------------------\n");
+        Logger::info("Autotuned parameters:\n");
+        if (Logger::getLevel()>=FLANN_LOG_INFO)
+        	print_params(bestParams_);
+        Logger::info("----------------------------------------------------\n");
+
+        flann_algorithm_t index_type = get_param<flann_algorithm_t>(bestParams_,"algorithm");
+        bestIndex_ = create_index_by_type(index_type, dataset_, bestParams_, distance_);
+        bestIndex_->buildIndex();
+        speedup_ = estimateSearchParams(bestSearchParams_);
+        Logger::info("----------------------------------------------------\n");
+        Logger::info("Search parameters:\n");
+        if (Logger::getLevel()>=FLANN_LOG_INFO)
+        	print_params(bestSearchParams_);
+        Logger::info("----------------------------------------------------\n");
+        bestParams_["search_params"] = bestSearchParams_;
+        bestParams_["speedup"] = speedup_;
+    }
+    
+    void buildIndex(const Matrix<ElementType>& dataset)
+    {
+    	dataset_ = dataset;
+    	this->buildIndex();
+    }
+
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        if (bestIndex_) {
+            bestIndex_->addPoints(points, rebuild_threshold);
+        }
+    }
+    
+    void removePoint(size_t id)
+    {
+        if (bestIndex_) {
+            bestIndex_->removePoint(id);
+        }
+    }
+
+    
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	ar & target_precision_;
+    	ar & build_weight_;
+    	ar & memory_weight_;
+    	ar & sample_fraction_;
+
+    	flann_algorithm_t index_type;
+    	if (Archive::is_saving::value) {
+    		index_type = get_param<flann_algorithm_t>(bestParams_,"algorithm");
+    	}
+    	ar & index_type;
+    	ar & bestSearchParams_.checks;
+
+    	if (Archive::is_loading::value) {
+    		bestParams_["algorithm"] = index_type;
+
+    		index_params_["algorithm"] = getType();
+            index_params_["target_precision_"] = target_precision_;
+            index_params_["build_weight_"] = build_weight_;
+            index_params_["memory_weight_"] = memory_weight_;
+            index_params_["sample_fraction_"] = sample_fraction_;
+    	}
+    }
+
+    void saveIndex(FILE* stream)
+    {
+        {
+            serialization::SaveArchive sa(stream);
+            sa & *this;
+        }
+
+    	bestIndex_->saveIndex(stream);
+    }
+
+    void loadIndex(FILE* stream)
+    {
+        {
+            serialization::LoadArchive la(stream);
+            la & *this;
+        }
+        
+        IndexParams params;
+        flann_algorithm_t index_type = get_param<flann_algorithm_t>(bestParams_,"algorithm");
+        bestIndex_ = create_index_by_type<Distance>((flann_algorithm_t)index_type, dataset_, params, distance_);
+        bestIndex_->loadIndex(stream);
+    }
+
+    int knnSearch(const Matrix<ElementType>& queries,
+            Matrix<size_t>& indices,
+            Matrix<DistanceType>& dists,
+            size_t knn,
+            const SearchParams& params) const
+    {
+        if (params.checks == FLANN_CHECKS_AUTOTUNED) {
+            return bestIndex_->knnSearch(queries, indices, dists, knn, bestSearchParams_);
+        }
+        else {
+            return bestIndex_->knnSearch(queries, indices, dists, knn, params);
+        }
+    }
+
+    int knnSearch(const Matrix<ElementType>& queries,
+            std::vector< std::vector<size_t> >& indices,
+            std::vector<std::vector<DistanceType> >& dists,
+            size_t knn,
+            const SearchParams& params) const
+    {
+        if (params.checks == FLANN_CHECKS_AUTOTUNED) {
+            return bestIndex_->knnSearch(queries, indices, dists, knn, bestSearchParams_);
+        }
+        else {
+            return bestIndex_->knnSearch(queries, indices, dists, knn, params);
+        }
+
+    }
+    
+    int radiusSearch(const Matrix<ElementType>& queries,
+            Matrix<size_t>& indices,
+            Matrix<DistanceType>& dists,
+            DistanceType radius,
+            const SearchParams& params) const
+    {
+        if (params.checks == FLANN_CHECKS_AUTOTUNED) {
+            return bestIndex_->radiusSearch(queries, indices, dists, radius, bestSearchParams_);
+        }
+        else {
+            return bestIndex_->radiusSearch(queries, indices, dists, radius, params);
+        }
+    }
+
+    int radiusSearch(const Matrix<ElementType>& queries,
+            std::vector< std::vector<size_t> >& indices,
+            std::vector<std::vector<DistanceType> >& dists,
+            DistanceType radius,
+            const SearchParams& params) const
+    {
+        if (params.checks == FLANN_CHECKS_AUTOTUNED) {
+            return bestIndex_->radiusSearch(queries, indices, dists, radius, bestSearchParams_);
+        }
+        else {
+            return bestIndex_->radiusSearch(queries, indices, dists, radius, params);
+        }        
+    }
+
+    
+    
+    /**
+     *      Method that searches for nearest-neighbors
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        // should not get here
+        assert(false);
+    }
+
+    IndexParams getParameters() const
+    {
+        return bestParams_;
+    }
+
+    FLANN_DEPRECATED SearchParams getSearchParameters() const
+    {
+        return bestSearchParams_;
+    }
+
+    FLANN_DEPRECATED float getSpeedup() const
+    {
+        return speedup_;
+    }
+
+
+    /**
+     *      Number of features in this index.
+     */
+    size_t size() const
+    {
+        return bestIndex_->size();
+    }
+
+    /**
+     *  The length of each vector in this index.
+     */
+    size_t veclen() const
+    {
+        return bestIndex_->veclen();
+    }
+
+    /**
+     * The amount of memory (in bytes) this index uses.
+     */
+    int usedMemory() const
+    {
+        return bestIndex_->usedMemory();
+    }
+
+    /**
+     * Algorithm name
+     */
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_AUTOTUNED;
+    }
+
+protected:
+    void buildIndexImpl()
+    {
+        /* nothing to do here */
+    }
+
+    void freeIndex()
+    {
+        /* nothing to do here */
+    }
+
+private:
+
+    struct CostData
+    {
+        float searchTimeCost;
+        float buildTimeCost;
+        float memoryCost;
+        float totalCost;
+        IndexParams params;
+    };
+
+    void evaluate_kmeans(CostData& cost)
+    {
+        StartStopTimer t;
+        int checks;
+        const int nn = 1;
+
+        Logger::info("KMeansTree using params: max_iterations=%d, branching=%d\n",
+                     get_param<int>(cost.params,"iterations"),
+                     get_param<int>(cost.params,"branching"));
+        KMeansIndex<Distance> kmeans(sampledDataset_, cost.params, distance_);
+        // measure index build time
+        t.start();
+        kmeans.buildIndex();
+        t.stop();
+        float buildTime = (float)t.value;
+
+        // measure search time
+        float searchTime = test_index_precision(kmeans, sampledDataset_, testDataset_, gt_matches_, target_precision_, checks, distance_, nn);
+
+        float datasetMemory = float(sampledDataset_.rows * sampledDataset_.cols * sizeof(float));
+        cost.memoryCost = (kmeans.usedMemory() + datasetMemory) / datasetMemory;
+        cost.searchTimeCost = searchTime;
+        cost.buildTimeCost = buildTime;
+        Logger::info("KMeansTree buildTime=%g, searchTime=%g, build_weight=%g\n", buildTime, searchTime, build_weight_);
+    }
+
+
+    void evaluate_kdtree(CostData& cost)
+    {
+        StartStopTimer t;
+        int checks;
+        const int nn = 1;
+
+        Logger::info("KDTree using params: trees=%d\n", get_param<int>(cost.params,"trees"));
+        KDTreeIndex<Distance> kdtree(sampledDataset_, cost.params, distance_);
+
+        t.start();
+        kdtree.buildIndex();
+        t.stop();
+        float buildTime = (float)t.value;
+
+        //measure search time
+        float searchTime = test_index_precision(kdtree, sampledDataset_, testDataset_, gt_matches_, target_precision_, checks, distance_, nn);
+
+        float datasetMemory = float(sampledDataset_.rows * sampledDataset_.cols * sizeof(float));
+        cost.memoryCost = (kdtree.usedMemory() + datasetMemory) / datasetMemory;
+        cost.searchTimeCost = searchTime;
+        cost.buildTimeCost = buildTime;
+        Logger::info("KDTree buildTime=%g, searchTime=%g\n", buildTime, searchTime);
+    }
+
+
+    //    struct KMeansSimpleDownhillFunctor {
+    //
+    //        Autotune& autotuner;
+    //        KMeansSimpleDownhillFunctor(Autotune& autotuner_) : autotuner(autotuner_) {};
+    //
+    //        float operator()(int* params) {
+    //
+    //            float maxFloat = numeric_limits<float>::max();
+    //
+    //            if (params[0]<2) return maxFloat;
+    //            if (params[1]<0) return maxFloat;
+    //
+    //            CostData c;
+    //            c.params["algorithm"] = KMEANS;
+    //            c.params["centers-init"] = CENTERS_RANDOM;
+    //            c.params["branching"] = params[0];
+    //            c.params["max-iterations"] = params[1];
+    //
+    //            autotuner.evaluate_kmeans(c);
+    //
+    //            return c.timeCost;
+    //
+    //        }
+    //    };
+    //
+    //    struct KDTreeSimpleDownhillFunctor {
+    //
+    //        Autotune& autotuner;
+    //        KDTreeSimpleDownhillFunctor(Autotune& autotuner_) : autotuner(autotuner_) {};
+    //
+    //        float operator()(int* params) {
+    //            float maxFloat = numeric_limits<float>::max();
+    //
+    //            if (params[0]<1) return maxFloat;
+    //
+    //            CostData c;
+    //            c.params["algorithm"] = KDTREE;
+    //            c.params["trees"] = params[0];
+    //
+    //            autotuner.evaluate_kdtree(c);
+    //
+    //            return c.timeCost;
+    //
+    //        }
+    //    };
+
+
+
+    void optimizeKMeans(std::vector<CostData>& costs)
+    {
+        Logger::info("KMEANS, Step 1: Exploring parameter space\n");
+
+        // explore kmeans parameters space using combinations of the parameters below
+        int maxIterations[] = { 1, 5, 10, 15 };
+        int branchingFactors[] = { 16, 32, 64, 128, 256 };
+
+        int kmeansParamSpaceSize = FLANN_ARRAY_LEN(maxIterations) * FLANN_ARRAY_LEN(branchingFactors);
+        costs.reserve(costs.size() + kmeansParamSpaceSize);
+
+        // evaluate kmeans for all parameter combinations
+        for (size_t i = 0; i < FLANN_ARRAY_LEN(maxIterations); ++i) {
+            for (size_t j = 0; j < FLANN_ARRAY_LEN(branchingFactors); ++j) {
+                CostData cost;
+                cost.params["algorithm"] = FLANN_INDEX_KMEANS;
+                cost.params["centers_init"] = FLANN_CENTERS_RANDOM;
+                cost.params["iterations"] = maxIterations[i];
+                cost.params["branching"] = branchingFactors[j];
+
+                evaluate_kmeans(cost);
+                costs.push_back(cost);
+            }
+        }
+
+        //         Logger::info("KMEANS, Step 2: simplex-downhill optimization\n");
+        //
+        //         const int n = 2;
+        //         // choose initial simplex points as the best parameters so far
+        //         int kmeansNMPoints[n*(n+1)];
+        //         float kmeansVals[n+1];
+        //         for (int i=0;i<n+1;++i) {
+        //             kmeansNMPoints[i*n] = (int)kmeansCosts[i].params["branching"];
+        //             kmeansNMPoints[i*n+1] = (int)kmeansCosts[i].params["max-iterations"];
+        //             kmeansVals[i] = kmeansCosts[i].timeCost;
+        //         }
+        //         KMeansSimpleDownhillFunctor kmeans_cost_func(*this);
+        //         // run optimization
+        //         optimizeSimplexDownhill(kmeansNMPoints,n,kmeans_cost_func,kmeansVals);
+        //         // store results
+        //         for (int i=0;i<n+1;++i) {
+        //             kmeansCosts[i].params["branching"] = kmeansNMPoints[i*2];
+        //             kmeansCosts[i].params["max-iterations"] = kmeansNMPoints[i*2+1];
+        //             kmeansCosts[i].timeCost = kmeansVals[i];
+        //         }
+    }
+
+
+    void optimizeKDTree(std::vector<CostData>& costs)
+    {
+        Logger::info("KD-TREE, Step 1: Exploring parameter space\n");
+
+        // explore kd-tree parameters space using the parameters below
+        int testTrees[] = { 1, 4, 8, 16, 32 };
+
+        // evaluate kdtree for all parameter combinations
+        for (size_t i = 0; i < FLANN_ARRAY_LEN(testTrees); ++i) {
+            CostData cost;
+            cost.params["algorithm"] = FLANN_INDEX_KDTREE;
+            cost.params["trees"] = testTrees[i];
+
+            evaluate_kdtree(cost);
+            costs.push_back(cost);
+        }
+
+        //         Logger::info("KD-TREE, Step 2: simplex-downhill optimization\n");
+        //
+        //         const int n = 1;
+        //         // choose initial simplex points as the best parameters so far
+        //         int kdtreeNMPoints[n*(n+1)];
+        //         float kdtreeVals[n+1];
+        //         for (int i=0;i<n+1;++i) {
+        //             kdtreeNMPoints[i] = (int)kdtreeCosts[i].params["trees"];
+        //             kdtreeVals[i] = kdtreeCosts[i].timeCost;
+        //         }
+        //         KDTreeSimpleDownhillFunctor kdtree_cost_func(*this);
+        //         // run optimization
+        //         optimizeSimplexDownhill(kdtreeNMPoints,n,kdtree_cost_func,kdtreeVals);
+        //         // store results
+        //         for (int i=0;i<n+1;++i) {
+        //             kdtreeCosts[i].params["trees"] = kdtreeNMPoints[i];
+        //             kdtreeCosts[i].timeCost = kdtreeVals[i];
+        //         }
+    }
+
+    /**
+     *  Chooses the best nearest-neighbor algorithm and estimates the optimal
+     *  parameters to use when building the index (for a given precision).
+     *  Returns a dictionary with the optimal parameters.
+     */
+    IndexParams estimateBuildParams()
+    {
+        std::vector<CostData> costs;
+
+        int sampleSize = int(sample_fraction_ * dataset_.rows);
+        int testSampleSize = std::min(sampleSize / 10, 1000);
+
+        Logger::info("Entering autotuning, dataset size: %d, sampleSize: %d, testSampleSize: %d, target precision: %g\n", dataset_.rows, sampleSize, testSampleSize, target_precision_);
+
+        // For a very small dataset, it makes no sense to build any fancy index, just
+        // use linear search
+        if (testSampleSize < 10) {
+            Logger::info("Choosing linear, dataset too small\n");
+            return LinearIndexParams();
+        }
+
+        // We use a fraction of the original dataset to speedup the autotune algorithm
+        sampledDataset_ = random_sample(dataset_, sampleSize);
+        // We use a cross-validation approach, first we sample a testset from the dataset
+        testDataset_ = random_sample(sampledDataset_, testSampleSize, true);
+
+        // We compute the ground truth using linear search
+        Logger::info("Computing ground truth... \n");
+        gt_matches_ = Matrix<size_t>(new size_t[testDataset_.rows], testDataset_.rows, 1);
+        StartStopTimer t;
+        int repeats = 0;
+        t.reset();
+        while (t.value<0.2) {
+        	repeats++;
+            t.start();
+        	compute_ground_truth<Distance>(sampledDataset_, testDataset_, gt_matches_, 0, distance_);
+            t.stop();
+        }
+
+        CostData linear_cost;
+        linear_cost.searchTimeCost = (float)t.value/repeats;
+        linear_cost.buildTimeCost = 0;
+        linear_cost.memoryCost = 0;
+        linear_cost.params["algorithm"] = FLANN_INDEX_LINEAR;
+
+        costs.push_back(linear_cost);
+
+        // Start parameter autotune process
+        Logger::info("Autotuning parameters...\n");
+
+        optimizeKMeans(costs);
+        optimizeKDTree(costs);
+
+        float bestTimeCost = costs[0].buildTimeCost * build_weight_ + costs[0].searchTimeCost;
+        for (size_t i = 0; i < costs.size(); ++i) {
+            float timeCost = costs[i].buildTimeCost * build_weight_ + costs[i].searchTimeCost;
+            Logger::debug("Time cost: %g\n", timeCost);
+            if (timeCost < bestTimeCost) {
+                bestTimeCost = timeCost;
+            }
+        }
+        Logger::debug("Best time cost: %g\n", bestTimeCost);
+
+    	IndexParams bestParams = costs[0].params;
+        if (bestTimeCost > 0) {
+        	float bestCost = (costs[0].buildTimeCost * build_weight_ + costs[0].searchTimeCost) / bestTimeCost;
+        	for (size_t i = 0; i < costs.size(); ++i) {
+        		float crtCost = (costs[i].buildTimeCost * build_weight_ + costs[i].searchTimeCost) / bestTimeCost +
+        				memory_weight_ * costs[i].memoryCost;
+        		Logger::debug("Cost: %g\n", crtCost);
+        		if (crtCost < bestCost) {
+        			bestCost = crtCost;
+        			bestParams = costs[i].params;
+        		}
+        	}
+            Logger::debug("Best cost: %g\n", bestCost);
+        }
+
+        delete[] gt_matches_.ptr();
+        delete[] testDataset_.ptr();
+        delete[] sampledDataset_.ptr();
+
+        return bestParams;
+    }
+
+
+
+    /**
+     *  Estimates the search time parameters needed to get the desired precision.
+     *  Precondition: the index is built
+     *  Postcondition: the searchParams will have the optimum params set, also the speedup obtained over linear search.
+     */
+    float estimateSearchParams(SearchParams& searchParams)
+    {
+        const int nn = 1;
+        const size_t SAMPLE_COUNT = 1000;
+
+        assert(bestIndex_ != NULL); // must have a valid index
+
+        float speedup = 0;
+
+        int samples = (int)std::min(dataset_.rows / 10, SAMPLE_COUNT);
+        if (samples > 0) {
+            Matrix<ElementType> testDataset = random_sample(dataset_, samples);
+
+            Logger::info("Computing ground truth\n");
+
+            // we need to compute the ground truth first
+            Matrix<size_t> gt_matches(new size_t[testDataset.rows], testDataset.rows, 1);
+            StartStopTimer t;
+            int repeats = 0;
+            t.reset();
+            while (t.value<0.2) {
+            	repeats++;
+                t.start();
+            	compute_ground_truth<Distance>(dataset_, testDataset, gt_matches, 1, distance_);
+                t.stop();
+            }
+            float linear = (float)t.value/repeats;
+
+            int checks;
+            Logger::info("Estimating number of checks\n");
+
+            float searchTime;
+            float cb_index;
+            if (bestIndex_->getType() == FLANN_INDEX_KMEANS) {
+                Logger::info("KMeans algorithm, estimating cluster border factor\n");
+                KMeansIndex<Distance>* kmeans = static_cast<KMeansIndex<Distance>*>(bestIndex_);
+                float bestSearchTime = -1;
+                float best_cb_index = -1;
+                int best_checks = -1;
+                for (cb_index = 0; cb_index < 1.1f; cb_index += 0.2f) {
+                    kmeans->set_cb_index(cb_index);
+                    searchTime = test_index_precision(*kmeans, dataset_, testDataset, gt_matches, target_precision_, checks, distance_, nn, 1);
+                    if ((searchTime < bestSearchTime) || (bestSearchTime == -1)) {
+                        bestSearchTime = searchTime;
+                        best_cb_index = cb_index;
+                        best_checks = checks;
+                    }
+                }
+                searchTime = bestSearchTime;
+                cb_index = best_cb_index;
+                checks = best_checks;
+
+                kmeans->set_cb_index(best_cb_index);
+                Logger::info("Optimum cb_index: %g\n", cb_index);
+                bestParams_["cb_index"] = cb_index;
+            }
+            else {
+                searchTime = test_index_precision(*bestIndex_, dataset_, testDataset, gt_matches, target_precision_, checks, distance_, nn, 1);
+            }
+
+            Logger::info("Required number of checks: %d \n", checks);
+            searchParams.checks = checks;
+
+            speedup = linear / searchTime;
+
+            delete[] gt_matches.ptr();
+            delete[] testDataset.ptr();
+        }
+
+        return speedup;
+    }
+
+
+    void swap(AutotunedIndex& other)
+    {
+    	BaseClass::swap(other);
+    	std::swap(bestIndex_, other.bestIndex_);
+    	std::swap(bestParams_, other.bestParams_);
+    	std::swap(bestSearchParams_, other.bestSearchParams_);
+    	std::swap(speedup_, other.speedup_);
+    	std::swap(dataset_, other.dataset_);
+    	std::swap(target_precision_, other.target_precision_);
+    	std::swap(build_weight_, other.build_weight_);
+    	std::swap(memory_weight_, other.memory_weight_);
+    	std::swap(sample_fraction_, other.sample_fraction_);
+    }
+
+private:
+    NNIndex<Distance>* bestIndex_;
+
+    IndexParams bestParams_;
+    SearchParams bestSearchParams_;
+
+    Matrix<ElementType> sampledDataset_;
+    Matrix<ElementType> testDataset_;
+    Matrix<size_t> gt_matches_;
+
+    float speedup_;
+
+    /**
+     * The dataset used by this index
+     */
+    Matrix<ElementType> dataset_;
+
+    /**
+     * Index parameters
+     */
+    float target_precision_;
+    float build_weight_;
+    float memory_weight_;
+    float sample_fraction_;
+
+    USING_BASECLASS_SYMBOLS
+};
+}
+
+#endif /* FLANN_AUTOTUNED_INDEX_H_ */

corelib/src/flann/algorithms/center_chooser.h

@@ -0,0 +1,385 @@
+/*
+ * center_chooser.h
+ *
+ *  Created on: 2012-11-04
+ *      Author: marius
+ */
+
+#ifndef CENTER_CHOOSER_H_
+#define CENTER_CHOOSER_H_
+
+#include <flann/util/matrix.h>
+
+namespace flann
+{
+
+template <typename Distance, typename ElementType>
+struct squareDistance
+{
+    typedef typename Distance::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist*dist; }
+};
+
+
+template <typename ElementType>
+struct squareDistance<L2_Simple<ElementType>, ElementType>
+{
+    typedef typename L2_Simple<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+template <typename ElementType>
+struct squareDistance<L2_3D<ElementType>, ElementType>
+{
+    typedef typename L2_3D<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+template <typename ElementType>
+struct squareDistance<L2<ElementType>, ElementType>
+{
+    typedef typename L2<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+
+template <typename ElementType>
+struct squareDistance<HellingerDistance<ElementType>, ElementType>
+{
+    typedef typename HellingerDistance<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+
+template <typename ElementType>
+struct squareDistance<ChiSquareDistance<ElementType>, ElementType>
+{
+    typedef typename ChiSquareDistance<ElementType>::ResultType ResultType;
+    ResultType operator()( ResultType dist ) { return dist; }
+};
+
+
+template <typename Distance>
+typename Distance::ResultType ensureSquareDistance( typename Distance::ResultType dist )
+{
+    typedef typename Distance::ElementType ElementType;
+
+    squareDistance<Distance, ElementType> dummy;
+    return dummy( dist );
+}
+
+
+
+template <typename Distance>
+class CenterChooser
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    CenterChooser(const Distance& distance, const std::vector<ElementType*>& points) : distance_(distance), points_(points) {};
+
+    virtual ~CenterChooser() {};
+    
+    void setDataSize(size_t cols) { cols_ = cols; }
+
+    /**
+     * Chooses cluster centers
+     *
+     * @param k number of centers to choose
+     * @param indices indices of points to choose the centers from
+     * @param indices_length length of indices
+     * @param centers indices of chosen centers
+     * @param centers_length length of centers array
+     */
+	virtual void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length) = 0;
+
+protected:
+	const Distance distance_;
+    const std::vector<ElementType*>& points_;
+    size_t cols_;
+};
+
+
+template <typename Distance>
+class RandomCenterChooser : public CenterChooser<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+    using CenterChooser<Distance>::points_;
+    using CenterChooser<Distance>::distance_;
+    using CenterChooser<Distance>::cols_;
+
+    RandomCenterChooser(const Distance& distance, const std::vector<ElementType*>& points) :
+    	CenterChooser<Distance>(distance, points) {}
+
+    void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length)
+    {
+        UniqueRandom r(indices_length);
+
+        int index;
+        for (index=0; index<k; ++index) {
+            bool duplicate = true;
+            int rnd;
+            while (duplicate) {
+                duplicate = false;
+                rnd = r.next();
+                if (rnd<0) {
+                    centers_length = index;
+                    return;
+                }
+
+                centers[index] = indices[rnd];
+
+                for (int j=0; j<index; ++j) {
+                    DistanceType sq = distance_(points_[centers[index]], points_[centers[j]], cols_);
+                    if (sq<1e-16) {
+                        duplicate = true;
+                    }
+                }
+            }
+        }
+
+        centers_length = index;
+    }
+};
+
+
+
+/**
+ * Chooses the initial centers using the Gonzales algorithm.
+ */
+template <typename Distance>
+class GonzalesCenterChooser : public CenterChooser<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    using CenterChooser<Distance>::points_;
+    using CenterChooser<Distance>::distance_;
+    using CenterChooser<Distance>::cols_;
+
+    GonzalesCenterChooser(const Distance& distance, const std::vector<ElementType*>& points) : 
+        CenterChooser<Distance>(distance, points) {}
+
+    void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length)
+    {
+        int n = indices_length;
+
+        int rnd = rand_int(n);
+        assert(rnd >=0 && rnd < n);
+
+        centers[0] = indices[rnd];
+
+        int index;
+        for (index=1; index<k; ++index) {
+
+            int best_index = -1;
+            DistanceType best_val = 0;
+            for (int j=0; j<n; ++j) {
+            	DistanceType dist = distance_(points_[centers[0]],points_[indices[j]],cols_);
+                for (int i=1; i<index; ++i) {
+                    DistanceType tmp_dist = distance_(points_[centers[i]],points_[indices[j]],cols_);
+                    if (tmp_dist<dist) {
+                        dist = tmp_dist;
+                    }
+                }
+                if (dist>best_val) {
+                    best_val = dist;
+                    best_index = j;
+                }
+            }
+            if (best_index!=-1) {
+                centers[index] = indices[best_index];
+            }
+            else {
+                break;
+            }
+        }
+        centers_length = index;
+    }
+};
+
+
+/**
+ * Chooses the initial centers using the algorithm proposed in the KMeans++ paper:
+ * Arthur, David; Vassilvitskii, Sergei - k-means++: The Advantages of Careful Seeding
+ */
+template <typename Distance>
+class KMeansppCenterChooser : public CenterChooser<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    using CenterChooser<Distance>::points_;
+    using CenterChooser<Distance>::distance_;
+    using CenterChooser<Distance>::cols_;
+
+    KMeansppCenterChooser(const Distance& distance, const std::vector<ElementType*>& points) : 
+        CenterChooser<Distance>(distance, points) {}
+
+    void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length)
+    {
+        int n = indices_length;
+
+        double currentPot = 0;
+        DistanceType* closestDistSq = new DistanceType[n];
+
+        // Choose one random center and set the closestDistSq values
+        int index = rand_int(n);
+        assert(index >=0 && index < n);
+        centers[0] = indices[index];
+
+        // Computing distance^2 will have the advantage of even higher probability further to pick new centers
+        // far from previous centers (and this complies to "k-means++: the advantages of careful seeding" article)
+        for (int i = 0; i < n; i++) {
+            closestDistSq[i] = distance_(points_[indices[i]], points_[indices[index]], cols_);
+            closestDistSq[i] = ensureSquareDistance<Distance>( closestDistSq[i] );
+            currentPot += closestDistSq[i];
+        }
+
+
+        const int numLocalTries = 1;
+
+        // Choose each center
+        int centerCount;
+        for (centerCount = 1; centerCount < k; centerCount++) {
+
+            // Repeat several trials
+            double bestNewPot = -1;
+            int bestNewIndex = 0;
+            for (int localTrial = 0; localTrial < numLocalTries; localTrial++) {
+
+                // Choose our center - have to be slightly careful to return a valid answer even accounting
+                // for possible rounding errors
+                double randVal = rand_double(currentPot);
+                for (index = 0; index < n-1; index++) {
+                    if (randVal <= closestDistSq[index]) break;
+                    else randVal -= closestDistSq[index];
+                }
+
+                // Compute the new potential
+                double newPot = 0;
+                for (int i = 0; i < n; i++) {
+                    DistanceType dist = distance_(points_[indices[i]], points_[indices[index]], cols_);
+                    newPot += std::min( ensureSquareDistance<Distance>(dist), closestDistSq[i] );
+                }
+
+                // Store the best result
+                if ((bestNewPot < 0)||(newPot < bestNewPot)) {
+                    bestNewPot = newPot;
+                    bestNewIndex = index;
+                }
+            }
+
+            // Add the appropriate center
+            centers[centerCount] = indices[bestNewIndex];
+            currentPot = bestNewPot;
+            for (int i = 0; i < n; i++) {
+                DistanceType dist = distance_(points_[indices[i]], points_[indices[bestNewIndex]], cols_);
+                closestDistSq[i] = std::min( ensureSquareDistance<Distance>(dist), closestDistSq[i] );
+            }
+        }
+
+        centers_length = centerCount;
+
+        delete[] closestDistSq;
+    }
+};
+
+
+
+/**
+ * Chooses the initial centers in a way inspired by Gonzales (by Pierre-Emmanuel Viel):
+ * select the first point of the list as a candidate, then parse the points list. If another
+ * point is further than current candidate from the other centers, test if it is a good center
+ * of a local aggregation. If it is, replace current candidate by this point. And so on...
+ *
+ * Used with KMeansIndex that computes centers coordinates by averaging positions of clusters points,
+ * this doesn't make a real difference with previous methods. But used with HierarchicalClusteringIndex
+ * class that pick centers among existing points instead of computing the barycenters, there is a real
+ * improvement.
+ */
+template <typename Distance>
+class GroupWiseCenterChooser : public CenterChooser<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    using CenterChooser<Distance>::points_;
+    using CenterChooser<Distance>::distance_;
+    using CenterChooser<Distance>::cols_;
+
+    GroupWiseCenterChooser(const Distance& distance, const std::vector<ElementType*>& points) :
+        CenterChooser<Distance>(distance, points) {}
+
+    void operator()(int k, int* indices, int indices_length, int* centers, int& centers_length)
+    {
+        const float kSpeedUpFactor = 1.3f;
+
+        int n = indices_length;
+
+        DistanceType* closestDistSq = new DistanceType[n];
+
+        // Choose one random center and set the closestDistSq values
+        int index = rand_int(n);
+        assert(index >=0 && index < n);
+        centers[0] = indices[index];
+
+        for (int i = 0; i < n; i++) {
+            closestDistSq[i] = distance_(points_[indices[i]], points_[indices[index]], cols_);
+        }
+
+
+        // Choose each center
+        int centerCount;
+        for (centerCount = 1; centerCount < k; centerCount++) {
+
+            // Repeat several trials
+            double bestNewPot = -1;
+            int bestNewIndex = 0;
+            DistanceType furthest = 0;
+            for (index = 0; index < n; index++) {
+
+                // We will test only the potential of the points further than current candidate
+                if( closestDistSq[index] > kSpeedUpFactor * (float)furthest ) {
+
+                    // Compute the new potential
+                    double newPot = 0;
+                    for (int i = 0; i < n; i++) {
+                        newPot += std::min( distance_(points_[indices[i]], points_[indices[index]], cols_)
+                                            , closestDistSq[i] );
+                    }
+
+                    // Store the best result
+                    if ((bestNewPot < 0)||(newPot <= bestNewPot)) {
+                        bestNewPot = newPot;
+                        bestNewIndex = index;
+                        furthest = closestDistSq[index];
+                    }
+                }
+            }
+
+            // Add the appropriate center
+            centers[centerCount] = indices[bestNewIndex];
+            for (int i = 0; i < n; i++) {
+                closestDistSq[i] = std::min( distance_(points_[indices[i]], points_[indices[bestNewIndex]], cols_)
+                                             , closestDistSq[i] );
+            }
+        }
+
+        centers_length = centerCount;
+
+        delete[] closestDistSq;
+    }
+};
+
+
+}
+
+
+#endif /* CENTER_CHOOSER_H_ */

corelib/src/flann/algorithms/composite_index.h

@@ -0,0 +1,239 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_COMPOSITE_INDEX_H_
+#define FLANN_COMPOSITE_INDEX_H_
+
+#include "flann/general.h"
+#include "flann/algorithms/nn_index.h"
+#include "flann/algorithms/kdtree_index.h"
+#include "flann/algorithms/kmeans_index.h"
+
+namespace flann
+{
+
+/**
+ * Index parameters for the CompositeIndex.
+ */
+struct CompositeIndexParams : public IndexParams
+{
+    CompositeIndexParams(int trees = 4, int branching = 32, int iterations = 11,
+                         flann_centers_init_t centers_init = FLANN_CENTERS_RANDOM, float cb_index = 0.2 )
+    {
+        (*this)["algorithm"] = FLANN_INDEX_KMEANS;
+        // number of randomized trees to use (for kdtree)
+        (*this)["trees"] = trees;
+        // branching factor
+        (*this)["branching"] = branching;
+        // max iterations to perform in one kmeans clustering (kmeans tree)
+        (*this)["iterations"] = iterations;
+        // algorithm used for picking the initial cluster centers for kmeans tree
+        (*this)["centers_init"] = centers_init;
+        // cluster boundary index. Used when searching the kmeans tree
+        (*this)["cb_index"] = cb_index;
+    }
+};
+
+
+/**
+ * This index builds a kd-tree index and a k-means index and performs nearest
+ * neighbour search both indexes. This gives a slight boost in search performance
+ * as some of the neighbours that are missed by one index are found by the other.
+ */
+template <typename Distance>
+class CompositeIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    typedef bool needs_kdtree_distance;
+
+    /**
+     * Index constructor
+     * @param inputData dataset containing the points to index
+     * @param params Index parameters
+     * @param d Distance functor
+     * @return
+     */
+    CompositeIndex(const IndexParams& params = CompositeIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+        kdtree_index_ = new KDTreeIndex<Distance>(params, d);
+        kmeans_index_ = new KMeansIndex<Distance>(params, d);
+
+    }
+
+    CompositeIndex(const Matrix<ElementType>& inputData, const IndexParams& params = CompositeIndexParams(),
+                   Distance d = Distance()) : BaseClass(params, d)
+    {
+        kdtree_index_ = new KDTreeIndex<Distance>(inputData, params, d);
+        kmeans_index_ = new KMeansIndex<Distance>(inputData, params, d);
+    }
+
+    CompositeIndex(const CompositeIndex& other) : BaseClass(other),
+    	kmeans_index_(other.kmeans_index_), kdtree_index_(other.kdtree_index_)
+    {
+    }
+
+    CompositeIndex& operator=(CompositeIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    virtual ~CompositeIndex()
+    {
+        delete kdtree_index_;
+        delete kmeans_index_;
+    }
+
+    BaseClass* clone() const
+    {
+    	return new CompositeIndex(*this);
+    }
+
+    /**
+     * @return The index type
+     */
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_COMPOSITE;
+    }
+
+    /**
+     * @return Size of the index
+     */
+    size_t size() const
+    {
+        return kdtree_index_->size();
+    }
+
+    /**
+     * \returns The dimensionality of the features in this index.
+     */
+    size_t veclen() const
+    {
+        return kdtree_index_->veclen();
+    }
+
+    /**
+     * \returns The amount of memory (in bytes) used by the index.
+     */
+    int usedMemory() const
+    {
+        return kmeans_index_->usedMemory() + kdtree_index_->usedMemory();
+    }
+
+    using NNIndex<Distance>::buildIndex;
+    /**
+     * \brief Builds the index
+     */
+    void buildIndex()
+    {
+        Logger::info("Building kmeans tree...\n");
+        kmeans_index_->buildIndex();
+        Logger::info("Building kdtree tree...\n");
+        kdtree_index_->buildIndex();
+    }
+    
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        kmeans_index_->addPoints(points, rebuild_threshold);
+        kdtree_index_->addPoints(points, rebuild_threshold);
+    }
+
+    void removePoint(size_t index)
+    {
+        kmeans_index_->removePoint(index);
+        kdtree_index_->removePoint(index);
+    }
+
+
+    /**
+     * \brief Saves the index to a stream
+     * \param stream The stream to save the index to
+     */
+    void saveIndex(FILE* stream)
+    {
+        kmeans_index_->saveIndex(stream);
+        kdtree_index_->saveIndex(stream);
+    }
+
+    /**
+     * \brief Loads the index from a stream
+     * \param stream The stream from which the index is loaded
+     */
+    void loadIndex(FILE* stream)
+    {
+        kmeans_index_->loadIndex(stream);
+        kdtree_index_->loadIndex(stream);
+    }
+
+    /**
+     * \brief Method that searches for nearest-neighbours
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        kmeans_index_->findNeighbors(result, vec, searchParams);
+        kdtree_index_->findNeighbors(result, vec, searchParams);
+    }
+
+protected:
+    void swap(CompositeIndex& other)
+    {
+    	std::swap(kmeans_index_, other.kmeans_index_);
+    	std::swap(kdtree_index_, other.kdtree_index_);
+    }
+
+    void buildIndexImpl()
+    {
+        /* nothing to do here */
+    }
+
+    void freeIndex()
+    {
+        /* nothing to do here */
+    }
+
+
+private:
+    /** The k-means index */
+    KMeansIndex<Distance>* kmeans_index_;
+
+    /** The kd-tree index */
+    KDTreeIndex<Distance>* kdtree_index_;
+};
+
+}
+
+#endif //FLANN_COMPOSITE_INDEX_H_

corelib/src/flann/algorithms/dist.h

@@ -0,0 +1,790 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_DIST_H_
+#define FLANN_DIST_H_
+
+#include <cmath>
+#include <cstdlib>
+#include <string.h>
+#ifdef _MSC_VER
+typedef unsigned __int32 uint32_t;
+typedef unsigned __int64 uint64_t;
+#else
+#include <stdint.h>
+#endif
+
+#include "flann/defines.h"
+
+
+namespace flann
+{
+
+template<typename T>
+struct Accumulator { typedef T Type; };
+template<>
+struct Accumulator<unsigned char>  { typedef float Type; };
+template<>
+struct Accumulator<unsigned short> { typedef float Type; };
+template<>
+struct Accumulator<unsigned int> { typedef float Type; };
+template<>
+struct Accumulator<char>   { typedef float Type; };
+template<>
+struct Accumulator<short>  { typedef float Type; };
+template<>
+struct Accumulator<int> { typedef float Type; };
+
+
+
+/**
+ * Squared Euclidean distance functor.
+ *
+ * This is the simpler, unrolled version. This is preferable for
+ * very low dimensionality data (eg 3D points)
+ */
+template<class T>
+struct L2_Simple
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff;
+        for(size_t i = 0; i < size; ++i ) {
+            diff = *a++ - *b++;
+            result += diff*diff;
+        }
+        return result;
+    }
+
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return (a-b)*(a-b);
+    }
+};
+
+template<class T>
+struct L2_3D
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        ResultType result = ResultType();        
+        ResultType diff;
+        diff = *a++ - *b++;
+        result += diff*diff;
+        diff = *a++ - *b++;
+        result += diff*diff;
+        diff = *a++ - *b++;
+        result += diff*diff;        
+        return result;
+    }
+
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return (a-b)*(a-b);
+    }
+};
+
+/**
+ * Squared Euclidean distance functor, optimized version
+ */
+template<class T>
+struct L2
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the squared Euclidean distance between two vectors.
+     *
+     *	This is highly optimised, with loop unrolling, as it is one
+     *	of the most expensive inner loops.
+     *
+     *	The computation of squared root at the end is omitted for
+     *	efficiency.
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = (ResultType)(a[0] - b[0]);
+            diff1 = (ResultType)(a[1] - b[1]);
+            diff2 = (ResultType)(a[2] - b[2]);
+            diff3 = (ResultType)(a[3] - b[3]);
+            result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
+            a += 4;
+            b += 4;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            diff0 = (ResultType)(*a++ - *b++);
+            result += diff0 * diff0;
+        }
+        return result;
+    }
+
+    /**
+     *	Partial euclidean distance, using just one dimension. This is used by the
+     *	kd-tree when computing partial distances while traversing the tree.
+     *
+     *	Squared root is omitted for efficiency.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return (a-b)*(a-b);
+    }
+};
+
+
+/*
+ * Manhattan distance functor, optimized version
+ */
+template<class T>
+struct L1
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the Manhattan (L_1) distance between two vectors.
+     *
+     *	This is highly optimised, with loop unrolling, as it is one
+     *	of the most expensive inner loops.
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = (ResultType)std::abs(a[0] - b[0]);
+            diff1 = (ResultType)std::abs(a[1] - b[1]);
+            diff2 = (ResultType)std::abs(a[2] - b[2]);
+            diff3 = (ResultType)std::abs(a[3] - b[3]);
+            result += diff0 + diff1 + diff2 + diff3;
+            a += 4;
+            b += 4;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            diff0 = (ResultType)std::abs(*a++ - *b++);
+            result += diff0;
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return std::abs(a-b);
+    }
+};
+
+
+
+template<class T>
+struct MinkowskiDistance
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    int order;
+
+    MinkowskiDistance(int order_) : order(order_) {}
+
+    /**
+     *  Compute the Minkowsky (L_p) distance between two vectors.
+     *
+     *	This is highly optimised, with loop unrolling, as it is one
+     *	of the most expensive inner loops.
+     *
+     *	The computation of squared root at the end is omitted for
+     *	efficiency.
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = (ResultType)std::abs(a[0] - b[0]);
+            diff1 = (ResultType)std::abs(a[1] - b[1]);
+            diff2 = (ResultType)std::abs(a[2] - b[2]);
+            diff3 = (ResultType)std::abs(a[3] - b[3]);
+            result += pow(diff0,order) + pow(diff1,order) + pow(diff2,order) + pow(diff3,order);
+            a += 4;
+            b += 4;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            diff0 = (ResultType)std::abs(*a++ - *b++);
+            result += pow(diff0,order);
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return pow(static_cast<ResultType>(std::abs(a-b)),order);
+    }
+};
+
+
+
+template<class T>
+struct MaxDistance
+{
+    typedef bool is_vector_space_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the max distance (L_infinity) between two vectors.
+     *
+     *  This distance is not a valid kdtree distance, it's not dimensionwise additive.
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = std::abs(a[0] - b[0]);
+            diff1 = std::abs(a[1] - b[1]);
+            diff2 = std::abs(a[2] - b[2]);
+            diff3 = std::abs(a[3] - b[3]);
+            if (diff0>result) {result = diff0; }
+            if (diff1>result) {result = diff1; }
+            if (diff2>result) {result = diff2; }
+            if (diff3>result) {result = diff3; }
+            a += 4;
+            b += 4;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            diff0 = std::abs(*a++ - *b++);
+            result = (diff0>result) ? diff0 : result;
+        }
+        return result;
+    }
+
+    /* This distance functor is not dimension-wise additive, which
+     * makes it an invalid kd-tree distance, not implementing the accum_dist method */
+
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/**
+ * Hamming distance functor - counts the bit differences between two strings - useful for the Brief descriptor
+ * bit count of A exclusive XOR'ed with B
+ */
+struct HammingLUT
+{
+    typedef unsigned char ElementType;
+    typedef int ResultType;
+
+    /** this will count the bits in a ^ b
+     */
+    ResultType operator()(const unsigned char* a, const unsigned char* b, int size) const
+    {
+        ResultType result = 0;
+        for (int i = 0; i < size; i++) {
+            result += byteBitsLookUp(a[i] ^ b[i]);
+        }
+        return result;
+    }
+
+
+    /** \brief given a byte, count the bits using a look up table
+     *  \param b the byte to count bits.  The look up table has an entry for all
+     *  values of b, where that entry is the number of bits.
+     *  \return the number of bits in byte b
+     */
+    static unsigned char byteBitsLookUp(unsigned char b)
+    {
+        static const unsigned char table[256]  = {
+            /* 0 */ 0, /* 1 */ 1, /* 2 */ 1, /* 3 */ 2,
+            /* 4 */ 1, /* 5 */ 2, /* 6 */ 2, /* 7 */ 3,
+            /* 8 */ 1, /* 9 */ 2, /* a */ 2, /* b */ 3,
+            /* c */ 2, /* d */ 3, /* e */ 3, /* f */ 4,
+            /* 10 */ 1, /* 11 */ 2, /* 12 */ 2, /* 13 */ 3,
+            /* 14 */ 2, /* 15 */ 3, /* 16 */ 3, /* 17 */ 4,
+            /* 18 */ 2, /* 19 */ 3, /* 1a */ 3, /* 1b */ 4,
+            /* 1c */ 3, /* 1d */ 4, /* 1e */ 4, /* 1f */ 5,
+            /* 20 */ 1, /* 21 */ 2, /* 22 */ 2, /* 23 */ 3,
+            /* 24 */ 2, /* 25 */ 3, /* 26 */ 3, /* 27 */ 4,
+            /* 28 */ 2, /* 29 */ 3, /* 2a */ 3, /* 2b */ 4,
+            /* 2c */ 3, /* 2d */ 4, /* 2e */ 4, /* 2f */ 5,
+            /* 30 */ 2, /* 31 */ 3, /* 32 */ 3, /* 33 */ 4,
+            /* 34 */ 3, /* 35 */ 4, /* 36 */ 4, /* 37 */ 5,
+            /* 38 */ 3, /* 39 */ 4, /* 3a */ 4, /* 3b */ 5,
+            /* 3c */ 4, /* 3d */ 5, /* 3e */ 5, /* 3f */ 6,
+            /* 40 */ 1, /* 41 */ 2, /* 42 */ 2, /* 43 */ 3,
+            /* 44 */ 2, /* 45 */ 3, /* 46 */ 3, /* 47 */ 4,
+            /* 48 */ 2, /* 49 */ 3, /* 4a */ 3, /* 4b */ 4,
+            /* 4c */ 3, /* 4d */ 4, /* 4e */ 4, /* 4f */ 5,
+            /* 50 */ 2, /* 51 */ 3, /* 52 */ 3, /* 53 */ 4,
+            /* 54 */ 3, /* 55 */ 4, /* 56 */ 4, /* 57 */ 5,
+            /* 58 */ 3, /* 59 */ 4, /* 5a */ 4, /* 5b */ 5,
+            /* 5c */ 4, /* 5d */ 5, /* 5e */ 5, /* 5f */ 6,
+            /* 60 */ 2, /* 61 */ 3, /* 62 */ 3, /* 63 */ 4,
+            /* 64 */ 3, /* 65 */ 4, /* 66 */ 4, /* 67 */ 5,
+            /* 68 */ 3, /* 69 */ 4, /* 6a */ 4, /* 6b */ 5,
+            /* 6c */ 4, /* 6d */ 5, /* 6e */ 5, /* 6f */ 6,
+            /* 70 */ 3, /* 71 */ 4, /* 72 */ 4, /* 73 */ 5,
+            /* 74 */ 4, /* 75 */ 5, /* 76 */ 5, /* 77 */ 6,
+            /* 78 */ 4, /* 79 */ 5, /* 7a */ 5, /* 7b */ 6,
+            /* 7c */ 5, /* 7d */ 6, /* 7e */ 6, /* 7f */ 7,
+            /* 80 */ 1, /* 81 */ 2, /* 82 */ 2, /* 83 */ 3,
+            /* 84 */ 2, /* 85 */ 3, /* 86 */ 3, /* 87 */ 4,
+            /* 88 */ 2, /* 89 */ 3, /* 8a */ 3, /* 8b */ 4,
+            /* 8c */ 3, /* 8d */ 4, /* 8e */ 4, /* 8f */ 5,
+            /* 90 */ 2, /* 91 */ 3, /* 92 */ 3, /* 93 */ 4,
+            /* 94 */ 3, /* 95 */ 4, /* 96 */ 4, /* 97 */ 5,
+            /* 98 */ 3, /* 99 */ 4, /* 9a */ 4, /* 9b */ 5,
+            /* 9c */ 4, /* 9d */ 5, /* 9e */ 5, /* 9f */ 6,
+            /* a0 */ 2, /* a1 */ 3, /* a2 */ 3, /* a3 */ 4,
+            /* a4 */ 3, /* a5 */ 4, /* a6 */ 4, /* a7 */ 5,
+            /* a8 */ 3, /* a9 */ 4, /* aa */ 4, /* ab */ 5,
+            /* ac */ 4, /* ad */ 5, /* ae */ 5, /* af */ 6,
+            /* b0 */ 3, /* b1 */ 4, /* b2 */ 4, /* b3 */ 5,
+            /* b4 */ 4, /* b5 */ 5, /* b6 */ 5, /* b7 */ 6,
+            /* b8 */ 4, /* b9 */ 5, /* ba */ 5, /* bb */ 6,
+            /* bc */ 5, /* bd */ 6, /* be */ 6, /* bf */ 7,
+            /* c0 */ 2, /* c1 */ 3, /* c2 */ 3, /* c3 */ 4,
+            /* c4 */ 3, /* c5 */ 4, /* c6 */ 4, /* c7 */ 5,
+            /* c8 */ 3, /* c9 */ 4, /* ca */ 4, /* cb */ 5,
+            /* cc */ 4, /* cd */ 5, /* ce */ 5, /* cf */ 6,
+            /* d0 */ 3, /* d1 */ 4, /* d2 */ 4, /* d3 */ 5,
+            /* d4 */ 4, /* d5 */ 5, /* d6 */ 5, /* d7 */ 6,
+            /* d8 */ 4, /* d9 */ 5, /* da */ 5, /* db */ 6,
+            /* dc */ 5, /* dd */ 6, /* de */ 6, /* df */ 7,
+            /* e0 */ 3, /* e1 */ 4, /* e2 */ 4, /* e3 */ 5,
+            /* e4 */ 4, /* e5 */ 5, /* e6 */ 5, /* e7 */ 6,
+            /* e8 */ 4, /* e9 */ 5, /* ea */ 5, /* eb */ 6,
+            /* ec */ 5, /* ed */ 6, /* ee */ 6, /* ef */ 7,
+            /* f0 */ 4, /* f1 */ 5, /* f2 */ 5, /* f3 */ 6,
+            /* f4 */ 5, /* f5 */ 6, /* f6 */ 6, /* f7 */ 7,
+            /* f8 */ 5, /* f9 */ 6, /* fa */ 6, /* fb */ 7,
+            /* fc */ 6, /* fd */ 7, /* fe */ 7, /* ff */ 8
+        };
+        return table[b];
+    }
+};
+
+/**
+ * Hamming distance functor (pop count between two binary vectors, i.e. xor them and count the number of bits set)
+ * That code was taken from brief.cpp in OpenCV
+ */
+template<class T>
+struct HammingPopcnt
+{
+    typedef T ElementType;
+    typedef int ResultType;
+
+    template<typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        ResultType result = 0;
+#if __GNUC__
+#if ANDROID && HAVE_NEON
+        static uint64_t features = android_getCpuFeatures();
+        if ((features& ANDROID_CPU_ARM_FEATURE_NEON)) {
+            for (size_t i = 0; i < size; i += 16) {
+                uint8x16_t A_vec = vld1q_u8 (a + i);
+                uint8x16_t B_vec = vld1q_u8 (b + i);
+                //uint8x16_t veorq_u8 (uint8x16_t, uint8x16_t)
+                uint8x16_t AxorB = veorq_u8 (A_vec, B_vec);
+
+                uint8x16_t bitsSet += vcntq_u8 (AxorB);
+                //uint16x8_t vpadalq_u8 (uint16x8_t, uint8x16_t)
+                uint16x8_t bitSet8 = vpaddlq_u8 (bitsSet);
+                uint32x4_t bitSet4 = vpaddlq_u16 (bitSet8);
+
+                uint64x2_t bitSet2 = vpaddlq_u32 (bitSet4);
+                result += vgetq_lane_u64 (bitSet2,0);
+                result += vgetq_lane_u64 (bitSet2,1);
+            }
+        }
+        else
+#endif
+        //for portability just use unsigned long -- and use the __builtin_popcountll (see docs for __builtin_popcountll)
+        typedef unsigned long long pop_t;
+        const size_t modulo = size % sizeof(pop_t);
+        const pop_t* a2 = reinterpret_cast<const pop_t*> (a);
+        const pop_t* b2 = reinterpret_cast<const pop_t*> (b);
+        const pop_t* a2_end = a2 + (size / sizeof(pop_t));
+
+        for (; a2 != a2_end; ++a2, ++b2) result += __builtin_popcountll((*a2) ^ (*b2));
+
+        if (modulo) {
+            //in the case where size is not dividable by sizeof(size_t)
+            //need to mask off the bits at the end
+            pop_t a_final = 0, b_final = 0;
+            memcpy(&a_final, a2, modulo);
+            memcpy(&b_final, b2, modulo);
+            result += __builtin_popcountll(a_final ^ b_final);
+        }
+#else
+        HammingLUT lut;
+        result = lut(reinterpret_cast<const unsigned char*> (a),
+                     reinterpret_cast<const unsigned char*> (b), size * sizeof(pop_t));
+#endif
+        return result;
+    }
+};
+
+template<typename T>
+struct Hamming
+{
+    typedef T ElementType;
+    typedef unsigned int ResultType;
+
+    /** This is popcount_3() from:
+     * http://en.wikipedia.org/wiki/Hamming_weight */
+    unsigned int popcnt32(uint32_t n) const
+    {
+        n -= ((n >> 1) & 0x55555555);
+        n = (n & 0x33333333) + ((n >> 2) & 0x33333333);
+        return (((n + (n >> 4))& 0xF0F0F0F)* 0x1010101) >> 24;
+    }
+
+    unsigned int popcnt64(uint64_t n) const
+    {
+        n -= ((n >> 1) & 0x5555555555555555LL);
+        n = (n & 0x3333333333333333LL) + ((n >> 2) & 0x3333333333333333LL);
+        return (((n + (n >> 4))& 0x0f0f0f0f0f0f0f0fLL)* 0x0101010101010101LL) >> 56;
+    }
+
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = 0) const
+    {
+#ifdef FLANN_PLATFORM_64_BIT
+        const uint64_t* pa = reinterpret_cast<const uint64_t*>(a);
+        const uint64_t* pb = reinterpret_cast<const uint64_t*>(b);
+        ResultType result = 0;
+        size /= (sizeof(uint64_t)/sizeof(unsigned char));
+        for(size_t i = 0; i < size; ++i ) {
+            result += popcnt64(*pa ^ *pb);
+            ++pa;
+            ++pb;
+        }
+#else
+        const uint32_t* pa = reinterpret_cast<const uint32_t*>(a);
+        const uint32_t* pb = reinterpret_cast<const uint32_t*>(b);
+        ResultType result = 0;
+        size /= (sizeof(uint32_t)/sizeof(unsigned char));
+        for(size_t i = 0; i < size; ++i ) {
+        	result += popcnt32(*pa ^ *pb);
+        	++pa;
+        	++pb;
+        }
+#endif
+        return result;
+    }
+};
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+template<class T>
+struct HistIntersectionDistance
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the histogram intersection distance
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType min0, min1, min2, min3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            min0 = (ResultType)(a[0] < b[0] ? a[0] : b[0]);
+            min1 = (ResultType)(a[1] < b[1] ? a[1] : b[1]);
+            min2 = (ResultType)(a[2] < b[2] ? a[2] : b[2]);
+            min3 = (ResultType)(a[3] < b[3] ? a[3] : b[3]);
+            result += min0 + min1 + min2 + min3;
+            a += 4;
+            b += 4;
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        /* Process last 0-3 pixels.  Not needed for standard vector lengths. */
+        while (a < last) {
+            min0 = (ResultType)(*a < *b ? *a : *b);
+            result += min0;
+            ++a;
+            ++b;
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        return a<b ? a : b;
+    }
+};
+
+
+
+template<class T>
+struct HellingerDistance
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the Hellinger distance
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType diff0, diff1, diff2, diff3;
+        Iterator1 last = a + size;
+        Iterator1 lastgroup = last - 3;
+
+        /* Process 4 items with each loop for efficiency. */
+        while (a < lastgroup) {
+            diff0 = sqrt(static_cast<ResultType>(a[0])) - sqrt(static_cast<ResultType>(b[0]));
+            diff1 = sqrt(static_cast<ResultType>(a[1])) - sqrt(static_cast<ResultType>(b[1]));
+            diff2 = sqrt(static_cast<ResultType>(a[2])) - sqrt(static_cast<ResultType>(b[2]));
+            diff3 = sqrt(static_cast<ResultType>(a[3])) - sqrt(static_cast<ResultType>(b[3]));
+            result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
+            a += 4;
+            b += 4;
+        }
+        while (a < last) {
+            diff0 = sqrt(static_cast<ResultType>(*a++)) - sqrt(static_cast<ResultType>(*b++));
+            result += diff0 * diff0;
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        ResultType dist = sqrt(static_cast<ResultType>(a)) - sqrt(static_cast<ResultType>(b));
+        return dist * dist;
+    }
+};
+
+
+template<class T>
+struct ChiSquareDistance
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the chi-square distance
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        ResultType sum, diff;
+        Iterator1 last = a + size;
+
+        while (a < last) {
+            sum = (ResultType)(*a + *b);
+            if (sum>0) {
+                diff = (ResultType)(*a - *b);
+                result += diff*diff/sum;
+            }
+            ++a;
+            ++b;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        ResultType result = ResultType();
+        ResultType sum, diff;
+
+        sum = (ResultType)(a+b);
+        if (sum>0) {
+            diff = (ResultType)(a-b);
+            result = diff*diff/sum;
+        }
+        return result;
+    }
+};
+
+
+template<class T>
+struct KL_Divergence
+{
+    typedef bool is_kdtree_distance;
+
+    typedef T ElementType;
+    typedef typename Accumulator<T>::Type ResultType;
+
+    /**
+     *  Compute the Kullback–Leibler divergence
+     */
+    template <typename Iterator1, typename Iterator2>
+    ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const
+    {
+        ResultType result = ResultType();
+        Iterator1 last = a + size;
+
+        while (a < last) {
+            if ( *a != 0 && *b != 0 ) {
+                ResultType ratio = (ResultType)(*a / *b);
+                if (ratio>0) {
+                    result += *a * log(ratio);
+                }
+            }
+            ++a;
+            ++b;
+
+            if ((worst_dist>0)&&(result>worst_dist)) {
+                return result;
+            }
+        }
+        return result;
+    }
+
+    /**
+     * Partial distance, used by the kd-tree.
+     */
+    template <typename U, typename V>
+    inline ResultType accum_dist(const U& a, const V& b, int) const
+    {
+        ResultType result = ResultType();
+        if( a != 0 && b != 0 ) {
+            ResultType ratio = (ResultType)(a / b);
+            if (ratio>0) {
+                result = a * log(ratio);
+            }
+		}
+        return result;
+    }
+};
+
+}
+
+#endif //FLANN_DIST_H_

corelib/src/flann/algorithms/hierarchical_clustering_index.h

@@ -0,0 +1,724 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). 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 FLANN_HIERARCHICAL_CLUSTERING_INDEX_H_
+#define FLANN_HIERARCHICAL_CLUSTERING_INDEX_H_
+
+#include <algorithm>
+#include <string>
+#include <map>
+#include <cassert>
+#include <limits>
+#include <cmath>
+
+#ifndef SIZE_MAX
+#define SIZE_MAX ((size_t) -1)
+#endif
+
+#include "flann/general.h"
+#include "flann/algorithms/nn_index.h"
+#include "flann/algorithms/dist.h"
+#include "flann/util/matrix.h"
+#include "flann/util/result_set.h"
+#include "flann/util/heap.h"
+#include "flann/util/allocator.h"
+#include "flann/util/random.h"
+#include "flann/util/saving.h"
+#include "flann/util/serialization.h"
+
+namespace flann
+{
+
+struct HierarchicalClusteringIndexParams : public IndexParams
+{
+    HierarchicalClusteringIndexParams(int branching = 32,
+                                      flann_centers_init_t centers_init = FLANN_CENTERS_RANDOM,
+                                      int trees = 4, int leaf_max_size = 100)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_HIERARCHICAL;
+        // The branching factor used in the hierarchical clustering
+        (*this)["branching"] = branching;
+        // Algorithm used for picking the initial cluster centers
+        (*this)["centers_init"] = centers_init;
+        // number of parallel trees to build
+        (*this)["trees"] = trees;
+        // maximum leaf size
+        (*this)["leaf_max_size"] = leaf_max_size;
+    }
+};
+
+
+
+/**
+ * Hierarchical index
+ *
+ * Contains a tree constructed through a hierarchical clustering
+ * and other information for indexing a set of points for nearest-neighbour matching.
+ */
+template <typename Distance>
+class HierarchicalClusteringIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    /**
+     * Constructor.
+     *
+     * @param index_params
+     * @param d
+     */
+    HierarchicalClusteringIndex(const IndexParams& index_params = HierarchicalClusteringIndexParams(), Distance d = Distance())
+        : BaseClass(index_params, d)
+    {
+        memoryCounter_ = 0;
+
+        branching_ = get_param(index_params_,"branching",32);
+        centers_init_ = get_param(index_params_,"centers_init", FLANN_CENTERS_RANDOM);
+        trees_ = get_param(index_params_,"trees",4);
+        leaf_max_size_ = get_param(index_params_,"leaf_max_size",100);
+
+        initCenterChooser();
+    }
+
+
+    /**
+     * Index constructor
+     *
+     * Params:
+     *          inputData = dataset with the input features
+     *          params = parameters passed to the hierarchical k-means algorithm
+     */
+    HierarchicalClusteringIndex(const Matrix<ElementType>& inputData, const IndexParams& index_params = HierarchicalClusteringIndexParams(),
+                                Distance d = Distance())
+        : BaseClass(index_params, d)
+    {
+        memoryCounter_ = 0;
+
+        branching_ = get_param(index_params_,"branching",32);
+        centers_init_ = get_param(index_params_,"centers_init", FLANN_CENTERS_RANDOM);
+        trees_ = get_param(index_params_,"trees",4);
+        leaf_max_size_ = get_param(index_params_,"leaf_max_size",100);
+
+        initCenterChooser();
+        
+        setDataset(inputData);
+
+        chooseCenters_->setDataSize(veclen_);
+    }
+
+
+    HierarchicalClusteringIndex(const HierarchicalClusteringIndex& other) : BaseClass(other),
+    		memoryCounter_(other.memoryCounter_),
+    		branching_(other.branching_),
+    		trees_(other.trees_),
+    		centers_init_(other.centers_init_),
+    		leaf_max_size_(other.leaf_max_size_)
+
+    {
+    	initCenterChooser();
+        tree_roots_.resize(other.tree_roots_.size());
+        for (size_t i=0;i<tree_roots_.size();++i) {
+        	copyTree(tree_roots_[i], other.tree_roots_[i]);
+        }
+    }
+
+    HierarchicalClusteringIndex& operator=(HierarchicalClusteringIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+
+    void initCenterChooser()
+    {
+        switch(centers_init_) {
+        case FLANN_CENTERS_RANDOM:
+        	chooseCenters_ = new RandomCenterChooser<Distance>(distance_, points_);
+        	break;
+        case FLANN_CENTERS_GONZALES:
+        	chooseCenters_ = new GonzalesCenterChooser<Distance>(distance_, points_);
+        	break;
+        case FLANN_CENTERS_KMEANSPP:
+            chooseCenters_ = new KMeansppCenterChooser<Distance>(distance_, points_);
+        	break;
+        case FLANN_CENTERS_GROUPWISE:
+            chooseCenters_ = new GroupWiseCenterChooser<Distance>(distance_, points_);
+            break;
+        default:
+            throw FLANNException("Unknown algorithm for choosing initial centers.");
+        }
+    }
+
+    /**
+     * Index destructor.
+     *
+     * Release the memory used by the index.
+     */
+    virtual ~HierarchicalClusteringIndex()
+    {
+    	delete chooseCenters_;
+    	freeIndex();
+    }
+
+    BaseClass* clone() const
+    {
+    	return new HierarchicalClusteringIndex(*this);
+    }
+
+    /**
+     * Computes the inde memory usage
+     * Returns: memory used by the index
+     */
+    int usedMemory() const
+    {
+        return pool_.usedMemory+pool_.wastedMemory+memoryCounter_;
+    }
+    
+    using BaseClass::buildIndex;
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+        size_t old_size = size_;
+
+        extendDataset(points);
+        
+        if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
+            buildIndex();
+        }
+        else {
+            for (size_t i=0;i<points.rows;++i) {
+                for (int j = 0; j < trees_; j++) {
+                    addPointToTree(tree_roots_[j], old_size + i);
+                }
+            }            
+        }
+    }
+
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_HIERARCHICAL;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	ar & branching_;
+    	ar & trees_;
+    	ar & centers_init_;
+    	ar & leaf_max_size_;
+
+    	if (Archive::is_loading::value) {
+    		tree_roots_.resize(trees_);
+    	}
+    	for (size_t i=0;i<tree_roots_.size();++i) {
+    		if (Archive::is_loading::value) {
+    			tree_roots_[i] = new(pool_) Node();
+    		}
+    		ar & *tree_roots_[i];
+    	}
+
+    	if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+            index_params_["branching"] = branching_;
+            index_params_["trees"] = trees_;
+            index_params_["centers_init"] = centers_init_;
+            index_params_["leaf_size"] = leaf_max_size_;
+    	}
+    }
+
+    void saveIndex(FILE* stream)
+    {
+    	serialization::SaveArchive sa(stream);
+    	sa & *this;
+    }
+
+
+    void loadIndex(FILE* stream)
+    {
+    	serialization::LoadArchive la(stream);
+    	la & *this;
+    }
+
+
+    /**
+     * Find set of nearest neighbors to vec. 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
+     *     searchParams = parameters that influence the search algorithm (checks)
+     */
+
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+    	if (removed_) {
+    		findNeighborsWithRemoved<true>(result, vec, searchParams);
+    	}
+    	else {
+    		findNeighborsWithRemoved<false>(result, vec, searchParams);
+    	}
+    }
+
+protected:
+
+    /**
+     * Builds the index
+     */
+    void buildIndexImpl()
+    {
+        chooseCenters_->setDataSize(veclen_);
+
+        if (branching_<2) {
+            throw FLANNException("Branching factor must be at least 2");
+        }
+        tree_roots_.resize(trees_);
+        std::vector<int> indices(size_);
+        for (int i=0; i<trees_; ++i) {
+            for (size_t j=0; j<size_; ++j) {
+                indices[j] = j;
+            }
+            tree_roots_[i] = new(pool_) Node();
+            computeClustering(tree_roots_[i], &indices[0], size_);
+        }
+    }
+
+private:
+
+    struct PointInfo
+    {
+    	/** Point index */
+    	size_t index;
+    	/** Point data */
+    	ElementType* point;
+
+    private:
+    	template<typename Archive>
+    	void serialize(Archive& ar)
+    	{
+    		typedef HierarchicalClusteringIndex<Distance> Index;
+    		Index* obj = static_cast<Index*>(ar.getObject());
+
+    		ar & index;
+//    		ar & point;
+
+			if (Archive::is_loading::value) {
+				point = obj->points_[index];
+			}
+    	}
+    	friend struct serialization::access;
+    };
+
+    /**
+     * Struture representing a node in the hierarchical k-means tree.
+     */
+    struct Node
+    {
+        /**
+         * The cluster center
+         */
+    	ElementType* pivot;
+    	size_t pivot_index;
+        /**
+         * Child nodes (only for non-terminal nodes)
+         */
+        std::vector<Node*> childs;
+        /**
+         * Node points (only for terminal nodes)
+         */
+        std::vector<PointInfo> points;
+
+		Node(){
+			pivot = NULL;
+			pivot_index = SIZE_MAX;
+		}
+        /**
+         * destructor
+         * calling Node destructor explicitly
+         */
+        ~Node()
+        {
+        	for(size_t i=0; i<childs.size(); i++){
+        		childs[i]->~Node();
+				pivot = NULL;
+				pivot_index = -1;
+        	}
+        };
+
+    private:
+    	template<typename Archive>
+    	void serialize(Archive& ar)
+    	{
+    		typedef HierarchicalClusteringIndex<Distance> Index;
+    		Index* obj = static_cast<Index*>(ar.getObject());
+    		ar & pivot_index;
+    		if (Archive::is_loading::value) {
+				if (pivot_index != SIZE_MAX)
+					pivot = obj->points_[pivot_index];
+				else
+					pivot = NULL;
+    		}
+    		size_t childs_size;
+    		if (Archive::is_saving::value) {
+    			childs_size = childs.size();
+    		}
+    		ar & childs_size;
+
+    		if (childs_size==0) {
+    			ar & points;
+    		}
+    		else {
+    			if (Archive::is_loading::value) {
+    				childs.resize(childs_size);
+    			}
+    			for (size_t i=0;i<childs_size;++i) {
+    				if (Archive::is_loading::value) {
+    					childs[i] = new(obj->pool_) Node();
+    				}
+    				ar & *childs[i];
+    			}
+    		}
+
+    	}
+    	friend struct serialization::access;
+    };
+    typedef Node* NodePtr;
+
+
+
+    /**
+     * Alias definition for a nicer syntax.
+     */
+    typedef BranchStruct<NodePtr, DistanceType> BranchSt;
+
+
+    /**
+     * Clears Node tree
+     * calling Node destructor explicitly
+     */
+    void freeIndex(){
+    	for (size_t i=0; i<tree_roots_.size(); ++i) {
+    		tree_roots_[i]->~Node();
+    	}
+    	pool_.free();
+    }
+
+    void copyTree(NodePtr& dst, const NodePtr& src)
+    {
+    	dst = new(pool_) Node();
+    	dst->pivot_index = src->pivot_index;
+    	dst->pivot = points_[dst->pivot_index];
+
+    	if (src->childs.size()==0) {
+    		dst->points = src->points;
+    	}
+    	else {
+    		dst->childs.resize(src->childs.size());
+    		for (size_t i=0;i<src->childs.size();++i) {
+    			copyTree(dst->childs[i], src->childs[i]);
+    		}
+    	}
+    }
+
+
+
+    void computeLabels(int* indices, int indices_length,  int* centers, int centers_length, int* labels, DistanceType& cost)
+    {
+        cost = 0;
+        for (int i=0; i<indices_length; ++i) {
+            ElementType* point = points_[indices[i]];
+            DistanceType dist = distance_(point, points_[centers[0]], veclen_);
+            labels[i] = 0;
+            for (int j=1; j<centers_length; ++j) {
+                DistanceType new_dist = distance_(point, points_[centers[j]], veclen_);
+                if (dist>new_dist) {
+                    labels[i] = j;
+                    dist = new_dist;
+                }
+            }
+            cost += dist;
+        }
+    }
+
+    /**
+     * The method responsible with actually doing the recursive hierarchical
+     * clustering
+     *
+     * Params:
+     *     node = the node to cluster
+     *     indices = indices of the points belonging to the current node
+     *     branching = the branching factor to use in the clustering
+     *
+     */
+    void computeClustering(NodePtr node, int* indices, int indices_length)
+    {
+        if (indices_length < leaf_max_size_) { // leaf node
+            node->points.resize(indices_length);
+            for (int i=0;i<indices_length;++i) {
+            	node->points[i].index = indices[i];
+            	node->points[i].point = points_[indices[i]];
+            }
+            node->childs.clear();
+            return;
+        }
+
+        std::vector<int> centers(branching_);
+        std::vector<int> labels(indices_length);
+
+        int centers_length;
+        (*chooseCenters_)(branching_, indices, indices_length, &centers[0], centers_length);
+
+        if (centers_length<branching_) {
+            node->points.resize(indices_length);
+            for (int i=0;i<indices_length;++i) {
+            	node->points[i].index = indices[i];
+            	node->points[i].point = points_[indices[i]];
+            }
+            node->childs.clear();
+            return;
+        }
+
+
+        //  assign points to clusters
+        DistanceType cost;
+        computeLabels(indices, indices_length, &centers[0], centers_length, &labels[0], cost);
+
+        node->childs.resize(branching_);
+        int start = 0;
+        int end = start;
+        for (int i=0; i<branching_; ++i) {
+            for (int j=0; j<indices_length; ++j) {
+                if (labels[j]==i) {
+                    std::swap(indices[j],indices[end]);
+                    std::swap(labels[j],labels[end]);
+                    end++;
+                }
+            }
+
+            node->childs[i] = new(pool_) Node();
+            node->childs[i]->pivot_index = centers[i];
+            node->childs[i]->pivot = points_[centers[i]];
+            node->childs[i]->points.clear();
+            computeClustering(node->childs[i],indices+start, end-start);
+            start=end;
+        }
+    }
+
+
+    template<bool with_removed>
+    void findNeighborsWithRemoved(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        int maxChecks = searchParams.checks;
+
+        // Priority queue storing intermediate branches in the best-bin-first search
+        Heap<BranchSt>* heap = new Heap<BranchSt>(size_);
+
+        DynamicBitset checked(size_);
+        int checks = 0;
+        for (int i=0; i<trees_; ++i) {
+            findNN<with_removed>(tree_roots_[i], result, vec, checks, maxChecks, heap, checked);
+        }
+
+        BranchSt branch;
+        while (heap->popMin(branch) && (checks<maxChecks || !result.full())) {
+            NodePtr node = branch.node;
+            findNN<with_removed>(node, result, vec, checks, maxChecks, heap, checked);
+        }
+
+        delete heap;
+    }
+
+
+    /**
+     * Performs one descent in the hierarchical k-means tree. The branches not
+     * visited are stored in a priority queue.
+     *
+     * Params:
+     *      node = node to explore
+     *      result = container for the k-nearest neighbors found
+     *      vec = query points
+     *      checks = how many points in the dataset have been checked so far
+     *      maxChecks = maximum dataset points to checks
+     */
+
+    template<bool with_removed>
+    void findNN(NodePtr node, ResultSet<DistanceType>& result, const ElementType* vec, int& checks, int maxChecks,
+                Heap<BranchSt>* heap,  DynamicBitset& checked) const
+    {
+        if (node->childs.empty()) {
+            if (checks>=maxChecks) {
+                if (result.full()) return;
+            }
+
+            for (size_t i=0; i<node->points.size(); ++i) {
+            	PointInfo& pointInfo = node->points[i];
+            	if (with_removed) {
+            		if (removed_points_.test(pointInfo.index)) continue;
+            	}
+                if (checked.test(pointInfo.index)) continue;
+                DistanceType dist = distance_(pointInfo.point, vec, veclen_);
+                result.addPoint(dist, pointInfo.index);
+                checked.set(pointInfo.index);
+                ++checks;
+            }
+        }
+        else {
+            DistanceType* domain_distances = new DistanceType[branching_];
+            int best_index = 0;
+            domain_distances[best_index] = distance_(vec, node->childs[best_index]->pivot, veclen_);
+            for (int i=1; i<branching_; ++i) {
+                domain_distances[i] = distance_(vec, node->childs[i]->pivot, veclen_);
+                if (domain_distances[i]<domain_distances[best_index]) {
+                    best_index = i;
+                }
+            }
+            for (int i=0; i<branching_; ++i) {
+                if (i!=best_index) {
+                    heap->insert(BranchSt(node->childs[i],domain_distances[i]));
+                }
+            }
+            delete[] domain_distances;
+            findNN<with_removed>(node->childs[best_index],result,vec, checks, maxChecks, heap, checked);
+        }
+    }
+    
+    void addPointToTree(NodePtr node, size_t index)
+    {
+        ElementType* point = points_[index];
+        
+        if (node->childs.empty()) { // leaf node
+        	PointInfo pointInfo;
+        	pointInfo.point = point;
+        	pointInfo.index = index;
+            node->points.push_back(pointInfo);
+
+            if (node->points.size()>=size_t(branching_)) {
+                std::vector<int> indices(node->points.size());
+
+                for (size_t i=0;i<node->points.size();++i) {
+                	indices[i] = node->points[i].index;
+                }
+                computeClustering(node, &indices[0], indices.size());
+            }
+        }
+        else {            
+            // find the closest child
+            int closest = 0;
+            ElementType* center = node->childs[closest]->pivot;
+            DistanceType dist = distance_(center, point, veclen_);
+            for (size_t i=1;i<size_t(branching_);++i) {
+                center = node->childs[i]->pivot;
+                DistanceType crt_dist = distance_(center, point, veclen_);
+                if (crt_dist<dist) {
+                    dist = crt_dist;
+                    closest = i;
+                }
+            }
+            addPointToTree(node->childs[closest], index);
+        }                
+    }
+
+    void swap(HierarchicalClusteringIndex& other)
+    {
+    	BaseClass::swap(other);
+
+    	std::swap(tree_roots_, other.tree_roots_);
+    	std::swap(pool_, other.pool_);
+    	std::swap(memoryCounter_, other.memoryCounter_);
+    	std::swap(branching_, other.branching_);
+    	std::swap(trees_, other.trees_);
+    	std::swap(centers_init_, other.centers_init_);
+    	std::swap(leaf_max_size_, other.leaf_max_size_);
+    	std::swap(chooseCenters_, other.chooseCenters_);
+    }
+
+private:
+
+    /**
+     * The root nodes in the tree.
+     */
+    std::vector<Node*> tree_roots_;
+
+    /**
+     * 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_;
+
+    /**
+     * Memory occupied by the index.
+     */
+    int memoryCounter_;
+
+    /** index parameters */
+    /**
+     * Branching factor to use for clustering
+     */
+    int branching_;
+    
+    /**
+     * How many parallel trees to build
+     */
+    int trees_;
+    
+    /**
+     * Algorithm to use for choosing cluster centers
+     */
+    flann_centers_init_t centers_init_;
+    
+    /**
+     * Max size of leaf nodes
+     */
+    int leaf_max_size_;
+    
+    /**
+     * Algorithm used to choose initial centers
+     */
+    CenterChooser<Distance>* chooseCenters_;
+
+    USING_BASECLASS_SYMBOLS
+};
+
+}
+
+#endif /* FLANN_HIERARCHICAL_CLUSTERING_INDEX_H_ */

corelib/src/flann/algorithms/kdtree_cuda_3d_index.cu

@@ -0,0 +1,842 @@
+/**********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2011       Andreas Muetzel (amuetzel@uni-koblenz.de). 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.
+ *************************************************************************/
+
+#include "kdtree_cuda_3d_index.h"
+#include <flann/algorithms/dist.h>
+#include <flann/util/cuda/result_set.h>
+// #define THRUST_DEBUG 1
+#include <cuda.h>
+#include <thrust/copy.h>
+#include <thrust/device_vector.h>
+#include <vector_types.h>
+#include <flann/util/cutil_math.h>
+#include <thrust/host_vector.h>
+#include <thrust/copy.h>
+#include <flann/util/cuda/heap.h>
+#include <thrust/scan.h>
+#include <thrust/count.h>
+#include <flann/algorithms/kdtree_cuda_builder.h>
+#include <vector_types.h>
+namespace flann
+{
+
+namespace KdTreeCudaPrivate
+{
+template< typename GPUResultSet, typename Distance >
+__device__
+void searchNeighbors(const cuda::kd_tree_builder_detail::SplitInfo* splits,
+                     const int* child1,
+                     const int* parent,
+                     const float4* aabbLow,
+                     const float4* aabbHigh, const float4* elements, const float4& q, GPUResultSet& result, const Distance& distance = Distance() )
+{
+
+    bool backtrack=false;
+    int lastNode=-1;
+    int current=0;
+
+    cuda::kd_tree_builder_detail::SplitInfo split;
+    while(true) {
+        if( current==-1 ) break;
+        split = splits[current];
+
+        float diff1;
+        if( split.split_dim==0 ) diff1=q.x- split.split_val;
+        else if( split.split_dim==1 ) diff1=q.y- split.split_val;
+        else if( split.split_dim==2 ) diff1=q.z- split.split_val;
+
+        // children are next to each other: leftChild+1 == rightChild
+        int leftChild= child1[current];
+        int bestChild=leftChild;
+        int otherChild=leftChild;
+
+        if ((diff1)<0) {
+            otherChild++;
+        }
+        else {
+            bestChild++;
+        }
+
+        if( !backtrack ) {
+            /* If this is a leaf node, then do check and return. */
+            if (leftChild==-1) {
+                for (int i=split.left; i<split.right; ++i) {
+                    float dist=distance.dist(elements[i],q);
+                    result.insert(i,dist);
+                }
+                backtrack=true;
+                lastNode=current;
+                current=parent[current];
+            }
+            else { // go to closer child node
+                lastNode=current;
+                current=bestChild;
+            }
+        }
+        else { // continue moving back up the tree or visit far node?
+              // minimum possible distance between query point and a point inside the AABB
+            float mindistsq=0;
+            float4 aabbMin=aabbLow[otherChild];
+            float4 aabbMax=aabbHigh[otherChild];
+
+            if( q.x < aabbMin.x ) mindistsq+=distance.axisDist(q.x, aabbMin.x);
+            else if( q.x > aabbMax.x ) mindistsq+=distance.axisDist(q.x, aabbMax.x);
+            if( q.y < aabbMin.y ) mindistsq+=distance.axisDist(q.y, aabbMin.y);
+            else if( q.y > aabbMax.y ) mindistsq+=distance.axisDist(q.y, aabbMax.y);
+            if( q.z < aabbMin.z ) mindistsq+=distance.axisDist(q.z, aabbMin.z);
+            else if( q.z > aabbMax.z ) mindistsq+=distance.axisDist(q.z, aabbMax.z);
+
+            //  the far node was NOT the last node (== not visited yet) AND there could be a closer point in it
+            if(( lastNode==bestChild) && (mindistsq <= result.worstDist() ) ) {
+                lastNode=current;
+                current=otherChild;
+                backtrack=false;
+            }
+            else {
+                lastNode=current;
+                current=parent[current];
+            }
+        }
+
+    }
+}
+
+
+template< typename GPUResultSet, typename Distance >
+__global__
+void nearestKernel(const cuda::kd_tree_builder_detail::SplitInfo* splits,
+                   const int* child1,
+                   const int* parent,
+                   const float4* aabbMin,
+                   const float4* aabbMax, const float4* elements, const float* query, int stride, int resultStride, int* resultIndex, float* resultDist, int querysize, GPUResultSet result, Distance dist = Distance())
+{
+    typedef float DistanceType;
+    typedef float ElementType;
+    //                  typedef DistanceType float;
+    size_t tid = blockDim.x*blockIdx.x + threadIdx.x;
+
+    if( tid >= querysize ) return;
+
+    float4 q = make_float4(query[tid*stride],query[tid*stride+1],query[tid*stride+2],0);
+
+    result.setResultLocation( resultDist, resultIndex, tid, resultStride );
+
+    searchNeighbors(splits,child1,parent,aabbMin,aabbMax,elements, q, result, dist);
+
+    result.finish();
+}
+
+}
+
+//! contains some pointers that use cuda data types and that cannot be easily
+//! forward-declared.
+//! basically it contains all GPU buffers
+template<typename Distance>
+struct KDTreeCuda3dIndex<Distance>::GpuHelper
+{
+    thrust::device_vector< cuda::kd_tree_builder_detail::SplitInfo >* gpu_splits_;
+    thrust::device_vector< int >* gpu_parent_;
+    thrust::device_vector< int >* gpu_child1_;
+    thrust::device_vector< float4 >* gpu_aabb_min_;
+    thrust::device_vector< float4 >* gpu_aabb_max_;
+    thrust::device_vector<float4>* gpu_points_;
+    thrust::device_vector<int>* gpu_vind_;
+    GpuHelper() :  gpu_splits_(0), gpu_parent_(0), gpu_child1_(0), gpu_aabb_min_(0), gpu_aabb_max_(0), gpu_points_(0), gpu_vind_(0){
+    }
+    ~GpuHelper()
+    {
+        delete gpu_splits_;
+        gpu_splits_=0;
+        delete gpu_parent_;
+        gpu_parent_=0;
+        delete gpu_child1_;
+        gpu_child1_=0;
+        delete gpu_aabb_max_;
+        gpu_aabb_max_=0;
+        delete gpu_aabb_min_;
+        gpu_aabb_min_=0;
+        delete gpu_vind_;
+        gpu_vind_=0;
+
+        delete gpu_points_;
+        gpu_points_=0;
+    }
+};
+
+//! thrust transform functor
+//! transforms indices in the internal data set back to the original indices
+struct map_indices
+{
+    const int* v_;
+
+    map_indices(const int* v) : v_(v) {
+    }
+
+    __host__ __device__
+    float operator() (const int&i) const
+    {
+        if( i>= 0 ) return v_[i];
+        else return i;
+    }
+};
+
+//! implementation of L2 distance for the CUDA kernels
+struct CudaL2
+{
+
+    static float
+    __host__ __device__
+    axisDist( float a, float b )
+    {
+        return (a-b)*(a-b);
+    }
+
+    static float
+    __host__ __device__
+    dist( float4 a, float4 b )
+    {
+        float4 diff = a-b;
+        return dot(diff,diff);
+    }
+};
+
+//! implementation of L1 distance for the CUDA kernels
+//! NOT TESTED!
+struct CudaL1
+{
+
+    static float
+    __host__ __device__
+    axisDist( float a, float b )
+    {
+        return fabs(a-b);
+    }
+
+    static float
+    __host__ __device__
+    dist( float4 a, float4 b )
+    {
+        return fabs(a.x-b.x)+fabs (a.y-b.y)+( a.z-b.z)+(a.w-b.w);
+    }
+};
+
+//! used to adapt CPU and GPU distance types.
+//! specializations define the ::type as their corresponding GPU distance type
+//! \see GpuDistance< L2<float> >, GpuDistance< L2_Simple<float> >
+template< class Distance >
+struct GpuDistance
+{
+};
+
+template<>
+struct GpuDistance< L2<float> >
+{
+    typedef CudaL2 type;
+};
+
+template<>
+struct GpuDistance< L2_Simple<float> >
+{
+    typedef CudaL2 type;
+};
+template<>
+struct GpuDistance< L1<float> >
+{
+    typedef CudaL1 type;
+};
+
+
+template< typename Distance >
+void KDTreeCuda3dIndex<Distance>::knnSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, size_t knn, const SearchParams& params) const
+{
+    assert(indices.rows >= queries.rows);
+    assert(dists.rows >= queries.rows);
+    assert(int(indices.cols) >= knn);
+    assert( dists.cols == indices.cols && dists.stride==indices.stride );
+    
+    int istride=queries.stride/sizeof(ElementType);
+    int ostride=indices.stride/4;
+
+    bool matrices_on_gpu = params.matrices_in_gpu_ram;
+
+    int threadsPerBlock = 128;
+    int blocksPerGrid=(queries.rows+threadsPerBlock-1)/threadsPerBlock;
+
+    float epsError = 1+params.eps;
+    bool sorted = params.sorted;
+    bool use_heap = params.use_heap;
+
+    typename GpuDistance<Distance>::type distance;
+//       std::cout<<" search: "<<std::endl;
+//       std::cout<<"  rows: "<<indices.rows<<" "<<dists.rows<<" "<<queries.rows<<std::endl;
+//       std::cout<<"  cols: "<<indices.cols<<" "<<dists.cols<<" "<<queries.cols<<std::endl;
+//       std::cout<<"  stride: "<<indices.stride<<" "<<dists.stride<<" "<<queries.stride<<std::endl;
+//       std::cout<<"  stride2:"<<istride<<" "<<ostride<<std::endl;
+//       std::cout<<"  knn:"<<knn<<"  matrices_on_gpu:"<<matrices_on_gpu<<std::endl;
+
+    if( !matrices_on_gpu ) {
+        thrust::device_vector<float> queriesDev(istride* queries.rows,0);
+        thrust::copy( queries.ptr(), queries.ptr()+istride*queries.rows, queriesDev.begin() );
+        thrust::device_vector<float> distsDev(queries.rows* ostride);
+        thrust::device_vector<int> indicesDev(queries.rows* ostride);
+
+
+
+        if( knn==1  ) {
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                                  istride,
+                                                                                  ostride,
+                                                                                  thrust::raw_pointer_cast(&indicesDev[0]),
+                                                                                  thrust::raw_pointer_cast(&distsDev[0]),
+                                                                                  queries.rows, flann::cuda::SingleResultSet<float>(epsError),distance);
+            //                          KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_nodes_)[0])),
+            //                                                                                                                                                                                                                                                                                          thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+            //                                                                                                                                                                                                                                                                                          thrust::raw_pointer_cast(&queriesDev[0]),
+            //                                                                                                                                                                                                                                                                                          queries.stride,
+            //                                                                                                                                                                                                                                                                                          thrust::raw_pointer_cast(&indicesDev[0]),
+            //                                                                                                                                                                                                                                                                                          thrust::raw_pointer_cast(&distsDev[0]),
+            //                                                                                                                                                                                                                                                                                          queries.rows, epsError);
+            //
+        }
+        else {
+            if( use_heap ) {
+                KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                      thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                      thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                                      istride,
+                                                                                      ostride,
+                                                                                      thrust::raw_pointer_cast(&indicesDev[0]),
+                                                                                      thrust::raw_pointer_cast(&distsDev[0]),
+                                                                                      queries.rows, flann::cuda::KnnResultSet<float, true>(knn,sorted,epsError)
+                                                                                      , distance);
+            }
+            else {
+                KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                      thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                      thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                                      istride,
+                                                                                      ostride,
+                                                                                      thrust::raw_pointer_cast(&indicesDev[0]),
+                                                                                      thrust::raw_pointer_cast(&distsDev[0]),
+                                                                                      queries.rows, flann::cuda::KnnResultSet<float, false>(knn,sorted,epsError),
+                                                                                      distance
+                                                                                      );
+            }
+        }
+        thrust::copy( distsDev.begin(), distsDev.end(), dists.ptr() );
+        thrust::transform(indicesDev.begin(), indicesDev.end(), indicesDev.begin(), map_indices(thrust::raw_pointer_cast( &((*gpu_helper_->gpu_vind_))[0]) ));
+        thrust::copy( indicesDev.begin(), indicesDev.end(), indices.ptr() );
+    }
+    else {
+        thrust::device_ptr<float> qd = thrust::device_pointer_cast(queries.ptr());
+        thrust::device_ptr<float> dd = thrust::device_pointer_cast(dists.ptr());
+        thrust::device_ptr<int> id = thrust::device_pointer_cast(indices.ptr());
+
+
+
+        if( knn==1  ) {
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  qd.get(),
+                                                                                  istride,
+                                                                                  ostride,
+                                                                                  id.get(),
+                                                                                  dd.get(),
+                                                                                  queries.rows, flann::cuda::SingleResultSet<float>(epsError),distance);
+            //                          KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_nodes_)[0])),
+            //                                                                                                                                                                                                                                                                                          thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+            //                                                                                                                                                                                                                                                                                          thrust::raw_pointer_cast(&queriesDev[0]),
+            //                                                                                                                                                                                                                                                                                          queries.stride,
+            //                                                                                                                                                                                                                                                                                          thrust::raw_pointer_cast(&indicesDev[0]),
+            //                                                                                                                                                                                                                                                                                          thrust::raw_pointer_cast(&distsDev[0]),
+            //                                                                                                                                                                                                                                                                                          queries.rows, epsError);
+            //
+        }
+        else {
+            if( use_heap ) {
+                KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                      thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                      qd.get(),
+                                                                                      istride,
+                                                                                      ostride,
+                                                                                      id.get(),
+                                                                                      dd.get(),
+                                                                                      queries.rows, flann::cuda::KnnResultSet<float, true>(knn,sorted,epsError)
+                                                                                      , distance);
+            }
+            else {
+                KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                      thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                      thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                      qd.get(),
+                                                                                      istride,
+                                                                                      ostride,
+                                                                                      id.get(),
+                                                                                      dd.get(),
+                                                                                      queries.rows, flann::cuda::KnnResultSet<float, false>(knn,sorted,epsError),
+                                                                                      distance
+                                                                                      );
+            }
+        }
+        thrust::transform(id, id+knn*queries.rows, id, map_indices(thrust::raw_pointer_cast( &((*gpu_helper_->gpu_vind_))[0]) ));
+    }
+}
+
+
+template< typename Distance>
+int KDTreeCuda3dIndex<Distance >::radiusSearchGpu(const Matrix<ElementType>& queries, std::vector< std::vector<int> >& indices,
+                                                  std::vector<std::vector<DistanceType> >& dists, float radius, const SearchParams& params) const
+{
+    //  assert(indices.roasdfws >= queries.rows);
+    //  assert(dists.rows >= queries.rows);
+
+    int max_neighbors = params.max_neighbors;
+    bool sorted = params.sorted;
+    bool use_heap = params.use_heap;
+    if (indices.size() < queries.rows ) indices.resize(queries.rows);
+    if (dists.size() < queries.rows ) dists.resize(queries.rows);
+    
+    int istride=queries.stride/sizeof(ElementType);
+
+    thrust::device_vector<float> queriesDev(istride* queries.rows,0);
+    thrust::copy( queries.ptr(), queries.ptr()+istride*queries.rows, queriesDev.begin() );
+    thrust::device_vector<int> countsDev(queries.rows);
+
+    typename GpuDistance<Distance>::type distance;
+
+    int threadsPerBlock = 128;
+    int blocksPerGrid=(queries.rows+threadsPerBlock-1)/threadsPerBlock;
+
+
+    KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                          thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                          thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                          thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                          thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                          thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                          thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                          istride,
+                                                                          1,
+                                                                          thrust::raw_pointer_cast(&countsDev[0]),
+                                                                          0,
+                                                                          queries.rows, flann::cuda::CountingRadiusResultSet<float>(radius,max_neighbors),
+                                                                          distance
+                                                                          );
+
+    thrust::host_vector<int> counts_host=countsDev;
+
+    if( max_neighbors!=0 ) { // we'll need this later, but the exclusive_scan will change the array
+        for( size_t i=0; i<queries.rows; i++ ) {
+            int count = counts_host[i];
+            if( count > 0 ) {
+                indices[i].resize(count);
+                dists[i].resize(count);
+            }
+            else {
+                indices[i].clear();
+                dists[i].clear();
+            }
+
+        }
+    }
+
+    int neighbors_last_elem = countsDev.back();
+    thrust::exclusive_scan( countsDev.begin(), countsDev.end(), countsDev.begin() );
+
+    size_t total_neighbors=neighbors_last_elem+countsDev.back();
+    if( max_neighbors==0 ) return total_neighbors;
+
+    thrust::device_vector<int> indicesDev(total_neighbors,-1);
+    thrust::device_vector<float> distsDev(total_neighbors,std::numeric_limits<float>::infinity());
+
+    if( max_neighbors<0 ) {
+        KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                              thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                              thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                              thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                              thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                              thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                              thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                              istride,
+                                                                              1,
+                                                                              thrust::raw_pointer_cast(&indicesDev[0]),
+                                                                              thrust::raw_pointer_cast(&distsDev[0]),
+                                                                              queries.rows, flann::cuda::RadiusResultSet<float>(radius,thrust::raw_pointer_cast(&countsDev[0]),sorted), distance);
+    }
+    else {
+        if( use_heap ) {
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                                  istride,
+                                                                                  1,
+                                                                                  thrust::raw_pointer_cast(&indicesDev[0]),
+                                                                                  thrust::raw_pointer_cast(&distsDev[0]),
+                                                                                  queries.rows, flann::cuda::RadiusKnnResultSet<float, true>(radius,max_neighbors, thrust::raw_pointer_cast(&countsDev[0]),sorted), distance);
+        }
+        else {
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                                  istride,
+                                                                                  1,
+                                                                                  thrust::raw_pointer_cast(&indicesDev[0]),
+                                                                                  thrust::raw_pointer_cast(&distsDev[0]),
+                                                                                  queries.rows, flann::cuda::RadiusKnnResultSet<float, false>(radius,max_neighbors, thrust::raw_pointer_cast(&countsDev[0]),sorted), distance);
+        }
+    }
+    thrust::transform(indicesDev.begin(), indicesDev.end(), indicesDev.begin(), map_indices(thrust::raw_pointer_cast( &((*gpu_helper_->gpu_vind_))[0]) ));
+    thrust::host_vector<int> indices_temp = indicesDev;
+    thrust::host_vector<float> dists_temp = distsDev;
+
+    int buffer_index=0;
+    for( size_t i=0; i<queries.rows; i++ ) {
+        for( size_t j=0; j<counts_host[i]; j++ ) {
+            dists[i][j]=dists_temp[buffer_index];
+            indices[i][j]=indices_temp[buffer_index];
+            ++buffer_index;
+        }
+    }
+
+    return buffer_index;
+}
+
+//! used in the radius search to count the total number of neighbors
+struct isNotMinusOne
+{
+    __host__ __device__
+    bool operator() ( int i ){
+        return i!=-1;
+    }
+};
+
+template< typename Distance>
+int KDTreeCuda3dIndex< Distance >::radiusSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, float radius, const SearchParams& params) const
+{
+	int max_neighbors = params.max_neighbors;
+    assert(indices.rows >= queries.rows);
+    assert(dists.rows >= queries.rows || max_neighbors==0 );
+    assert(indices.stride==dists.stride  || max_neighbors==0 );
+    assert( indices.cols==indices.stride/sizeof(int) );
+    assert(dists.rows >= queries.rows || max_neighbors==0 );
+
+    bool sorted = params.sorted;
+    bool matrices_on_gpu = params.matrices_in_gpu_ram;
+    float epsError = 1+params.eps;
+    bool use_heap = params.use_heap;
+    int istride=queries.stride/sizeof(ElementType);
+    int ostride= indices.stride/4;
+
+
+    if( max_neighbors<0 ) max_neighbors=indices.cols;
+
+    if( !matrices_on_gpu ) {
+        thrust::device_vector<float> queriesDev(istride* queries.rows,0);
+        thrust::copy( queries.ptr(), queries.ptr()+istride*queries.rows, queriesDev.begin() );
+        typename GpuDistance<Distance>::type distance;
+        int threadsPerBlock = 128;
+        int blocksPerGrid=(queries.rows+threadsPerBlock-1)/threadsPerBlock;
+        if( max_neighbors== 0 ) {
+            thrust::device_vector<int> indicesDev(queries.rows* ostride);
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                                  istride,
+                                                                                  ostride,
+                                                                                  thrust::raw_pointer_cast(&indicesDev[0]),
+                                                                                  0,
+                                                                                  queries.rows, flann::cuda::CountingRadiusResultSet<float>(radius,-1),
+                                                                                  distance
+                                                                                  );
+            thrust::copy( indicesDev.begin(), indicesDev.end(), indices.ptr() );
+            return thrust::reduce(indicesDev.begin(), indicesDev.end() );
+        }
+
+
+        thrust::device_vector<float> distsDev(queries.rows* max_neighbors);
+        thrust::device_vector<int> indicesDev(queries.rows* max_neighbors);
+
+        if( use_heap ) {
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                                  istride,
+                                                                                  ostride,
+                                                                                  thrust::raw_pointer_cast(&indicesDev[0]),
+                                                                                  thrust::raw_pointer_cast(&distsDev[0]),
+                                                                                  queries.rows, flann::cuda::KnnRadiusResultSet<float, true>(max_neighbors,sorted,epsError, radius), distance);
+        }
+        else {
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  thrust::raw_pointer_cast(&queriesDev[0]),
+                                                                                  istride,
+                                                                                  ostride,
+                                                                                  thrust::raw_pointer_cast(&indicesDev[0]),
+                                                                                  thrust::raw_pointer_cast(&distsDev[0]),
+                                                                                  queries.rows, flann::cuda::KnnRadiusResultSet<float, false>(max_neighbors,sorted,epsError, radius), distance);
+        }
+
+        thrust::copy( distsDev.begin(), distsDev.end(), dists.ptr() );
+        thrust::transform(indicesDev.begin(), indicesDev.end(), indicesDev.begin(), map_indices(thrust::raw_pointer_cast( &((*gpu_helper_->gpu_vind_))[0]) ));
+        thrust::copy( indicesDev.begin(), indicesDev.end(), indices.ptr() );
+
+        return thrust::count_if(indicesDev.begin(), indicesDev.end(), isNotMinusOne() );
+    }
+    else {
+
+        thrust::device_ptr<float> qd=thrust::device_pointer_cast(queries.ptr());
+        thrust::device_ptr<float> dd=thrust::device_pointer_cast(dists.ptr());
+        thrust::device_ptr<int> id=thrust::device_pointer_cast(indices.ptr());
+        typename GpuDistance<Distance>::type distance;
+        int threadsPerBlock = 128;
+        int blocksPerGrid=(queries.rows+threadsPerBlock-1)/threadsPerBlock;
+
+        if( max_neighbors== 0 ) {
+            thrust::device_vector<int> indicesDev(queries.rows* indices.stride);
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  qd.get(),
+                                                                                  istride,
+                                                                                  ostride,
+                                                                                  id.get(),
+                                                                                  0,
+                                                                                  queries.rows, flann::cuda::CountingRadiusResultSet<float>(radius,-1),
+                                                                                  distance
+                                                                                  );
+            thrust::copy( indicesDev.begin(), indicesDev.end(), indices.ptr() );
+            return thrust::reduce(indicesDev.begin(), indicesDev.end() );
+        }
+
+        if( use_heap ) {
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  qd.get(),
+                                                                                  istride,
+                                                                                  ostride,
+                                                                                  id.get(),
+                                                                                  dd.get(),
+                                                                                  queries.rows, flann::cuda::KnnRadiusResultSet<float, true>(max_neighbors,sorted,epsError, radius), distance);
+        }
+        else {
+            KdTreeCudaPrivate::nearestKernel<<<blocksPerGrid, threadsPerBlock>>> (thrust::raw_pointer_cast(&((*gpu_helper_->gpu_splits_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_child1_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_parent_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_min_)[0])),
+                                                                                  thrust::raw_pointer_cast(&((*gpu_helper_->gpu_aabb_max_)[0])),
+                                                                                  thrust::raw_pointer_cast( &((*gpu_helper_->gpu_points_)[0]) ),
+                                                                                  qd.get(),
+                                                                                  istride,
+                                                                                  ostride,
+                                                                                  id.get(),
+                                                                                  dd.get(),
+                                                                                  queries.rows, flann::cuda::KnnRadiusResultSet<float, false>(max_neighbors,sorted,epsError, radius), distance);
+        }
+
+        thrust::transform(id, id+max_neighbors*queries.rows, id, map_indices(thrust::raw_pointer_cast( &((*gpu_helper_->gpu_vind_))[0]) ));
+
+        return thrust::count_if(id, id+max_neighbors*queries.rows, isNotMinusOne() );
+    }
+}
+
+template<typename Distance>
+void KDTreeCuda3dIndex<Distance>::uploadTreeToGpu()
+{
+    // just make sure that no weird alignment stuff is going on...
+    // shouldn't, but who knows
+    // (I would make this a (boost) static assertion, but so far flann seems to avoid boost
+    //  assert( sizeof( KdTreeCudaPrivate::GpuNode)==sizeof( Node ) );
+    delete gpu_helper_;
+    gpu_helper_ = new GpuHelper;
+    gpu_helper_->gpu_points_=new thrust::device_vector<float4>(size_);
+    thrust::device_vector<float4> tmp(size_);
+    if( get_param(index_params_,"input_is_gpu_float4",false) ) {
+		assert( dataset_.cols == 3 && dataset_.stride==4*sizeof(float));
+        thrust::copy( thrust::device_pointer_cast((float4*)dataset_.ptr()),thrust::device_pointer_cast((float4*)(dataset_.ptr()))+size_,tmp.begin());
+        
+    }
+    else {
+        // k is limited to 4 -> use 128bit-alignment regardless of dimensionality
+        // makes cpu search about 5% slower, but gpu can read a float4 w/ a single instruction
+        // (vs a float2 and a float load for a float3 value)
+        // pad data directly to avoid having to copy and re-format the data when
+        // copying it to the GPU
+        data_ = flann::Matrix<ElementType>(new ElementType[size_*4], size_, dim_,4*4);
+        for (size_t i=0; i<size_; ++i) {
+            for (size_t j=0; j<dim_; ++j) {
+                data_[i][j] = dataset_[i][j];
+            }
+            for (size_t j=dim_; j<4; ++j) {
+                data_[i][j] = 0;
+            }
+        }
+        thrust::copy((float4*)data_.ptr(),(float4*)(data_.ptr())+size_,tmp.begin());
+    }
+
+    CudaKdTreeBuilder builder( tmp, leaf_max_size_ );
+    builder.buildTree();
+
+    gpu_helper_->gpu_splits_ = builder.splits_;
+    gpu_helper_->gpu_aabb_min_ = builder.aabb_min_;
+    gpu_helper_->gpu_aabb_max_ = builder.aabb_max_;
+    gpu_helper_->gpu_child1_ = builder.child1_;
+    gpu_helper_->gpu_parent_=builder.parent_;
+    gpu_helper_->gpu_vind_=builder.index_x_;
+    thrust::gather( builder.index_x_->begin(), builder.index_x_->end(), tmp.begin(), gpu_helper_->gpu_points_->begin());
+
+    //  gpu_helper_->gpu_nodes_=new thrust::device_vector<KdTreeCudaPrivate::GpuNode>(node_count_);
+
+
+    //  gpu_helper_->gpu_vind_=new thrust::device_vector<int>(size_);
+    //  thrust::copy( (KdTreeCudaPrivate::GpuNode*)&(tree_[0]), ((KdTreeCudaPrivate::GpuNode*)&(tree_[0]))+tree_.size(),  gpu_helper_->gpu_nodes_->begin());
+
+    //  thrust::copy(vind_.begin(),vind_.end(),gpu_helper_->gpu_vind_->begin());
+
+    //  buildGpuTree();
+}
+
+
+template<typename Distance>
+void KDTreeCuda3dIndex<Distance>::clearGpuBuffers()
+{
+    delete gpu_helper_;
+    gpu_helper_=0;
+}
+
+// explicit instantiations for distance-independent functions
+template
+void KDTreeCuda3dIndex<flann::L2<float> >::uploadTreeToGpu();
+
+template
+void KDTreeCuda3dIndex<flann::L2<float> >::clearGpuBuffers();
+
+template
+struct KDTreeCuda3dIndex<flann::L2<float> >::GpuHelper;
+
+template
+void KDTreeCuda3dIndex<flann::L2<float> >::knnSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, size_t knn, const SearchParams& params) const;
+
+template
+int KDTreeCuda3dIndex< flann::L2<float> >::radiusSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, float radius, const SearchParams& params) const;
+template
+int KDTreeCuda3dIndex< flann::L2<float> >::radiusSearchGpu(const Matrix<ElementType>& queries, std::vector< std::vector<int> >& indices,
+                                                           std::vector<std::vector<DistanceType> >& dists, float radius, const SearchParams& params) const;
+
+// explicit instantiations for distance-independent functions
+template
+void KDTreeCuda3dIndex<flann::L2_Simple<float> >::uploadTreeToGpu();
+
+template
+void KDTreeCuda3dIndex<flann::L2_Simple<float> >::clearGpuBuffers();
+
+template
+struct KDTreeCuda3dIndex<flann::L2_Simple<float> >::GpuHelper;
+
+template
+void KDTreeCuda3dIndex<flann::L2_Simple<float> >::knnSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, size_t knn, const SearchParams& params) const;
+
+template
+int KDTreeCuda3dIndex< flann::L2_Simple<float> >::radiusSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, float radius, const SearchParams& params) const;
+template
+int KDTreeCuda3dIndex< flann::L2_Simple<float> >::radiusSearchGpu(const Matrix<ElementType>& queries, std::vector< std::vector<int> >& indices,
+                                                                  std::vector<std::vector<DistanceType> >& dists, float radius, const SearchParams& params) const;
+
+
+// explicit instantiations for distance-independent functions
+template
+void KDTreeCuda3dIndex<flann::L1<float> >::uploadTreeToGpu();
+
+template
+void KDTreeCuda3dIndex<flann::L1<float> >::clearGpuBuffers();
+
+template
+struct KDTreeCuda3dIndex<flann::L1<float> >::GpuHelper;
+
+template
+void KDTreeCuda3dIndex<flann::L1<float> >::knnSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, size_t knn, const SearchParams& params) const;
+
+template
+int KDTreeCuda3dIndex< flann::L1<float> >::radiusSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, float radius, const SearchParams& params) const;
+template
+int KDTreeCuda3dIndex< flann::L1<float> >::radiusSearchGpu(const Matrix<ElementType>& queries, std::vector< std::vector<int> >& indices,
+                                                           std::vector<std::vector<DistanceType> >& dists, float radius, const SearchParams& params) const;
+}

corelib/src/flann/algorithms/kdtree_cuda_3d_index.h

@@ -0,0 +1,327 @@
+/***********************************************************************
+ * 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       Andreas Muetzel (amuetzel@uni-koblenz.de). 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 FLANN_KDTREE_CUDA_3D_INDEX_H_
+#define FLANN_KDTREE_CUDA_3D_INDEX_H_
+
+#include <algorithm>
+#include <map>
+#include <cassert>
+#include <cstring>
+#include "flann/general.h"
+#include "flann/algorithms/nn_index.h"
+#include "flann/util/matrix.h"
+#include "flann/util/result_set.h"
+#include "flann/util/heap.h"
+#include "flann/util/allocator.h"
+#include "flann/util/random.h"
+#include "flann/util/saving.h"
+#include "flann/util/params.h"
+
+namespace flann
+{
+
+struct KDTreeCuda3dIndexParams : public IndexParams
+{
+    KDTreeCuda3dIndexParams( int leaf_max_size = 64 )
+    {
+        (*this)["algorithm"] = FLANN_INDEX_KDTREE_CUDA;
+        (*this)["leaf_max_size"] = leaf_max_size;
+        (*this)["dim"] = 3;
+    }
+};
+
+/**
+ * Cuda KD Tree.
+ * Tree is built with GPU assistance and search is performed on the GPU, too.
+ *
+ * Usually faster than the CPU search for data (and query) sets larger than 250000-300000 points, depending
+ * on your CPU and GPU.
+ */
+template <typename Distance>
+class KDTreeCuda3dIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    int visited_leafs;
+
+    typedef bool needs_kdtree_distance;
+
+    /**
+     * KDTree constructor
+     *
+     * Params:
+     *          inputData = dataset with the input features
+     *          params = parameters passed to the kdtree algorithm
+     */
+    KDTreeCuda3dIndex(const Matrix<ElementType>& inputData, const IndexParams& params = KDTreeCuda3dIndexParams(),
+                      Distance d = Distance() ) : BaseClass(params,d), dataset_(inputData), leaf_count_(0), visited_leafs(0), node_count_(0), current_node_count_(0)
+    {
+        size_ = dataset_.rows;
+        dim_ = dataset_.cols;
+
+        int dim_param = get_param(params,"dim",-1);
+        if (dim_param>0) dim_ = dim_param;
+        leaf_max_size_ = get_param(params,"leaf_max_size",10);
+        assert( dim_ == 3 );
+        gpu_helper_=0;
+    }
+
+    KDTreeCuda3dIndex(const KDTreeCuda3dIndex& other);
+    KDTreeCuda3dIndex operator=(KDTreeCuda3dIndex other);
+
+    /**
+     * Standard destructor
+     */
+    ~KDTreeCuda3dIndex()
+    {
+        delete[] data_.ptr();
+        clearGpuBuffers();
+    }
+
+    BaseClass* clone() const
+    {
+    	throw FLANNException("KDTreeCuda3dIndex cloning is not implemented");
+    }
+
+    /**
+     * Builds the index
+     */
+    void buildIndex()
+    {
+        // Create a permutable array of indices to the input vectors.
+        vind_.resize(size_);
+        for (size_t i = 0; i < size_; i++) {
+            vind_[i] = i;
+        }
+
+        leaf_count_=0;
+        node_count_=0;
+        //         computeBoundingBox(root_bbox_);
+        //                              tree_.reserve(log2((double)size_/leaf_max_size_));
+        //         divideTree(0, size_, root_bbox_,-1 );   // construct the tree
+
+        delete[] data_.ptr();
+
+        uploadTreeToGpu();
+    }
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_KDTREE_SINGLE;
+    }
+
+
+    void removePoint(size_t index)
+    {
+    	throw FLANNException( "removePoint not implemented for this index type!" );
+    }
+
+    ElementType* getPoint(size_t id)
+    {
+    	return dataset_[id];
+    }
+
+    void saveIndex(FILE* stream)
+    {
+        throw FLANNException( "Index saving not implemented!" );
+
+    }
+
+
+    void loadIndex(FILE* stream)
+    {
+        throw FLANNException( "Index loading not implemented!" );
+    }
+
+    size_t veclen() const
+    {
+        return dim_;
+    }
+
+    /**
+     * Computes the inde memory usage
+     * Returns: memory used by the index
+     * TODO: return system or gpu RAM or both?
+     */
+    int usedMemory() const
+    {
+        //         return tree_.size()*sizeof(Node)+dataset_.rows*sizeof(int);  // pool memory and vind array memory
+        return 0;
+    }
+
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, size_t knn, const SearchParams& params) const
+    {
+    	knnSearchGpu(queries,indices, dists, knn, params);
+        return knn*queries.rows; // hack...
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                          std::vector< std::vector<int> >& indices,
+                          std::vector<std::vector<DistanceType> >& dists,
+                          size_t knn,
+                          const SearchParams& params) const
+    {
+    	knnSearchGpu(queries,indices, dists, knn, params);
+        return knn*queries.rows; // hack...
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    void knnSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists, size_t knn, const SearchParams& params) const;
+
+    int knnSearchGpu(const Matrix<ElementType>& queries,
+                     std::vector< std::vector<int> >& indices,
+                     std::vector<std::vector<DistanceType> >& dists,
+                     size_t knn,
+                     const SearchParams& params) const
+    {
+        flann::Matrix<int> ind( new int[knn*queries.rows], queries.rows,knn);
+        flann::Matrix<DistanceType> dist( new DistanceType[knn*queries.rows], queries.rows,knn);
+        knnSearchGpu(queries,ind,dist,knn,params);
+        for( size_t i = 0; i<queries.rows; i++ ) {
+            indices[i].resize(knn);
+            dists[i].resize(knn);
+            for( size_t j=0; j<knn; j++ ) {
+                indices[i][j]=ind[i][j];
+                dists[i][j]=dist[i][j];
+            }
+        }
+        delete [] ind.ptr();
+        delete [] dist.ptr();
+        return knn*queries.rows; // hack...
+    }
+
+    int radiusSearch(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists,
+                             float radius, const SearchParams& params) const
+    {
+    	return radiusSearchGpu(queries,indices, dists, radius, params);
+    }
+
+    int radiusSearch(const Matrix<ElementType>& queries, std::vector< std::vector<int> >& indices,
+                             std::vector<std::vector<DistanceType> >& dists, float radius, const SearchParams& params) const
+    {
+    	return radiusSearchGpu(queries,indices, dists, radius, params);
+    }
+
+    int radiusSearchGpu(const Matrix<ElementType>& queries, Matrix<int>& indices, Matrix<DistanceType>& dists,
+                        float radius, const SearchParams& params) const;
+
+    int radiusSearchGpu(const Matrix<ElementType>& queries, std::vector< std::vector<int> >& indices,
+                        std::vector<std::vector<DistanceType> >& dists, float radius, const SearchParams& params) const;
+
+    /**
+     * Not implemented, since it is only used by single-element searches.
+     * (but is needed b/c it is abstract in the base class)
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+    }
+
+protected:
+    void buildIndexImpl()
+    {
+        /* nothing to do here */
+    }
+
+    void freeIndex()
+    {
+        /* nothing to do here */
+    }
+
+private:
+
+    void uploadTreeToGpu( );
+
+    void clearGpuBuffers( );
+
+
+
+
+private:
+
+    struct GpuHelper;
+
+    GpuHelper* gpu_helper_;
+
+    const Matrix<ElementType> dataset_;
+
+    int leaf_max_size_;
+
+    int leaf_count_;
+    int node_count_;
+    //! used by convertTreeToGpuFormat
+    int current_node_count_;
+
+
+    /**
+     *  Array of indices to vectors in the dataset.
+     */
+    std::vector<int> vind_;
+
+    Matrix<ElementType> data_;
+
+    size_t dim_;
+
+    USING_BASECLASS_SYMBOLS
+};   // class KDTreeCuda3dIndex
+
+
+}
+
+#endif //FLANN_KDTREE_SINGLE_INDEX_H_

corelib/src/flann/algorithms/kdtree_cuda_builder.h

@@ -0,0 +1,729 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2011       Andreas Muetzel (amuetzel@uni-koblenz.de). 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 FLANN_CUDA_KD_TREE_BUILDER_H_
+#define FLANN_CUDA_KD_TREE_BUILDER_H_
+#include <thrust/host_vector.h>
+#include <thrust/device_vector.h>
+#include <thrust/sort.h>
+#include <thrust/partition.h>
+#include <thrust/unique.h>
+#include <thrust/scan.h>
+#include <flann/util/cutil_math.h>
+#include <stdlib.h>
+
+// #define PRINT_DEBUG_TIMING
+
+namespace flann
+{
+//      template< typename T >
+//      void print_vector(  const thrust::device_vector<T>& v )
+//      {
+//              for( int i=0; i< v.size(); i++ )
+//              {
+//                      std::cout<<v[i]<<std::endl;
+//              }
+//      }
+//
+//      template< typename T1, typename T2 >
+//      void print_vector(  const thrust::device_vector<T1>& v1, const thrust::device_vector<T2>& v2 )
+//      {
+//              for( int i=0; i< v1.size(); i++ )
+//              {
+//                      std::cout<<i<<": "<<v1[i]<<" "<<v2[i]<<std::endl;
+//              }
+//      }
+//
+//      template< typename T1, typename T2, typename T3 >
+//      void print_vector(  const thrust::device_vector<T1>& v1, const thrust::device_vector<T2>& v2, const thrust::device_vector<T3>& v3 )
+//      {
+//              for( int i=0; i< v1.size(); i++ )
+//              {
+//                      std::cout<<i<<": "<<v1[i]<<" "<<v2[i]<<" "<<v3[i]<<std::endl;
+//              }
+//      }
+//
+//      template< typename T >
+//      void print_vector_by_index(  const thrust::device_vector<T>& v,const thrust::device_vector<int>& ind )
+//      {
+//              for( int i=0; i< v.size(); i++ )
+//              {
+//                      std::cout<<v[ind[i]]<<std::endl;
+//              }
+//      }
+
+//      std::ostream& operator <<(std::ostream& stream, const cuda::kd_tree_builder_detail::SplitInfo& s) {
+//      stream<<"(split l/r: "<< s.left <<" "<< s.right<< "  split:"<<s.split_dim<<" "<<s.split_val<<")";
+//      return stream;
+// }
+//
+//
+// std::ostream& operator <<(std::ostream& stream, const cuda::kd_tree_builder_detail::NodeInfo& s) {
+//      stream<<"(node: "<<s.child1()<<" "<<s.parent()<<" "<<s.child2()<<")";
+//      return stream;
+// }
+//
+// std::ostream& operator <<(std::ostream& stream, const float4& s) {
+//      stream<<"("<<s.x<<","<<s.y<<","<<s.z<<","<<s.w<<")";
+//      return stream;
+// }
+namespace cuda
+{
+namespace kd_tree_builder_detail
+{
+//! normal node: contains the split dimension and value
+//! leaf node: left == index of first points, right==index of last point +1
+struct SplitInfo
+{
+    union {
+        struct
+        {
+            // begin of child nodes
+            int left;
+            // end of child nodes
+            int right;
+        };
+        struct
+        {
+            int split_dim;
+            float split_val;
+        };
+    };
+
+};
+
+struct IsEven
+{
+    typedef int result_type;
+    __device__
+    int operator()(int i )
+    {
+        return (i& 1)==0;
+    }
+};
+
+struct SecondElementIsEven
+{
+    __host__ __device__
+    bool operator()( const thrust::tuple<int,int>& i )
+    {
+        return (thrust::get<1>(i)& 1)==0;
+    }
+};
+
+//! just for convenience: access a float4 by an index in [0,1,2]
+//! (casting it to a float* and accessing it by the index is way slower...)
+__host__ __device__
+float get_value_by_index( const float4& f, int i )
+{
+    switch(i) {
+    case 0:
+        return f.x;
+    case 1:
+        return f.y;
+    default:
+        return f.z;
+    }
+
+}
+
+//! mark a point as belonging to the left or right child of its current parent
+//! called after parents are split
+struct MovePointsToChildNodes
+{
+    MovePointsToChildNodes( int* child1, SplitInfo* splits, float* x, float* y, float* z, int* ox, int* oy, int* oz, int* lrx, int* lry, int* lrz )
+        : child1_(child1), splits_(splits), x_(x), y_(y), z_(z), ox_(ox), oy_(oy), oz_(oz), lrx_(lrx), lry_(lry), lrz_(lrz){}
+    //  int dim;
+    //  float threshold;
+    int* child1_;
+    SplitInfo* splits_;
+
+    // coordinate values
+    float* x_, * y_, * z_;
+    // owner indices -> which node does the point belong to?
+    int* ox_, * oy_, * oz_;
+    // temp info: will be set to 1 of a point is moved to the right child node, 0 otherwise
+    // (used later in the scan op to separate the points of the children into continuous ranges)
+    int* lrx_, * lry_, * lrz_;
+    __device__
+    void operator()( const thrust::tuple<int, int, int, int>& data )
+    {
+        int index = thrust::get<0>(data);
+        int owner = ox_[index]; // before a split, all points at the same position in the index array have the same owner
+        int point_ind1=thrust::get<1>(data);
+        int point_ind2=thrust::get<2>(data);
+        int point_ind3=thrust::get<3>(data);
+        int leftChild=child1_[owner];
+        int split_dim;
+        float dim_val1, dim_val2, dim_val3;
+        SplitInfo split;
+        lrx_[index]=0;
+        lry_[index]=0;
+        lrz_[index]=0;
+        // this element already belongs to a leaf node -> everything alright, no need to change anything
+        if( leftChild==-1 ) {
+            return;
+        }
+        // otherwise: load split data, and assign this index to the new owner
+        split = splits_[owner];
+        split_dim=split.split_dim;
+        switch( split_dim ) {
+        case 0:
+            dim_val1=x_[point_ind1];
+            dim_val2=x_[point_ind2];
+            dim_val3=x_[point_ind3];
+            break;
+        case 1:
+            dim_val1=y_[point_ind1];
+            dim_val2=y_[point_ind2];
+            dim_val3=y_[point_ind3];
+            break;
+        default:
+            dim_val1=z_[point_ind1];
+            dim_val2=z_[point_ind2];
+            dim_val3=z_[point_ind3];
+            break;
+
+        }
+
+
+        int r1=leftChild +(dim_val1 > split.split_val);
+        ox_[index]=r1;
+        int r2=leftChild+(dim_val2 > split.split_val);
+        oy_[index]=r2;
+        oz_[index]=leftChild+(dim_val3 > split.split_val);
+
+        lrx_[index] = (dim_val1 > split.split_val);
+        lry_[index] = (dim_val2 > split.split_val);
+        lrz_[index] = (dim_val3 > split.split_val);
+        //                      return thrust::make_tuple( r1, r2, leftChild+(dim_val > split.split_val) );
+    }
+};
+
+//! used to update the left/right pointers and aabb infos after the node splits
+struct SetLeftAndRightAndAABB
+{
+    int maxPoints;
+    int nElements;
+
+    SplitInfo* nodes;
+    int* counts;
+    int* labels;
+    float4* aabbMin;
+    float4* aabbMax;
+    const float* x,* y,* z;
+    const int* ix, * iy, * iz;
+
+    __host__ __device__
+    void operator()( int i )
+    {
+        int index=labels[i];
+        int right;
+        int left = counts[i];
+        nodes[index].left=left;
+        if( i < nElements-1 ) {
+            right=counts[i+1];
+        }
+        else { // index==nNodes
+            right=maxPoints;
+        }
+        nodes[index].right=right;
+        aabbMin[index].x=x[ix[left]];
+        aabbMin[index].y=y[iy[left]];
+        aabbMin[index].z=z[iz[left]];
+        aabbMax[index].x=x[ix[right-1]];
+        aabbMax[index].y=y[iy[right-1]];
+        aabbMax[index].z=z[iz[right-1]];
+    }
+};
+
+
+//! - decide whether a node has to be split
+//! if yes:
+//! - allocate child nodes
+//! - set split axis as axis of maximum aabb length
+struct SplitNodes
+{
+    int maxPointsPerNode;
+    int* node_count;
+    int* nodes_allocated;
+    int* out_of_space;
+    int* child1_;
+    int* parent_;
+    SplitInfo* splits;
+
+    __device__
+    void operator()( thrust::tuple<int&, int&,SplitInfo&,float4&,float4&, int> node ) // float4: aabbMin, aabbMax
+    {
+        int& parent=thrust::get<0>(node);
+        int& child1=thrust::get<1>(node);
+        SplitInfo& s=thrust::get<2>(node);
+        const float4& aabbMin=thrust::get<3>(node);
+        const float4& aabbMax=thrust::get<4>(node);
+        int my_index = thrust::get<5>(node);
+        bool split_node=false;
+        // first, each thread block counts the number of nodes that it needs to allocate...
+        __shared__ int block_nodes_to_allocate;
+        if( threadIdx.x== 0 ) block_nodes_to_allocate=0;
+        __syncthreads();
+
+        // don't split if all points are equal
+        // (could lead to an infinite loop, and doesn't make any sense anyway)
+        bool all_points_in_node_are_equal=aabbMin.x == aabbMax.x && aabbMin.y==aabbMax.y && aabbMin.z==aabbMax.z;
+
+        int offset_to_global=0;
+
+        // maybe this could be replaced with a reduction...
+        if(( child1==-1) &&( s.right-s.left > maxPointsPerNode) && !all_points_in_node_are_equal ) { // leaf node
+            split_node=true;
+            offset_to_global = atomicAdd( &block_nodes_to_allocate,2 );
+        }
+
+        __syncthreads();
+        __shared__ int block_left;
+        __shared__ bool enough_space;
+        // ... then the first thread tries to allocate this many nodes...
+        if( threadIdx.x==0) {
+            block_left = atomicAdd( node_count, block_nodes_to_allocate );
+            enough_space = block_left+block_nodes_to_allocate < *nodes_allocated;
+            // if it doesn't succeed, no nodes will be created by this block
+            if( !enough_space ) {
+                atomicAdd( node_count, -block_nodes_to_allocate );
+                *out_of_space=1;
+            }
+        }
+
+        __syncthreads();
+        // this thread needs to split it's node && there was enough space for all the nodes
+        // in this block.
+        //(The whole "allocate-per-block-thing" is much faster than letting each element allocate
+        // its space on its own, because shared memory atomics are A LOT faster than
+        // global mem atomics!)
+        if( split_node && enough_space ) {
+            int left = block_left + offset_to_global;
+
+            splits[left].left=s.left;
+            splits[left].right=s.right;
+            splits[left+1].left=0;
+            splits[left+1].right=0;
+
+            // split axis/position: middle of longest aabb extent
+            float4 aabbDim=aabbMax-aabbMin;
+            int maxDim=0;
+            float maxDimLength=aabbDim.x;
+            float4 splitVal=(aabbMax+aabbMin);
+            splitVal*=0.5f;
+            for( int i=1; i<=2; i++ ) {
+                float val = get_value_by_index(aabbDim,i);
+                if( val > maxDimLength ) {
+                    maxDim=i;
+                    maxDimLength=val;
+                }
+            }
+            s.split_dim=maxDim;
+            s.split_val=get_value_by_index(splitVal,maxDim);
+
+            child1_[my_index]=left;
+            splits[my_index]=s;
+
+            parent_[left]=my_index;
+            parent_[left+1]=my_index;
+            child1_[left]=-1;
+            child1_[left+1]=-1;
+        }
+    }
+};
+
+
+//! computes the scatter target address for the split operation, see Sengupta,Harris,Zhang,Owen: Scan Primitives for GPU Computing
+//! in my use case, this is about 2x as fast as thrust::partition
+struct set_addr3
+{
+    const int* val_, * f_;
+
+    int npoints_;
+    __device__
+    int operator()( int id )
+    {
+        int nf = f_[npoints_-1] + (val_[npoints_-1]);
+        int f=f_[id];
+        int t = id -f+nf;
+        return val_[id] ? f : t;
+    }
+};
+
+//! converts a float4 point (xyz) to a tuple of three float vals (used to separate the
+//! float4 input buffer into three arrays in the beginning of the tree build)
+struct pointxyz_to_px_py_pz
+{
+    __device__
+    thrust::tuple<float,float,float> operator()( const float4& val )
+    {
+        return thrust::make_tuple(val.x, val.y, val.z);
+    }
+};
+} // namespace kd_tree_builder_detail
+
+} // namespace cuda
+
+
+std::ostream& operator <<(std::ostream& stream, const cuda::kd_tree_builder_detail::SplitInfo& s)
+{
+    stream<<"(split l/r: "<< s.left <<" "<< s.right<< "  split:"<<s.split_dim<<" "<<s.split_val<<")";
+    return stream;
+}
+class CudaKdTreeBuilder
+{
+public:
+    CudaKdTreeBuilder( const thrust::device_vector<float4>& points, int max_leaf_size ) : /*out_of_space_(1,0),node_count_(1,1),*/ max_leaf_size_(max_leaf_size)
+    {
+        points_=&points;
+        int prealloc = points.size()/max_leaf_size_*16;
+        allocation_info_.resize(3);
+        allocation_info_[NodeCount]=1;
+        allocation_info_[NodesAllocated]=prealloc;
+        allocation_info_[OutOfSpace]=0;
+
+        //              std::cout<<points_->size()<<std::endl;
+
+        child1_=new thrust::device_vector<int>(prealloc,-1);
+        parent_=new thrust::device_vector<int>(prealloc,-1);
+        cuda::kd_tree_builder_detail::SplitInfo s;
+        s.left=0;
+        s.right=0;
+        splits_=new thrust::device_vector<cuda::kd_tree_builder_detail::SplitInfo>(prealloc,s);
+        s.right=points.size();
+        (*splits_)[0]=s;
+
+        aabb_min_=new thrust::device_vector<float4>(prealloc);
+        aabb_max_=new thrust::device_vector<float4>(prealloc);
+
+        index_x_=new thrust::device_vector<int>(points_->size());
+        index_y_=new thrust::device_vector<int>(points_->size());
+        index_z_=new thrust::device_vector<int>(points_->size());
+
+        owners_x_=new thrust::device_vector<int>(points_->size(),0);
+        owners_y_=new thrust::device_vector<int>(points_->size(),0);
+        owners_z_=new thrust::device_vector<int>(points_->size(),0);
+
+        leftright_x_ = new thrust::device_vector<int>(points_->size(),0);
+        leftright_y_ = new thrust::device_vector<int>(points_->size(),0);
+        leftright_z_ = new thrust::device_vector<int>(points_->size(),0);
+
+        tmp_index_=new thrust::device_vector<int>(points_->size());
+        tmp_owners_=new thrust::device_vector<int>(points_->size());
+        tmp_misc_=new thrust::device_vector<int>(points_->size());
+
+        points_x_=new thrust::device_vector<float>(points_->size());
+        points_y_=new thrust::device_vector<float>(points_->size());
+        points_z_=new thrust::device_vector<float>(points_->size());
+        delete_node_info_=false;
+    }
+
+    ~CudaKdTreeBuilder()
+    {
+        if( delete_node_info_ ) {
+            delete child1_;
+            delete parent_;
+            delete splits_;
+            delete aabb_min_;
+            delete aabb_max_;
+            delete index_x_;
+        }
+
+        delete index_y_;
+        delete index_z_;
+        delete owners_x_;
+        delete owners_y_;
+        delete owners_z_;
+        delete points_x_;
+        delete points_y_;
+        delete points_z_;
+        delete leftright_x_;
+        delete leftright_y_;
+        delete leftright_z_;
+        delete tmp_index_;
+        delete tmp_owners_;
+        delete tmp_misc_;
+    }
+
+	//! build the tree
+	//! general idea:
+	//! - build sorted lists of the points in x y and z order (to be able to compute tight AABBs in O(1) )
+	//! - while( nodes to split exist )
+	//!    - split non-child nodes along longest axis if the number of points is > max_points_per_node
+	//!    - for each point: determine whether it is in a node that was split. If yes, mark it as belonging to the left or right child node of its current parent node
+	//!    - reorder the points so that the points of a single node are continuous in the node array
+	//!    - update the left/right pointers and AABBs of all nodes
+    void buildTree()
+    {
+        //              std::cout<<"buildTree()"<<std::endl;
+        //              sleep(1);
+        //              Util::Timer stepTimer;
+        thrust::transform( points_->begin(), points_->end(), thrust::make_zip_iterator(thrust::make_tuple(points_x_->begin(), points_y_->begin(),points_z_->begin()) ), cuda::kd_tree_builder_detail::pointxyz_to_px_py_pz() );
+
+        thrust::counting_iterator<int> it(0);
+        thrust::copy( it, it+points_->size(), index_x_->begin() );
+
+        thrust::copy( index_x_->begin(), index_x_->end(), index_y_->begin() );
+        thrust::copy( index_x_->begin(), index_x_->end(), index_z_->begin() );
+
+        thrust::device_vector<float> tmpv(points_->size());
+
+        // create sorted index list -> can be used to compute AABBs in O(1)
+        thrust::copy(points_x_->begin(), points_x_->end(), tmpv.begin());
+        thrust::sort_by_key( tmpv.begin(), tmpv.end(), index_x_->begin() );
+        thrust::copy(points_y_->begin(), points_y_->end(), tmpv.begin());
+        thrust::sort_by_key( tmpv.begin(), tmpv.end(), index_y_->begin() );
+        thrust::copy(points_z_->begin(), points_z_->end(), tmpv.begin());
+        thrust::sort_by_key( tmpv.begin(), tmpv.end(), index_z_->begin() );
+
+
+        (*aabb_min_)[0]=make_float4((*points_x_)[(*index_x_)[0]],(*points_y_)[(*index_y_)[0]],(*points_z_)[(*index_z_)[0]],0);
+
+        (*aabb_max_)[0]=make_float4((*points_x_)[(*index_x_)[points_->size()-1]],(*points_y_)[(*index_y_)[points_->size()-1]],(*points_z_)[(*index_z_)[points_->size()-1]],0);
+        #ifdef PRINT_DEBUG_TIMING
+        cudaDeviceSynchronize();
+        std::cout<<" initial stuff:"<<stepTimer.elapsed()<<std::endl;
+        stepTimer.restart();
+        #endif
+        int last_node_count=0;
+        for( int i=0;; i++ ) {
+            cuda::kd_tree_builder_detail::SplitNodes sn;
+
+            sn.maxPointsPerNode=max_leaf_size_;
+            sn.node_count=thrust::raw_pointer_cast(&allocation_info_[NodeCount]);
+            sn.nodes_allocated=thrust::raw_pointer_cast(&allocation_info_[NodesAllocated]);
+            sn.out_of_space=thrust::raw_pointer_cast(&allocation_info_[OutOfSpace]);
+            sn.child1_=thrust::raw_pointer_cast(&(*child1_)[0]);
+            sn.parent_=thrust::raw_pointer_cast(&(*parent_)[0]);
+            sn.splits=thrust::raw_pointer_cast(&(*splits_)[0]);
+
+            thrust::counting_iterator<int> cit(0);
+            thrust::for_each( thrust::make_zip_iterator(thrust::make_tuple( parent_->begin(), child1_->begin(),  splits_->begin(), aabb_min_->begin(), aabb_max_->begin(), cit  )),
+                              thrust::make_zip_iterator(thrust::make_tuple( parent_->begin()+last_node_count, child1_->begin()+last_node_count,splits_->begin()+last_node_count, aabb_min_->begin()+last_node_count, aabb_max_->begin()+last_node_count,cit+last_node_count  )),
+                              sn   );
+            // copy allocation info to host
+            thrust::host_vector<int> alloc_info = allocation_info_;
+
+            if( last_node_count == alloc_info[NodeCount] ) { // no more nodes were split -> done
+                break;
+            }
+            last_node_count=alloc_info[NodeCount];
+			
+			// a node was un-splittable due to a lack of space
+            if( alloc_info[OutOfSpace]==1 ) {
+                resize_node_vectors(alloc_info[NodesAllocated]*2);
+                alloc_info[OutOfSpace]=0;
+                alloc_info[NodesAllocated]*=2;
+                allocation_info_=alloc_info;
+            }
+            #ifdef PRINT_DEBUG_TIMING
+            cudaDeviceSynchronize();
+            std::cout<<" node split:"<<stepTimer.elapsed()<<std::endl;
+            stepTimer.restart();
+            #endif
+
+            // foreach point: point was in node that was split?move it to child (leaf) node : do nothing
+            cuda::kd_tree_builder_detail::MovePointsToChildNodes sno( thrust::raw_pointer_cast(&(*child1_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*splits_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*points_x_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*points_y_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*points_z_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*owners_x_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*owners_y_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*owners_z_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*leftright_x_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*leftright_y_)[0]),
+                                                                      thrust::raw_pointer_cast(&(*leftright_z_)[0])
+                                                                      );
+            thrust::counting_iterator<int> ci0(0);
+            thrust::for_each( thrust::make_zip_iterator( thrust::make_tuple( ci0, index_x_->begin(), index_y_->begin(), index_z_->begin()) ),
+                              thrust::make_zip_iterator( thrust::make_tuple( ci0+points_->size(), index_x_->end(), index_y_->end(), index_z_->end()) ),sno  );
+
+            #ifdef PRINT_DEBUG_TIMING
+            cudaDeviceSynchronize();
+            std::cout<<" set new owners:"<<stepTimer.elapsed()<<std::endl;
+            stepTimer.restart();
+            #endif
+
+            // move points around so that each leaf node's points are continuous
+            separate_left_and_right_children(*index_x_,*owners_x_,*tmp_index_,*tmp_owners_, *leftright_x_);
+            std::swap(tmp_index_, index_x_);
+            std::swap(tmp_owners_, owners_x_);
+            separate_left_and_right_children(*index_y_,*owners_y_,*tmp_index_,*tmp_owners_, *leftright_y_,false);
+            std::swap(tmp_index_, index_y_);
+            separate_left_and_right_children(*index_z_,*owners_z_,*tmp_index_,*tmp_owners_, *leftright_z_,false);
+            std::swap(tmp_index_, index_z_);
+
+            #ifdef PRINT_DEBUG_TIMING
+            cudaDeviceSynchronize();
+            std::cout<<" split:"<<stepTimer.elapsed()<<std::endl;
+            stepTimer.restart();
+            #endif
+            // calculate new AABB etc
+            update_leftright_and_aabb( *points_x_, *points_y_, *points_z_, *index_x_, *index_y_, *index_z_, *owners_x_, *splits_,*aabb_min_, *aabb_max_);
+            #ifdef PRINT_DEBUG_TIMING
+            cudaDeviceSynchronize();
+            std::cout<<" update_leftright_and_aabb:"<<stepTimer.elapsed()<<std::endl;
+            stepTimer.restart();
+            print_vector(node_count_);
+            #endif
+
+        }
+    }
+    
+	template<class Distance> 
+	friend class KDTreeCuda3dIndex;
+
+protected:
+
+
+    //! takes the partitioned nodes, and sets the left-/right info of leaf nodes, as well as the AABBs
+    void
+    update_leftright_and_aabb( const thrust::device_vector<float>& x, const thrust::device_vector<float>& y,const thrust::device_vector<float>& z,
+                               const thrust::device_vector<int>& ix, const thrust::device_vector<int>& iy,const thrust::device_vector<int>& iz,
+                               const thrust::device_vector<int>& owners,
+                               thrust::device_vector<cuda::kd_tree_builder_detail::SplitInfo>& splits, thrust::device_vector<float4>& aabbMin,thrust::device_vector<float4>& aabbMax)
+    {
+        thrust::device_vector<int>* labelsUnique=tmp_owners_;
+        thrust::device_vector<int>* countsUnique=tmp_index_;
+		// assume: points of each node are continuous in the array
+		
+		// find which nodes are here, and where each node's points begin and end
+        int unique_labels = thrust::unique_by_key_copy( owners.begin(), owners.end(), thrust::counting_iterator<int>(0), labelsUnique->begin(), countsUnique->begin()).first - labelsUnique->begin();
+
+		// update the info
+        cuda::kd_tree_builder_detail::SetLeftAndRightAndAABB s;
+        s.maxPoints=x.size();
+        s.nElements=unique_labels;
+        s.nodes=thrust::raw_pointer_cast(&(splits[0]));
+        s.counts=thrust::raw_pointer_cast(&( (*countsUnique)[0]));
+        s.labels=thrust::raw_pointer_cast(&( (*labelsUnique)[0]));
+        s.x=thrust::raw_pointer_cast(&x[0]);
+        s.y=thrust::raw_pointer_cast(&y[0]);
+        s.z=thrust::raw_pointer_cast(&z[0]);
+        s.ix=thrust::raw_pointer_cast(&ix[0]);
+        s.iy=thrust::raw_pointer_cast(&iy[0]);
+        s.iz=thrust::raw_pointer_cast(&iz[0]);
+        s.aabbMin=thrust::raw_pointer_cast(&aabbMin[0]);
+        s.aabbMax=thrust::raw_pointer_cast(&aabbMax[0]);
+
+        thrust::counting_iterator<int> it(0);
+        thrust::for_each(it, it+unique_labels, s);
+    }
+
+    //! Separates the left and right children of each node into continuous parts of the array.
+    //! More specifically, it seperates children with even and odd node indices because nodes are always
+    //! allocated in pairs -> child1==child2+1 -> child1 even and child2 odd, or vice-versa.
+    //! Since the split operation is stable, this results in continuous partitions
+    //! for all the single nodes.
+    //! (basically the split primitive according to sengupta et al)
+    //! about twice as fast as thrust::partition
+    void separate_left_and_right_children( thrust::device_vector<int>& key_in, thrust::device_vector<int>& val_in, thrust::device_vector<int>& key_out, thrust::device_vector<int>& val_out, thrust::device_vector<int>& left_right_marks, bool scatter_val_out=true )
+    {
+        thrust::device_vector<int>* f_tmp = &val_out;
+        thrust::device_vector<int>* addr_tmp = tmp_misc_;
+
+        thrust::exclusive_scan( /*thrust::make_transform_iterator(*/ left_right_marks.begin() /*,cuda::kd_tree_builder_detail::IsEven*/
+                                                                     /*())*/, /*thrust::make_transform_iterator(*/ left_right_marks.end() /*,cuda::kd_tree_builder_detail::IsEven*/
+                                                                     /*())*/,     f_tmp->begin() );
+        cuda::kd_tree_builder_detail::set_addr3 sa;
+        sa.val_=thrust::raw_pointer_cast(&left_right_marks[0]);
+        sa.f_=thrust::raw_pointer_cast(&(*f_tmp)[0]);
+        sa.npoints_=key_in.size();
+        thrust::counting_iterator<int> it(0);
+        thrust::transform(it, it+val_in.size(), addr_tmp->begin(), sa);
+
+        thrust::scatter(key_in.begin(), key_in.end(), addr_tmp->begin(), key_out.begin());
+        if( scatter_val_out ) thrust::scatter(val_in.begin(), val_in.end(), addr_tmp->begin(), val_out.begin());
+    }
+
+    //! allocates additional space in all the node-related vectors.
+    //! new_size elements will be added to all vectors.
+    void resize_node_vectors( size_t new_size )
+    {
+        size_t add = new_size - child1_->size();
+        child1_->insert(child1_->end(), add, -1);
+        parent_->insert(parent_->end(), add, -1);
+        cuda::kd_tree_builder_detail::SplitInfo s;
+        s.left=0;
+        s.right=0;
+        splits_->insert(splits_->end(), add, s);
+        float4 f;
+        aabb_min_->insert(aabb_min_->end(), add, f);
+        aabb_max_->insert(aabb_max_->end(), add, f);
+    }
+
+
+    const thrust::device_vector<float4>* points_;
+	
+	// tree data, those are stored per-node
+	
+	//! left child of each node. (right child==left child + 1, due to the alloc mechanism)
+	//! child1_[node]==-1 if node is a leaf node
+    thrust::device_vector<int>* child1_;
+	//! parent node of each node
+    thrust::device_vector<int>* parent_;
+	//! split info (dim/value or left/right pointers)
+    thrust::device_vector<cuda::kd_tree_builder_detail::SplitInfo>* splits_;
+	//! min aabb value of each node
+    thrust::device_vector<float4>* aabb_min_;
+	//! max aabb value of each node
+    thrust::device_vector<float4>* aabb_max_;
+
+    enum AllocationInfo
+    {
+        NodeCount=0,
+        NodesAllocated=1,
+        OutOfSpace=2
+    };
+    // those were put into a single vector of 3 elements so that only one mem transfer will be needed for all three of them
+    //  thrust::device_vector<int> out_of_space_;
+    //  thrust::device_vector<int> node_count_;
+    //  thrust::device_vector<int> nodes_allocated_;
+    thrust::device_vector<int> allocation_info_;
+	
+    int max_leaf_size_;
+
+	// coordinate values of the points
+    thrust::device_vector<float>* points_x_, * points_y_, * points_z_;
+	// indices
+    thrust::device_vector<int>* index_x_,  * index_y_,  * index_z_;
+	// owner node
+    thrust::device_vector<int>* owners_x_, * owners_y_, * owners_z_;
+	// contains info about whether a point was partitioned to the left or right child after a split
+    thrust::device_vector<int>* leftright_x_, * leftright_y_, * leftright_z_;
+    thrust::device_vector<int>* tmp_index_, * tmp_owners_, * tmp_misc_;
+    bool delete_node_info_;
+};
+
+
+} // namespace flann
+#endif

corelib/src/flann/algorithms/kdtree_index.h

@@ -0,0 +1,765 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_KDTREE_INDEX_H_
+#define FLANN_KDTREE_INDEX_H_
+
+#include <algorithm>
+#include <map>
+#include <cassert>
+#include <cstring>
+#include <stdarg.h>
+#include <cmath>
+
+#include "flann/general.h"
+#include "flann/algorithms/nn_index.h"
+#include "flann/util/dynamic_bitset.h"
+#include "flann/util/matrix.h"
+#include "flann/util/result_set.h"
+#include "flann/util/heap.h"
+#include "flann/util/allocator.h"
+#include "flann/util/random.h"
+#include "flann/util/saving.h"
+
+
+namespace flann
+{
+
+struct KDTreeIndexParams : public IndexParams
+{
+    KDTreeIndexParams(int trees = 4)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_KDTREE;
+        (*this)["trees"] = trees;
+    }
+};
+
+
+/**
+ * Randomized kd-tree index
+ *
+ * Contains the k-d trees and other information for indexing a set of points
+ * for nearest-neighbor matching.
+ */
+template <typename Distance>
+class KDTreeIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    typedef bool needs_kdtree_distance;
+
+
+    /**
+     * KDTree constructor
+     *
+     * Params:
+     *          inputData = dataset with the input features
+     *          params = parameters passed to the kdtree algorithm
+     */
+    KDTreeIndex(const IndexParams& params = KDTreeIndexParams(), Distance d = Distance() ) :
+    	BaseClass(params, d), mean_(NULL), var_(NULL)
+    {
+        trees_ = get_param(index_params_,"trees",4);
+    }
+
+
+    /**
+     * KDTree constructor
+     *
+     * Params:
+     *          inputData = dataset with the input features
+     *          params = parameters passed to the kdtree algorithm
+     */
+    KDTreeIndex(const Matrix<ElementType>& dataset, const IndexParams& params = KDTreeIndexParams(),
+                Distance d = Distance() ) : BaseClass(params,d ), mean_(NULL), var_(NULL)
+    {
+        trees_ = get_param(index_params_,"trees",4);
+
+        setDataset(dataset);
+    }
+
+    KDTreeIndex(const KDTreeIndex& other) : BaseClass(other),
+    		trees_(other.trees_)
+    {
+        tree_roots_.resize(other.tree_roots_.size());
+        for (size_t i=0;i<tree_roots_.size();++i) {
+        	copyTree(tree_roots_[i], other.tree_roots_[i]);
+        }
+    }
+
+    KDTreeIndex& operator=(KDTreeIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    /**
+     * Standard destructor
+     */
+    virtual ~KDTreeIndex()
+    {
+    	freeIndex();
+    }
+
+    BaseClass* clone() const
+    {
+    	return new KDTreeIndex(*this);
+    }
+
+    using BaseClass::buildIndex;
+    
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+
+        size_t old_size = size_;
+        extendDataset(points);
+        
+        if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
+            buildIndex();
+        }
+        else {
+            for (size_t i=old_size;i<size_;++i) {
+                for (int j = 0; j < trees_; j++) {
+                    addPointToTree(tree_roots_[j], i);
+                }
+            }
+        }        
+    }
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_KDTREE;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	ar & trees_;
+
+    	if (Archive::is_loading::value) {
+    		tree_roots_.resize(trees_);
+    	}
+    	for (size_t i=0;i<tree_roots_.size();++i) {
+    		if (Archive::is_loading::value) {
+    			tree_roots_[i] = new(pool_) Node();
+    		}
+    		ar & *tree_roots_[i];
+    	}
+
+    	if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+            index_params_["trees"] = trees_;
+    	}
+    }
+
+
+    void saveIndex(FILE* stream)
+    {
+    	serialization::SaveArchive sa(stream);
+    	sa & *this;
+    }
+
+
+    void loadIndex(FILE* stream)
+    {
+    	freeIndex();
+    	serialization::LoadArchive la(stream);
+    	la & *this;
+    }
+
+    /**
+     * Computes the inde memory usage
+     * Returns: memory used by the index
+     */
+    int usedMemory() const
+    {
+        return int(pool_.usedMemory+pool_.wastedMemory+size_*sizeof(int));  // pool memory and vind array memory
+    }
+
+    /**
+     * Find set of nearest neighbors to vec. 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
+     *     maxCheck = the maximum number of restarts (in a best-bin-first manner)
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        int maxChecks = searchParams.checks;
+        float epsError = 1+searchParams.eps;
+
+        if (maxChecks==FLANN_CHECKS_UNLIMITED) {
+        	if (removed_) {
+        		getExactNeighbors<true>(result, vec, epsError);
+        	}
+        	else {
+        		getExactNeighbors<false>(result, vec, epsError);
+        	}
+        }
+        else {
+        	if (removed_) {
+        		getNeighbors<true>(result, vec, maxChecks, epsError);
+        	}
+        	else {
+        		getNeighbors<false>(result, vec, maxChecks, epsError);
+        	}
+        }
+    }
+
+protected:
+
+    /**
+     * Builds the index
+     */
+    void buildIndexImpl()
+    {
+        // Create a permutable array of indices to the input vectors.
+    	std::vector<int> ind(size_);
+        for (size_t i = 0; i < size_; ++i) {
+            ind[i] = int(i);
+        }
+
+        mean_ = new DistanceType[veclen_];
+        var_ = new DistanceType[veclen_];
+
+        tree_roots_.resize(trees_);
+        /* Construct the randomized trees. */
+        for (int i = 0; i < trees_; i++) {
+            /* Randomize the order of vectors to allow for unbiased sampling. */
+            std::random_shuffle(ind.begin(), ind.end());
+            tree_roots_[i] = divideTree(&ind[0], int(size_) );
+        }
+        delete[] mean_;
+        delete[] var_;
+    }
+
+    void freeIndex()
+    {
+    	for (size_t i=0;i<tree_roots_.size();++i) {
+    		// using placement new, so call destructor explicitly
+    		if (tree_roots_[i]!=NULL) tree_roots_[i]->~Node();
+    	}
+    	pool_.free();
+    }
+
+
+private:
+
+    /*--------------------- Internal Data Structures --------------------------*/
+    struct Node
+    {
+    	/**
+         * Dimension used for subdivision.
+         */
+        int divfeat;
+        /**
+         * The values used for subdivision.
+         */
+        DistanceType divval;
+        /**
+         * Point data
+         */
+        ElementType* point;
+		/**
+		* The child nodes.
+		*/
+		Node* child1, *child2;
+		Node(){
+			child1 = NULL;
+			child2 = NULL;
+		}
+		~Node() {
+			if (child1 != NULL) { child1->~Node(); child1 = NULL; }
+
+			if (child2 != NULL) { child2->~Node(); child2 = NULL; }
+		}
+
+    private:
+    	template<typename Archive>
+    	void serialize(Archive& ar)
+    	{
+    		typedef KDTreeIndex<Distance> Index;
+    		Index* obj = static_cast<Index*>(ar.getObject());
+
+    		ar & divfeat;
+    		ar & divval;
+
+    		bool leaf_node = false;
+    		if (Archive::is_saving::value) {
+    			leaf_node = ((child1==NULL) && (child2==NULL));
+    		}
+    		ar & leaf_node;
+
+    		if (leaf_node) {
+    			if (Archive::is_loading::value) {
+    				point = obj->points_[divfeat];
+    			}
+    		}
+
+    		if (!leaf_node) {
+				if (Archive::is_loading::value) {
+					child1 = new(obj->pool_) Node();
+					child2 = new(obj->pool_) Node();
+				}
+    			ar & *child1;
+    			ar & *child2;
+    		}
+    	}
+    	friend struct serialization::access;
+    };
+    typedef Node* NodePtr;
+    typedef BranchStruct<NodePtr, DistanceType> BranchSt;
+    typedef BranchSt* Branch;
+
+
+    void copyTree(NodePtr& dst, const NodePtr& src)
+    {
+    	dst = new(pool_) Node();
+    	dst->divfeat = src->divfeat;
+    	dst->divval = src->divval;
+    	if (src->child1==NULL && src->child2==NULL) {
+    		dst->point = points_[dst->divfeat];
+    		dst->child1 = NULL;
+    		dst->child2 = NULL;
+    	}
+    	else {
+    		copyTree(dst->child1, src->child1);
+    		copyTree(dst->child2, src->child2);
+    	}
+    }
+
+    /**
+     * 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(int* ind, int count)
+    {
+        NodePtr node = new(pool_) Node(); // allocate memory
+
+        /* If too few exemplars remain, then make this a leaf node. */
+        if (count == 1) {
+            node->child1 = node->child2 = NULL;    /* Mark as leaf node. */
+            node->divfeat = *ind;    /* Store index of this vec. */
+            node->point = points_[*ind];
+        }
+        else {
+            int idx;
+            int cutfeat;
+            DistanceType cutval;
+            meanSplit(ind, count, idx, cutfeat, cutval);
+
+            node->divfeat = cutfeat;
+            node->divval = cutval;
+            node->child1 = divideTree(ind, idx);
+            node->child2 = divideTree(ind+idx, count-idx);
+        }
+
+        return node;
+    }
+
+
+    /**
+     * Choose which feature to use in order to subdivide this set of vectors.
+     * Make a random choice among those with the highest variance, and use
+     * its variance as the threshold value.
+     */
+    void meanSplit(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval)
+    {
+        memset(mean_,0,veclen_*sizeof(DistanceType));
+        memset(var_,0,veclen_*sizeof(DistanceType));
+
+        /* Compute mean values.  Only the first SAMPLE_MEAN values need to be
+            sampled to get a good estimate.
+         */
+        int cnt = std::min((int)SAMPLE_MEAN+1, count);
+        for (int j = 0; j < cnt; ++j) {
+            ElementType* v = points_[ind[j]];
+            for (size_t k=0; k<veclen_; ++k) {
+                mean_[k] += v[k];
+            }
+        }
+        DistanceType div_factor = DistanceType(1)/cnt;
+        for (size_t k=0; k<veclen_; ++k) {
+            mean_[k] *= div_factor;
+        }
+
+        /* Compute variances (no need to divide by count). */
+        for (int j = 0; j < cnt; ++j) {
+            ElementType* v = points_[ind[j]];
+            for (size_t k=0; k<veclen_; ++k) {
+                DistanceType dist = v[k] - mean_[k];
+                var_[k] += dist * dist;
+            }
+        }
+        /* Select one of the highest variance indices at random. */
+        cutfeat = selectDivision(var_);
+        cutval = mean_[cutfeat];
+
+        int 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;
+
+        /* If either list is empty, it means that all remaining features
+         * are identical. Split in the middle to maintain a balanced tree.
+         */
+        if ((lim1==count)||(lim2==0)) index = count/2;
+    }
+
+
+    /**
+     * Select the top RAND_DIM largest values from v and return the index of
+     * one of these selected at random.
+     */
+    int selectDivision(DistanceType* v)
+    {
+        int num = 0;
+        size_t topind[RAND_DIM];
+
+        /* Create a list of the indices of the top RAND_DIM values. */
+        for (size_t i = 0; i < veclen_; ++i) {
+            if ((num < RAND_DIM)||(v[i] > v[topind[num-1]])) {
+                /* Put this element at end of topind. */
+                if (num < RAND_DIM) {
+                    topind[num++] = i;            /* Add to list. */
+                }
+                else {
+                    topind[num-1] = i;         /* Replace last element. */
+                }
+                /* Bubble end value down to right location by repeated swapping. */
+                int j = num - 1;
+                while (j > 0  &&  v[topind[j]] > v[topind[j-1]]) {
+                    std::swap(topind[j], topind[j-1]);
+                    --j;
+                }
+            }
+        }
+        /* Select a random integer in range [0,num-1], and return that index. */
+        int rnd = rand_int(num);
+        return (int)topind[rnd];
+    }
+
+
+    /**
+     *  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(int* ind, int count, int cutfeat, DistanceType cutval, int& lim1, int& lim2)
+    {
+        /* Move vector indices for left subtree to front of list. */
+        int left = 0;
+        int right = count-1;
+        for (;; ) {
+            while (left<=right && points_[ind[left]][cutfeat]<cutval) ++left;
+            while (left<=right && points_[ind[right]][cutfeat]>=cutval) --right;
+            if (left>right) break;
+            std::swap(ind[left], ind[right]); ++left; --right;
+        }
+        lim1 = left;
+        right = count-1;
+        for (;; ) {
+            while (left<=right && points_[ind[left]][cutfeat]<=cutval) ++left;
+            while (left<=right && points_[ind[right]][cutfeat]>cutval) --right;
+            if (left>right) break;
+            std::swap(ind[left], ind[right]); ++left; --right;
+        }
+        lim2 = left;
+    }
+
+    /**
+     * Performs an exact nearest neighbor search. The exact search performs a full
+     * traversal of the tree.
+     */
+    template<bool with_removed>
+    void getExactNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, float epsError) const
+    {
+        //		checkID -= 1;  /* Set a different unique ID for each search. */
+
+        if (trees_ > 1) {
+            fprintf(stderr,"It doesn't make any sense to use more than one tree for exact search");
+        }
+        if (trees_>0) {
+            searchLevelExact<with_removed>(result, vec, tree_roots_[0], 0.0, epsError);
+        }
+    }
+
+    /**
+     * Performs the approximate nearest-neighbor search. The search is approximate
+     * because the tree traversal is abandoned after a given number of descends in
+     * the tree.
+     */
+    template<bool with_removed>
+    void getNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, int maxCheck, float epsError) const
+    {
+        int i;
+        BranchSt branch;
+
+        int checkCount = 0;
+        Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
+        DynamicBitset checked(size_);
+
+        /* Search once through each tree down to root. */
+        for (i = 0; i < trees_; ++i) {
+            searchLevel<with_removed>(result, vec, tree_roots_[i], 0, checkCount, maxCheck, epsError, heap, checked);
+        }
+
+        /* Keep searching other branches from heap until finished. */
+        while ( heap->popMin(branch) && (checkCount < maxCheck || !result.full() )) {
+            searchLevel<with_removed>(result, vec, branch.node, branch.mindist, checkCount, maxCheck, epsError, heap, checked);
+        }
+
+        delete heap;
+
+    }
+
+    /**
+     *  Search starting from a given node of the tree.  Based on any mismatches at
+     *  higher levels, all exemplars below this level must have a distance of
+     *  at least "mindistsq".
+     */
+    template<bool with_removed>
+    void searchLevel(ResultSet<DistanceType>& result_set, const ElementType* vec, NodePtr node, DistanceType mindist, int& checkCount, int maxCheck,
+                     float epsError, Heap<BranchSt>* heap, DynamicBitset& checked) const
+    {
+        if (result_set.worstDist()<mindist) {
+            //			printf("Ignoring branch, too far\n");
+            return;
+        }
+
+        /* If this is a leaf node, then do check and return. */
+        if ((node->child1 == NULL)&&(node->child2 == NULL)) {
+            int index = node->divfeat;
+            if (with_removed) {
+            	if (removed_points_.test(index)) return;
+            }
+            /*  Do not check same node more than once when searching multiple trees. */
+            if ( checked.test(index) || ((checkCount>=maxCheck)&& result_set.full()) ) return;
+            checked.set(index);
+            checkCount++;
+
+            DistanceType dist = distance_(node->point, vec, veclen_);
+            result_set.addPoint(dist,index);
+            return;
+        }
+
+        /* Which child branch should be taken first? */
+        ElementType val = vec[node->divfeat];
+        DistanceType diff = val - node->divval;
+        NodePtr bestChild = (diff < 0) ? node->child1 : node->child2;
+        NodePtr otherChild = (diff < 0) ? node->child2 : node->child1;
+
+        /* Create a branch record for the branch not taken.  Add distance
+            of this feature boundary (we don't attempt to correct for any
+            use of this feature in a parent node, which is unlikely to
+            happen and would have only a small effect).  Don't bother
+            adding more branches to heap after halfway point, as cost of
+            adding exceeds their value.
+         */
+
+        DistanceType new_distsq = mindist + distance_.accum_dist(val, node->divval, node->divfeat);
+        //		if (2 * checkCount < maxCheck  ||  !result.full()) {
+        if ((new_distsq*epsError < result_set.worstDist())||  !result_set.full()) {
+            heap->insert( BranchSt(otherChild, new_distsq) );
+        }
+
+        /* Call recursively to search next level down. */
+        searchLevel<with_removed>(result_set, vec, bestChild, mindist, checkCount, maxCheck, epsError, heap, checked);
+    }
+
+    /**
+     * Performs an exact search in the tree starting from a node.
+     */
+    template<bool with_removed>
+    void searchLevelExact(ResultSet<DistanceType>& result_set, const ElementType* vec, const NodePtr node, DistanceType mindist, const float epsError) const
+    {
+        /* If this is a leaf node, then do check and return. */
+        if ((node->child1 == NULL)&&(node->child2 == NULL)) {
+            int index = node->divfeat;
+            if (with_removed) {
+            	if (removed_points_.test(index)) return; // ignore removed points
+            }
+            DistanceType dist = distance_(node->point, vec, veclen_);
+            result_set.addPoint(dist,index);
+
+            return;
+        }
+
+        /* Which child branch should be taken first? */
+        ElementType val = vec[node->divfeat];
+        DistanceType diff = val - node->divval;
+        NodePtr bestChild = (diff < 0) ? node->child1 : node->child2;
+        NodePtr otherChild = (diff < 0) ? node->child2 : node->child1;
+
+        /* Create a branch record for the branch not taken.  Add distance
+            of this feature boundary (we don't attempt to correct for any
+            use of this feature in a parent node, which is unlikely to
+            happen and would have only a small effect).  Don't bother
+            adding more branches to heap after halfway point, as cost of
+            adding exceeds their value.
+         */
+
+        DistanceType new_distsq = mindist + distance_.accum_dist(val, node->divval, node->divfeat);
+
+        /* Call recursively to search next level down. */
+        searchLevelExact<with_removed>(result_set, vec, bestChild, mindist, epsError);
+
+        if (mindist*epsError<=result_set.worstDist()) {
+            searchLevelExact<with_removed>(result_set, vec, otherChild, new_distsq, epsError);
+        }
+    }
+    
+    void addPointToTree(NodePtr node, int ind)
+    {
+        ElementType* point = points_[ind];
+        
+        if ((node->child1==NULL) && (node->child2==NULL)) {
+            ElementType* leaf_point = node->point;
+            ElementType max_span = 0;
+            size_t div_feat = 0;
+            for (size_t i=0;i<veclen_;++i) {
+                ElementType span = std::abs(point[i]-leaf_point[i]);
+                if (span > max_span) {
+                    max_span = span;
+                    div_feat = i;
+                }
+            }
+            NodePtr left = new(pool_) Node();
+            left->child1 = left->child2 = NULL;
+            NodePtr right = new(pool_) Node();
+            right->child1 = right->child2 = NULL;
+
+            if (point[div_feat]<leaf_point[div_feat]) {
+                left->divfeat = ind;
+                left->point = point;
+                right->divfeat = node->divfeat;
+                right->point = node->point;
+            }
+            else {
+                left->divfeat = node->divfeat;
+                left->point = node->point;
+                right->divfeat = ind;
+                right->point = point;
+            }
+            node->divfeat = div_feat;
+            node->divval = (point[div_feat]+leaf_point[div_feat])/2;
+            node->child1 = left;
+            node->child2 = right;            
+        }
+        else {
+            if (point[node->divfeat]<node->divval) {
+                addPointToTree(node->child1,ind);
+            }
+            else {
+                addPointToTree(node->child2,ind);                
+            }
+        }
+    }
+private:
+    void swap(KDTreeIndex& other)
+    {
+    	BaseClass::swap(other);
+    	std::swap(trees_, other.trees_);
+    	std::swap(tree_roots_, other.tree_roots_);
+    	std::swap(pool_, other.pool_);
+    }
+
+private:
+
+    enum
+    {
+        /**
+         * To improve efficiency, only SAMPLE_MEAN random values are used to
+         * compute the mean and variance at each level when building a tree.
+         * A value of 100 seems to perform as well as using all values.
+         */
+        SAMPLE_MEAN = 100,
+        /**
+         * Top random dimensions to consider
+         *
+         * When creating random trees, the dimension on which to subdivide is
+         * selected at random from among the top RAND_DIM dimensions with the
+         * highest variance.  A value of 5 works well.
+         */
+        RAND_DIM=5
+    };
+
+
+    /**
+     * Number of randomized trees that are used
+     */
+    int trees_;
+
+    DistanceType* mean_;
+    DistanceType* var_;
+
+    /**
+     * Array of k-d trees used to find neighbours.
+     */
+    std::vector<NodePtr> tree_roots_;
+
+    /**
+     * 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_;
+
+    USING_BASECLASS_SYMBOLS
+};   // class KDTreeIndex
+
+}
+
+#endif //FLANN_KDTREE_INDEX_H_

corelib/src/flann/algorithms/kdtree_single_index.h

@@ -0,0 +1,698 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_KDTREE_SINGLE_INDEX_H_
+#define FLANN_KDTREE_SINGLE_INDEX_H_
+
+#include <algorithm>
+#include <map>
+#include <cassert>
+#include <cstring>
+
+#include "flann/general.h"
+#include "flann/algorithms/nn_index.h"
+#include "flann/util/matrix.h"
+#include "flann/util/result_set.h"
+#include "flann/util/heap.h"
+#include "flann/util/allocator.h"
+#include "flann/util/random.h"
+#include "flann/util/saving.h"
+
+namespace flann
+{
+
+struct KDTreeSingleIndexParams : public IndexParams
+{
+    KDTreeSingleIndexParams(int leaf_max_size = 10, bool reorder = true)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_KDTREE_SINGLE;
+        (*this)["leaf_max_size"] = leaf_max_size;
+        (*this)["reorder"] = reorder;
+    }
+};
+
+
+/**
+ * Single kd-tree index
+ *
+ * Contains the k-d trees and other information for indexing a set of points
+ * for nearest-neighbor matching.
+ */
+template <typename Distance>
+class KDTreeSingleIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    typedef bool needs_kdtree_distance;
+
+    /**
+     * KDTree constructor
+     *
+     * Params:
+     *          params = parameters passed to the kdtree algorithm
+     */
+    KDTreeSingleIndex(const IndexParams& params = KDTreeSingleIndexParams(), Distance d = Distance() ) :
+        BaseClass(params, d), root_node_(NULL)
+    {
+        leaf_max_size_ = get_param(params,"leaf_max_size",10);
+        reorder_ = get_param(params, "reorder", true);
+    }
+
+    /**
+     * KDTree constructor
+     *
+     * Params:
+     *          inputData = dataset with the input features
+     *          params = parameters passed to the kdtree algorithm
+     */
+    KDTreeSingleIndex(const Matrix<ElementType>& inputData, const IndexParams& params = KDTreeSingleIndexParams(),
+                      Distance d = Distance() ) : BaseClass(params, d), root_node_(NULL)
+    {
+        leaf_max_size_ = get_param(params,"leaf_max_size",10);
+        reorder_ = get_param(params, "reorder", true);
+
+        setDataset(inputData);
+    }
+
+
+    KDTreeSingleIndex(const KDTreeSingleIndex& other) : BaseClass(other),
+            leaf_max_size_(other.leaf_max_size_),
+            reorder_(other.reorder_),
+            vind_(other.vind_),
+            root_bbox_(other.root_bbox_)
+    {
+        if (reorder_) {
+            data_ = flann::Matrix<ElementType>(new ElementType[size_*veclen_], size_, veclen_);
+            std::copy(other.data_[0], other.data_[0]+size_*veclen_, data_[0]);
+        }
+        copyTree(root_node_, other.root_node_);
+    }
+
+    KDTreeSingleIndex& operator=(KDTreeSingleIndex other)
+    {
+        this->swap(other);
+        return *this;
+    }
+    
+    /**
+     * Standard destructor
+     */
+    virtual ~KDTreeSingleIndex()
+    {
+        freeIndex();
+    }
+    
+    BaseClass* clone() const
+    {
+        return new KDTreeSingleIndex(*this);
+    }
+
+    using BaseClass::buildIndex;
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+        extendDataset(points);
+        buildIndex();
+    }
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_KDTREE_SINGLE;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+        ar.setObject(this);
+
+        if (reorder_) index_params_["save_dataset"] = false;
+
+        ar & *static_cast<NNIndex<Distance>*>(this);
+
+        ar & reorder_;
+        ar & leaf_max_size_;
+        ar & root_bbox_;
+        ar & vind_;
+
+        if (reorder_) {
+            ar & data_;
+        }
+
+        if (Archive::is_loading::value) {
+            root_node_ = new(pool_) Node();
+        }
+
+        ar & *root_node_;
+
+        if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+            index_params_["leaf_max_size"] = leaf_max_size_;
+            index_params_["reorder"] = reorder_;
+        }
+    }
+
+
+    void saveIndex(FILE* stream)
+    {
+        serialization::SaveArchive sa(stream);
+        sa & *this;
+    }
+
+
+    void loadIndex(FILE* stream)
+    {
+        freeIndex();
+        serialization::LoadArchive la(stream);
+        la & *this;
+    }
+
+    /**
+     * Computes the inde memory usage
+     * Returns: memory used by the index
+     */
+    int usedMemory() const
+    {
+        return pool_.usedMemory+pool_.wastedMemory+size_*sizeof(int);  // pool memory and vind array memory
+    }
+
+    /**
+     * Find set of nearest neighbors to vec. 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
+     *     maxCheck = the maximum number of restarts (in a best-bin-first manner)
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
+    {
+        float epsError = 1+searchParams.eps;
+
+        std::vector<DistanceType> dists(veclen_,0);
+        DistanceType distsq = computeInitialDistances(vec, dists);
+        if (removed_) {
+            searchLevel<true>(result, vec, root_node_, distsq, dists, epsError);
+        }
+        else {
+            searchLevel<false>(result, vec, root_node_, distsq, dists, epsError);
+        }
+    }
+
+protected:
+
+    /**
+     * Builds the index
+     */
+    void buildIndexImpl()
+    {
+        // Create a permutable array of indices to the input vectors.
+        vind_.resize(size_);
+        for (size_t i = 0; i < size_; i++) {
+            vind_[i] = i;
+        }
+
+        computeBoundingBox(root_bbox_);
+        root_node_ = divideTree(0, size_, root_bbox_ );   // construct the tree
+
+        if (reorder_) {
+            data_ = flann::Matrix<ElementType>(new ElementType[size_*veclen_], size_, veclen_);
+            for (size_t i=0; i<size_; ++i) {
+                std::copy(points_[vind_[i]], points_[vind_[i]]+veclen_, data_[i]);
+            }
+        }
+    }
+
+private:
+
+
+    /*--------------------- Internal Data Structures --------------------------*/
+    struct Node
+    {
+    	/**
+    	 * Indices of points in leaf node
+    	 */
+    	int left, right;
+    	/**
+    	 * Dimension used for subdivision.
+    	 */
+    	int divfeat;
+    	/**
+    	 * The values used for subdivision.
+    	 */
+    	DistanceType divlow, divhigh;
+        /**
+         * The child nodes.
+         */
+        Node* child1, * child2;
+        
+        ~Node()
+        {
+            if (child1) child1->~Node();
+            if (child2) child2->~Node();
+        }
+
+    private:
+        template<typename Archive>
+        void serialize(Archive& ar)
+        {
+            typedef KDTreeSingleIndex<Distance> Index;
+            Index* obj = static_cast<Index*>(ar.getObject());
+
+            ar & left;
+            ar & right;
+            ar & divfeat;
+            ar & divlow;
+            ar & divhigh;
+
+            bool leaf_node = false;
+            if (Archive::is_saving::value) {
+                leaf_node = ((child1==NULL) && (child2==NULL));
+            }
+            ar & leaf_node;
+
+            if (!leaf_node) {
+                if (Archive::is_loading::value) {
+                    child1 = new(obj->pool_) Node();
+                    child2 = new(obj->pool_) Node();
+                }
+                ar & *child1;
+                ar & *child2;
+            }
+        }
+        friend struct serialization::access;
+    };
+    typedef Node* NodePtr;
+
+
+    struct Interval
+    {
+        DistanceType low, high;
+        
+    private:
+        template <typename Archive>
+        void serialize(Archive& ar)
+        {
+            ar & low;
+            ar & high;
+        }
+        friend struct serialization::access;
+    };
+
+    typedef std::vector<Interval> BoundingBox;
+
+    typedef BranchStruct<NodePtr, DistanceType> BranchSt;
+    typedef BranchSt* Branch;
+
+
+    
+    void freeIndex()
+    {
+        if (data_.ptr()) {
+            delete[] data_.ptr();
+            data_ = flann::Matrix<ElementType>();
+        }
+        if (root_node_) root_node_->~Node();
+        pool_.free();
+    }
+    
+    void copyTree(NodePtr& dst, const NodePtr& src)
+    {
+        dst = new(pool_) Node();
+        *dst = *src;
+        if (src->child1!=NULL && src->child2!=NULL) {
+            copyTree(dst->child1, src->child1);
+            copyTree(dst->child2, src->child2);
+        }
+    }
+
+
+
+    void computeBoundingBox(BoundingBox& bbox)
+    {
+        bbox.resize(veclen_);
+        for (size_t i=0; i<veclen_; ++i) {
+            bbox[i].low = (DistanceType)points_[0][i];
+            bbox[i].high = (DistanceType)points_[0][i];
+        }
+        for (size_t k=1; k<size_; ++k) {
+            for (size_t i=0; i<veclen_; ++i) {
+                if (points_[k][i]<bbox[i].low) bbox[i].low = (DistanceType)points_[k][i];
+                if (points_[k][i]>bbox[i].high) bbox[i].high = (DistanceType)points_[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(int left, int right, BoundingBox& bbox)
+    {
+        NodePtr node = new (pool_) Node(); // allocate memory
+
+        /* If too few exemplars remain, then make this a leaf node. */
+        if ( (right-left) <= leaf_max_size_) {
+            node->child1 = node->child2 = NULL;    /* Mark as leaf node. */
+            node->left = left;
+            node->right = right;
+
+            // compute bounding-box of leaf points
+            for (size_t i=0; i<veclen_; ++i) {
+                bbox[i].low = (DistanceType)points_[vind_[left]][i];
+                bbox[i].high = (DistanceType)points_[vind_[left]][i];
+            }
+            for (int k=left+1; k<right; ++k) {
+                for (size_t i=0; i<veclen_; ++i) {
+                    if (bbox[i].low>points_[vind_[k]][i]) bbox[i].low=(DistanceType)points_[vind_[k]][i];
+                    if (bbox[i].high<points_[vind_[k]][i]) bbox[i].high=(DistanceType)points_[vind_[k]][i];
+                }
+            }
+        }
+        else {
+            int idx;
+            int cutfeat;
+            DistanceType cutval;
+            middleSplit(&vind_[0]+left, right-left, idx, cutfeat, cutval, bbox);
+
+            node->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->divlow = left_bbox[cutfeat].high;
+            node->divhigh = right_bbox[cutfeat].low;
+
+            for (size_t i=0; i<veclen_; ++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(int* ind, int count, int dim, ElementType& min_elem, ElementType& max_elem)
+    {
+        min_elem = points_[ind[0]][dim];
+        max_elem = points_[ind[0]][dim];
+        for (int i=1; i<count; ++i) {
+            ElementType val = points_[ind[i]][dim];
+            if (val<min_elem) min_elem = val;
+            if (val>max_elem) max_elem = val;
+        }
+    }
+
+    void middleSplit(int* ind, int count, int& 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 (size_t i=1; i<veclen_; ++i) {
+            ElementType span = bbox[i].high-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<veclen_; ++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;
+                }
+            }
+        }
+        int 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;
+        
+        assert(index > 0 && index < count);
+    }
+
+
+    void middleSplit_(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
+    {
+        const float eps_val=0.00001f;
+        DistanceType max_span = bbox[0].high-bbox[0].low;
+        for (size_t i=1; i<veclen_; ++i) {
+            DistanceType span = bbox[i].high-bbox[i].low;
+            if (span>max_span) {
+                max_span = span;
+            }
+        }
+        DistanceType max_spread = -1;
+        cutfeat = 0;
+        for (size_t i=0; i<veclen_; ++i) {
+            DistanceType span = bbox[i].high-bbox[i].low;
+            if (span>(DistanceType)((1-eps_val)*max_span)) {
+                ElementType min_elem, max_elem;
+                computeMinMax(ind, count, cutfeat, min_elem, max_elem);
+                DistanceType spread = (DistanceType)(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 = (DistanceType)min_elem;
+        else if (split_val>max_elem) cutval = (DistanceType)max_elem;
+        else cutval = split_val;
+
+        int 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;
+        
+        assert(index > 0 && index < count);
+    }
+
+
+    /**
+     *  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(int* ind, int count, int cutfeat, DistanceType cutval, int& lim1, int& lim2)
+    {
+        int left = 0;
+        int right = count-1;
+        for (;; ) {
+            while (left<=right && points_[ind[left]][cutfeat]<cutval) ++left;
+            while (left<=right && points_[ind[right]][cutfeat]>=cutval) --right;
+            if (left>right) break;
+            std::swap(ind[left], ind[right]); ++left; --right;
+        }
+
+        lim1 = left;
+        right = count-1;
+        for (;; ) {
+            while (left<=right && points_[ind[left]][cutfeat]<=cutval) ++left;
+            while (left<=right && points_[ind[right]][cutfeat]>cutval) --right;
+            if (left>right) break;
+            std::swap(ind[left], ind[right]); ++left; --right;
+        }
+        lim2 = left;
+    }
+
+    DistanceType computeInitialDistances(const ElementType* vec, std::vector<DistanceType>& dists) const
+    {
+        DistanceType distsq = 0.0;
+
+        for (size_t i = 0; i < veclen_; ++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.
+     */
+    template <bool with_removed>
+    void searchLevel(ResultSet<DistanceType>& 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)) {
+            DistanceType worst_dist = result_set.worstDist();
+            for (int i=node->left; i<node->right; ++i) {
+                if (with_removed) {
+                    if (removed_points_.test(vind_[i])) continue;
+                }
+                ElementType* point = reorder_ ? data_[i] : points_[vind_[i]];
+                DistanceType dist = distance_(vec, point, veclen_, worst_dist);
+                if (dist<worst_dist) {
+                    result_set.addPoint(dist,vind_[i]);
+                }
+            }
+            return;
+        }
+
+        /* Which child branch should be taken first? */
+        int idx = node->divfeat;
+        ElementType val = vec[idx];
+        DistanceType diff1 = val - node->divlow;
+        DistanceType diff2 = val - node->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->divhigh, idx);
+        }
+        else {
+            bestChild = node->child2;
+            otherChild = node->child1;
+            cut_dist = distance_.accum_dist( val, node->divlow, idx);
+        }
+
+        /* Call recursively to search next level down. */
+        searchLevel<with_removed>(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<with_removed>(result_set, vec, otherChild, mindistsq, dists, epsError);
+        }
+        dists[idx] = dst;
+    }
+
+    
+    void swap(KDTreeSingleIndex& other)
+    {
+        BaseClass::swap(other);
+        std::swap(leaf_max_size_, other.leaf_max_size_);
+        std::swap(reorder_, other.reorder_);
+        std::swap(vind_, other.vind_);
+        std::swap(data_, other.data_);
+        std::swap(root_node_, other.root_node_);
+        std::swap(root_bbox_, other.root_bbox_);
+        std::swap(pool_, other.pool_);
+    }
+    
+private:
+
+
+
+    int leaf_max_size_;
+    
+    
+    bool reorder_;
+
+    /**
+     *  Array of indices to vectors in the dataset.
+     */
+    std::vector<int> vind_;
+
+    Matrix<ElementType> data_;
+
+    /**
+     * Array of k-d trees used to find neighbours.
+     */
+    NodePtr root_node_;
+
+    /**
+     * Root bounding box
+     */
+    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_;
+
+    USING_BASECLASS_SYMBOLS
+
+};   // class KDTreeSingleIndex
+
+}
+
+#endif //FLANN_KDTREE_SINGLE_INDEX_H_

corelib/src/flann/algorithms/linear_index.h

@@ -0,0 +1,163 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_LINEAR_INDEX_H_
+#define FLANN_LINEAR_INDEX_H_
+
+#include "flann/general.h"
+#include "flann/algorithms/nn_index.h"
+
+namespace flann
+{
+
+struct LinearIndexParams : public IndexParams
+{
+    LinearIndexParams()
+    {
+        (* this)["algorithm"] = FLANN_INDEX_LINEAR;
+    }
+};
+
+template <typename Distance>
+class LinearIndex : public NNIndex<Distance>
+{
+public:
+
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    LinearIndex(const IndexParams& params = LinearIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+    }
+
+    LinearIndex(const Matrix<ElementType>& input_data, const IndexParams& params = LinearIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+        setDataset(input_data);
+    }
+
+    LinearIndex(const LinearIndex& other) : BaseClass(other)
+    {
+    }
+
+    LinearIndex& operator=(LinearIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    virtual ~LinearIndex()
+    {
+    }
+
+    BaseClass* clone() const
+    {
+    	return new LinearIndex(*this);
+    }
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+        extendDataset(points);
+    }
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_LINEAR;
+    }
+
+
+    int usedMemory() const
+    {
+        return 0;
+    }
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+    	}
+    }
+
+    void saveIndex(FILE* stream)
+    {
+    	serialization::SaveArchive sa(stream);
+    	sa & *this;
+    }
+
+    void loadIndex(FILE* stream)
+    {
+    	serialization::LoadArchive la(stream);
+    	la & *this;
+    }
+
+    void findNeighbors(ResultSet<DistanceType>& resultSet, const ElementType* vec, const SearchParams& /*searchParams*/) const
+    {
+    	if (removed_) {
+    		for (size_t i = 0; i < points_.size(); ++i) {
+    			if (removed_points_.test(i)) continue;
+    			DistanceType dist = distance_(points_[i], vec, veclen_);
+    			resultSet.addPoint(dist, i);
+    		}
+    	}
+    	else {
+    		for (size_t i = 0; i < points_.size(); ++i) {
+    			DistanceType dist = distance_(points_[i], vec, veclen_);
+    			resultSet.addPoint(dist, i);
+    		}
+    	}
+    }
+protected:
+    void buildIndexImpl()
+    {
+        /* nothing to do here for linear search */
+    }
+
+    void freeIndex()
+    {
+        /* nothing to do here for linear search */
+    }
+
+private:
+
+    USING_BASECLASS_SYMBOLS
+};
+
+}
+
+#endif // FLANN_LINEAR_INDEX_H_

corelib/src/flann/algorithms/lsh_index.h

@@ -0,0 +1,548 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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.
+ *************************************************************************/
+
+/***********************************************************************
+ * Author: Vincent Rabaud
+ *************************************************************************/
+
+#ifndef FLANN_LSH_INDEX_H_
+#define FLANN_LSH_INDEX_H_
+
+#include <algorithm>
+#include <cassert>
+#include <cstring>
+#include <map>
+#include <vector>
+
+#include "flann/general.h"
+#include "flann/algorithms/nn_index.h"
+#include "flann/util/matrix.h"
+#include "flann/util/result_set.h"
+#include "flann/util/heap.h"
+#include "flann/util/lsh_table.h"
+#include "flann/util/allocator.h"
+#include "flann/util/random.h"
+#include "flann/util/saving.h"
+
+namespace flann
+{
+
+struct LshIndexParams : public IndexParams
+{
+    LshIndexParams(unsigned int table_number = 12, unsigned int key_size = 20, unsigned int multi_probe_level = 2)
+    {
+        (* this)["algorithm"] = FLANN_INDEX_LSH;
+        // The number of hash tables to use
+        (*this)["table_number"] = table_number;
+        // The length of the key in the hash tables
+        (*this)["key_size"] = key_size;
+        // Number of levels to use in multi-probe (0 for standard LSH)
+        (*this)["multi_probe_level"] = multi_probe_level;
+    }
+};
+
+/**
+ * Randomized kd-tree index
+ *
+ * Contains the k-d trees and other information for indexing a set of points
+ * for nearest-neighbor matching.
+ */
+template<typename Distance>
+class LshIndex : public NNIndex<Distance>
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    typedef NNIndex<Distance> BaseClass;
+
+    /** Constructor
+     * @param params parameters passed to the LSH algorithm
+     * @param d the distance used
+     */
+    LshIndex(const IndexParams& params = LshIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+        table_number_ = get_param<unsigned int>(index_params_,"table_number",12);
+        key_size_ = get_param<unsigned int>(index_params_,"key_size",20);
+        multi_probe_level_ = get_param<unsigned int>(index_params_,"multi_probe_level",2);
+
+        fill_xor_mask(0, key_size_, multi_probe_level_, xor_masks_);
+    }
+
+
+    /** Constructor
+     * @param input_data dataset with the input features
+     * @param params parameters passed to the LSH algorithm
+     * @param d the distance used
+     */
+    LshIndex(const Matrix<ElementType>& input_data, const IndexParams& params = LshIndexParams(), Distance d = Distance()) :
+    	BaseClass(params, d)
+    {
+        table_number_ = get_param<unsigned int>(index_params_,"table_number",12);
+        key_size_ = get_param<unsigned int>(index_params_,"key_size",20);
+        multi_probe_level_ = get_param<unsigned int>(index_params_,"multi_probe_level",2);
+
+        fill_xor_mask(0, key_size_, multi_probe_level_, xor_masks_);
+
+        setDataset(input_data);
+    }
+
+    LshIndex(const LshIndex& other) : BaseClass(other),
+    	tables_(other.tables_),
+    	table_number_(other.table_number_),
+    	key_size_(other.key_size_),
+    	multi_probe_level_(other.multi_probe_level_),
+    	xor_masks_(other.xor_masks_)
+    {
+    }
+    
+    LshIndex& operator=(LshIndex other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    virtual ~LshIndex()
+    {
+    	freeIndex();
+    }
+
+
+    BaseClass* clone() const
+    {
+    	return new LshIndex(*this);
+    }
+    
+    using BaseClass::buildIndex;
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        assert(points.cols==veclen_);
+        size_t old_size = size_;
+
+        extendDataset(points);
+        
+        if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
+            buildIndex();
+        }
+        else {
+            for (unsigned int i = 0; i < table_number_; ++i) {
+                lsh::LshTable<ElementType>& table = tables_[i];                
+                for (size_t i=old_size;i<size_;++i) {
+                    table.add(i, points_[i]);
+                }            
+            }
+        }
+    }
+
+
+    flann_algorithm_t getType() const
+    {
+        return FLANN_INDEX_LSH;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar.setObject(this);
+
+    	ar & *static_cast<NNIndex<Distance>*>(this);
+
+    	ar & table_number_;
+    	ar & key_size_;
+    	ar & multi_probe_level_;
+
+    	ar & xor_masks_;
+    	ar & tables_;
+
+    	if (Archive::is_loading::value) {
+            index_params_["algorithm"] = getType();
+            index_params_["table_number"] = table_number_;
+            index_params_["key_size"] = key_size_;
+            index_params_["multi_probe_level"] = multi_probe_level_;
+    	}
+    }
+
+    void saveIndex(FILE* stream)
+    {
+    	serialization::SaveArchive sa(stream);
+    	sa & *this;
+    }
+
+    void loadIndex(FILE* stream)
+    {
+    	serialization::LoadArchive la(stream);
+    	la & *this;
+    }
+
+    /**
+     * Computes the index memory usage
+     * Returns: memory used by the index
+     */
+    int usedMemory() const
+    {
+        return size_ * sizeof(int);
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+    					Matrix<size_t>& indices,
+    					Matrix<DistanceType>& dists,
+    					size_t knn,
+    					const SearchParams& params) const
+    {
+        assert(queries.cols == veclen_);
+        assert(indices.rows >= queries.rows);
+        assert(dists.rows >= queries.rows);
+        assert(indices.cols >= knn);
+        assert(dists.cols >= knn);
+
+        int count = 0;
+        if (params.use_heap==FLANN_True) {
+#pragma omp parallel num_threads(params.cores)
+        	{
+        		KNNUniqueResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+        		for (int i = 0; i < (int)queries.rows; i++) {
+        			resultSet.clear();
+        			findNeighbors(resultSet, queries[i], params);
+        			size_t n = std::min(resultSet.size(), knn);
+        			resultSet.copy(indices[i], dists[i], n, params.sorted);
+        			indices_to_ids(indices[i], indices[i], n);
+        			count += n;
+        		}
+        	}
+        }
+        else {
+#pragma omp parallel num_threads(params.cores)
+        	{
+        		KNNResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+        		for (int i = 0; i < (int)queries.rows; i++) {
+        			resultSet.clear();
+        			findNeighbors(resultSet, queries[i], params);
+        			size_t n = std::min(resultSet.size(), knn);
+        			resultSet.copy(indices[i], dists[i], n, params.sorted);
+        			indices_to_ids(indices[i], indices[i], n);
+        			count += n;
+        		}
+        	}
+        }
+
+        return count;
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+					std::vector< std::vector<size_t> >& indices,
+					std::vector<std::vector<DistanceType> >& dists,
+    				size_t knn,
+    				const SearchParams& params) const
+    {
+        assert(queries.cols == veclen_);
+		if (indices.size() < queries.rows ) indices.resize(queries.rows);
+		if (dists.size() < queries.rows ) dists.resize(queries.rows);
+
+		int count = 0;
+		if (params.use_heap==FLANN_True) {
+#pragma omp parallel num_threads(params.cores)
+			{
+				KNNUniqueResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n > 0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+		else {
+#pragma omp parallel num_threads(params.cores)
+			{
+				KNNResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n > 0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+
+		return count;
+    }
+
+    /**
+     * Find set of nearest neighbors to vec. 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
+     *     maxCheck = the maximum number of restarts (in a best-bin-first manner)
+     */
+    void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& /*searchParams*/) const
+    {
+        getNeighbors(vec, result);
+    }
+
+protected:
+
+    /**
+     * Builds the index
+     */
+    void buildIndexImpl()
+    {
+        tables_.resize(table_number_);
+        std::vector<std::pair<size_t,ElementType*> > features;
+        features.reserve(points_.size());
+        for (size_t i=0;i<points_.size();++i) {
+        	features.push_back(std::make_pair(i, points_[i]));
+        }
+        for (unsigned int i = 0; i < table_number_; ++i) {
+            lsh::LshTable<ElementType>& table = tables_[i];
+            table = lsh::LshTable<ElementType>(veclen_, key_size_);
+
+            // Add the features to the table
+            table.add(features);
+        }
+    }
+
+    void freeIndex()
+    {
+        /* nothing to do here */
+    }
+
+
+private:
+    /** Defines the comparator on score and index
+     */
+    typedef std::pair<float, unsigned int> ScoreIndexPair;
+    struct SortScoreIndexPairOnSecond
+    {
+        bool operator()(const ScoreIndexPair& left, const ScoreIndexPair& right) const
+        {
+            return left.second < right.second;
+        }
+    };
+
+    /** Fills the different xor masks to use when getting the neighbors in multi-probe LSH
+     * @param key the key we build neighbors from
+     * @param lowest_index the lowest index of the bit set
+     * @param level the multi-probe level we are at
+     * @param xor_masks all the xor mask
+     */
+    void fill_xor_mask(lsh::BucketKey key, int lowest_index, unsigned int level,
+                       std::vector<lsh::BucketKey>& xor_masks)
+    {
+        xor_masks.push_back(key);
+        if (level == 0) return;
+        for (int index = lowest_index - 1; index >= 0; --index) {
+            // Create a new key
+            lsh::BucketKey new_key = key | (lsh::BucketKey(1) << index);
+            fill_xor_mask(new_key, index, level - 1, xor_masks);
+        }
+    }
+
+    /** Performs the approximate nearest-neighbor search.
+     * @param vec the feature to analyze
+     * @param do_radius flag indicating if we check the radius too
+     * @param radius the radius if it is a radius search
+     * @param do_k flag indicating if we limit the number of nn
+     * @param k_nn the number of nearest neighbors
+     * @param checked_average used for debugging
+     */
+    void getNeighbors(const ElementType* vec, bool do_radius, float radius, bool do_k, unsigned int k_nn,
+                      float& checked_average)
+    {
+        static std::vector<ScoreIndexPair> score_index_heap;
+
+        if (do_k) {
+            unsigned int worst_score = std::numeric_limits<unsigned int>::max();
+            typename std::vector<lsh::LshTable<ElementType> >::const_iterator table = tables_.begin();
+            typename std::vector<lsh::LshTable<ElementType> >::const_iterator table_end = tables_.end();
+            for (; table != table_end; ++table) {
+                size_t key = table->getKey(vec);
+                std::vector<lsh::BucketKey>::const_iterator xor_mask = xor_masks_.begin();
+                std::vector<lsh::BucketKey>::const_iterator xor_mask_end = xor_masks_.end();
+                for (; xor_mask != xor_mask_end; ++xor_mask) {
+                    size_t sub_key = key ^ (*xor_mask);
+                    const lsh::Bucket* bucket = table->getBucketFromKey(sub_key);
+                    if (bucket == 0) continue;
+
+                    // Go over each descriptor index
+                    std::vector<lsh::FeatureIndex>::const_iterator training_index = bucket->begin();
+                    std::vector<lsh::FeatureIndex>::const_iterator last_training_index = bucket->end();
+                    DistanceType hamming_distance;
+
+                    // Process the rest of the candidates
+                    for (; training_index < last_training_index; ++training_index) {
+                    	if (removed_ && removed_points_.test(*training_index)) continue;
+                        hamming_distance = distance_(vec, points_[*training_index].point, veclen_);
+
+                        if (hamming_distance < worst_score) {
+                            // Insert the new element
+                            score_index_heap.push_back(ScoreIndexPair(hamming_distance, training_index));
+                            std::push_heap(score_index_heap.begin(), score_index_heap.end());
+
+                            if (score_index_heap.size() > (unsigned int)k_nn) {
+                                // Remove the highest distance value as we have too many elements
+                                std::pop_heap(score_index_heap.begin(), score_index_heap.end());
+                                score_index_heap.pop_back();
+                                // Keep track of the worst score
+                                worst_score = score_index_heap.front().first;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+        else {
+            typename std::vector<lsh::LshTable<ElementType> >::const_iterator table = tables_.begin();
+            typename std::vector<lsh::LshTable<ElementType> >::const_iterator table_end = tables_.end();
+            for (; table != table_end; ++table) {
+                size_t key = table->getKey(vec);
+                std::vector<lsh::BucketKey>::const_iterator xor_mask = xor_masks_.begin();
+                std::vector<lsh::BucketKey>::const_iterator xor_mask_end = xor_masks_.end();
+                for (; xor_mask != xor_mask_end; ++xor_mask) {
+                    size_t sub_key = key ^ (*xor_mask);
+                    const lsh::Bucket* bucket = table->getBucketFromKey(sub_key);
+                    if (bucket == 0) continue;
+
+                    // Go over each descriptor index
+                    std::vector<lsh::FeatureIndex>::const_iterator training_index = bucket->begin();
+                    std::vector<lsh::FeatureIndex>::const_iterator last_training_index = bucket->end();
+                    DistanceType hamming_distance;
+
+                    // Process the rest of the candidates
+                    for (; training_index < last_training_index; ++training_index) {
+                    	if (removed_ && removed_points_.test(*training_index)) continue;
+                        // Compute the Hamming distance
+                        hamming_distance = distance_(vec, points_[*training_index].point, veclen_);
+                        if (hamming_distance < radius) score_index_heap.push_back(ScoreIndexPair(hamming_distance, training_index));
+                    }
+                }
+            }
+        }
+    }
+
+    /** Performs the approximate nearest-neighbor search.
+     * This is a slower version than the above as it uses the ResultSet
+     * @param vec the feature to analyze
+     */
+    void getNeighbors(const ElementType* vec, ResultSet<DistanceType>& result) const
+    {
+        typename std::vector<lsh::LshTable<ElementType> >::const_iterator table = tables_.begin();
+        typename std::vector<lsh::LshTable<ElementType> >::const_iterator table_end = tables_.end();
+        for (; table != table_end; ++table) {
+            size_t key = table->getKey(vec);
+            std::vector<lsh::BucketKey>::const_iterator xor_mask = xor_masks_.begin();
+            std::vector<lsh::BucketKey>::const_iterator xor_mask_end = xor_masks_.end();
+            for (; xor_mask != xor_mask_end; ++xor_mask) {
+                size_t sub_key = key ^ (*xor_mask);
+                const lsh::Bucket* bucket = table->getBucketFromKey(sub_key);
+                if (bucket == 0) continue;
+
+                // Go over each descriptor index
+                std::vector<lsh::FeatureIndex>::const_iterator training_index = bucket->begin();
+                std::vector<lsh::FeatureIndex>::const_iterator last_training_index = bucket->end();
+                DistanceType hamming_distance;
+
+                // Process the rest of the candidates
+                for (; training_index < last_training_index; ++training_index) {
+                	if (removed_ && removed_points_.test(*training_index)) continue;
+                    // Compute the Hamming distance
+                    hamming_distance = distance_(vec, points_[*training_index], veclen_);
+                    result.addPoint(hamming_distance, *training_index);
+                }
+            }
+        }
+    }
+
+
+    void swap(LshIndex& other)
+    {
+    	BaseClass::swap(other);
+    	std::swap(tables_, other.tables_);
+    	std::swap(size_at_build_, other.size_at_build_);
+    	std::swap(table_number_, other.table_number_);
+    	std::swap(key_size_, other.key_size_);
+    	std::swap(multi_probe_level_, other.multi_probe_level_);
+    	std::swap(xor_masks_, other.xor_masks_);
+    }
+
+    /** The different hash tables */
+    std::vector<lsh::LshTable<ElementType> > tables_;
+    
+    /** table number */
+    unsigned int table_number_;
+    /** key size */
+    unsigned int key_size_;
+    /** How far should we look for neighbors in multi-probe LSH */
+    unsigned int multi_probe_level_;
+
+    /** The XOR masks to apply to a key to get the neighboring buckets */
+    std::vector<lsh::BucketKey> xor_masks_;
+
+    USING_BASECLASS_SYMBOLS
+};
+}
+
+#endif //FLANN_LSH_INDEX_H_

corelib/src/flann/algorithms/nn_index.h

@@ -0,0 +1,907 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_NNINDEX_H
+#define FLANN_NNINDEX_H
+
+#include <vector>
+
+#include "flann/general.h"
+#include "flann/util/matrix.h"
+#include "flann/util/params.h"
+#include "flann/util/result_set.h"
+#include "flann/util/dynamic_bitset.h"
+#include "flann/util/saving.h"
+
+namespace flann
+{
+
+#define KNN_HEAP_THRESHOLD 250
+
+
+class IndexBase
+{
+public:
+    virtual ~IndexBase() {};
+
+    virtual size_t veclen() const = 0;
+
+    virtual size_t size() const = 0;
+
+    virtual flann_algorithm_t getType() const = 0;
+
+    virtual int usedMemory() const = 0;
+
+    virtual IndexParams getParameters() const = 0;
+
+    virtual void loadIndex(FILE* stream) = 0;
+
+    virtual void saveIndex(FILE* stream) = 0;
+};
+
+/**
+ * Nearest-neighbour index base class
+ */
+template <typename Distance>
+class NNIndex : public IndexBase
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+	NNIndex(Distance d) : distance_(d), last_id_(0), size_(0), size_at_build_(0), veclen_(0),
+			removed_(false), removed_count_(0), data_ptr_(NULL)
+	{
+	}
+
+	NNIndex(const IndexParams& params, Distance d) : distance_(d), last_id_(0), size_(0), size_at_build_(0), veclen_(0),
+			index_params_(params), removed_(false), removed_count_(0), data_ptr_(NULL)
+	{
+	}
+
+	NNIndex(const NNIndex& other) :
+		distance_(other.distance_),
+		last_id_(other.last_id_),
+		size_(other.size_),
+		size_at_build_(other.size_at_build_),
+		veclen_(other.veclen_),
+		index_params_(other.index_params_),
+		removed_(other.removed_),
+		removed_points_(other.removed_points_),
+		removed_count_(other.removed_count_),
+		ids_(other.ids_),
+		points_(other.points_),
+		data_ptr_(NULL)
+	{
+		if (other.data_ptr_) {
+			data_ptr_ = new ElementType[size_*veclen_];
+			std::copy(other.data_ptr_, other.data_ptr_+size_*veclen_, data_ptr_);
+			for (size_t i=0;i<size_;++i) {
+				points_[i] = data_ptr_ + i*veclen_;
+			}
+		}
+	}
+
+	virtual ~NNIndex()
+	{
+		if (data_ptr_) {
+			delete[] data_ptr_;
+		}
+	}
+
+
+	virtual NNIndex* clone() const = 0;
+
+	/**
+	 * Builds the index
+	 */
+	virtual void buildIndex()
+	{
+    	freeIndex();
+    	cleanRemovedPoints();
+
+    	// building index
+		buildIndexImpl();
+
+        size_at_build_ = size_;
+
+	}
+
+	/**
+	 * Builds the index using the specified dataset
+	 * @param dataset the dataset to use
+	 */
+    virtual void buildIndex(const Matrix<ElementType>& dataset)
+    {
+        setDataset(dataset);
+        this->buildIndex();
+    }
+
+	/**
+	 * @brief Incrementally add points to the index.
+	 * @param points Matrix with points to be added
+	 * @param rebuild_threshold
+	 */
+    virtual void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        throw FLANNException("Functionality not supported by this index");
+    }
+
+    /**
+     * Remove point from the index
+     * @param index Index of point to be removed
+     */
+    virtual void removePoint(size_t id)
+    {
+    	if (!removed_) {
+    		ids_.resize(size_);
+    		for (size_t i=0;i<size_;++i) {
+    			ids_[i] = i;
+    		}
+    		removed_points_.resize(size_);
+    		removed_points_.reset();
+    		last_id_ = size_;
+        	removed_ = true;
+    	}
+
+    	size_t point_index = id_to_index(id);
+    	if (point_index!=size_t(-1) && !removed_points_.test(point_index)) {
+    		removed_points_.set(point_index);
+    		removed_count_++;
+    	}
+    }
+
+
+    /**
+     * Get point with specific id
+     * @param id
+     * @return
+     */
+    virtual ElementType* getPoint(size_t id)
+    {
+    	size_t index = id_to_index(id);
+    	if (index!=size_t(-1)) {
+    		return points_[index];
+    	}
+    	else {
+    		return NULL;
+    	}
+    }
+
+    /**
+     * @return number of features in this index.
+     */
+    inline size_t size() const
+    {
+    	return size_ - removed_count_;
+    }
+
+    /**
+     * @return The dimensionality of the features in this index.
+     */
+    inline size_t veclen() const
+    {
+        return veclen_;
+    }
+
+    /**
+     * Returns the parameters used by the index.
+     *
+     * @return The index parameters
+     */
+    IndexParams getParameters() const
+    {
+        return index_params_;
+    }
+
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	IndexHeader header;
+
+    	if (Archive::is_saving::value) {
+            header.h.data_type = flann_datatype_value<ElementType>::value;
+            header.h.index_type = getType();
+            header.h.rows = size_;
+            header.h.cols = veclen_;
+    	}
+    	ar & header;
+
+    	// sanity checks
+    	if (Archive::is_loading::value) {
+            if (strncmp(header.h.signature,
+                        FLANN_SIGNATURE_,
+                        strlen(FLANN_SIGNATURE_) - strlen("v0.0")) != 0) {
+    	        throw FLANNException("Invalid index file, wrong signature");
+    	    }
+
+            if (header.h.data_type != flann_datatype_value<ElementType>::value) {
+                throw FLANNException("Datatype of saved index is different than of the one to be created.");
+            }
+
+            if (header.h.index_type != getType()) {
+                throw FLANNException("Saved index type is different then the current index type.");
+            }
+            // TODO: check for distance type
+
+    	}
+
+    	ar & size_;
+    	ar & veclen_;
+    	ar & size_at_build_;
+
+    	bool save_dataset;
+    	if (Archive::is_saving::value) {
+    		save_dataset = get_param(index_params_,"save_dataset", false);
+    	}
+    	ar & save_dataset;
+
+    	if (save_dataset) {
+    		if (Archive::is_loading::value) {
+    			if (data_ptr_) {
+    				delete[] data_ptr_;
+    			}
+    			data_ptr_ = new ElementType[size_*veclen_];
+    			points_.resize(size_);
+        		for (size_t i=0;i<size_;++i) {
+        			points_[i] = data_ptr_ + i*veclen_;
+        		}
+    		}
+    		for (size_t i=0;i<size_;++i) {
+    			ar & serialization::make_binary_object (points_[i], veclen_*sizeof(ElementType));
+    		}
+    	} else {
+    		if (points_.size()!=size_) {
+    			throw FLANNException("Saved index does not contain the dataset and no dataset was provided.");
+    		}
+    	}
+
+    	ar & last_id_;
+    	ar & ids_;
+    	ar & removed_;
+    	if (removed_) {
+    		ar & removed_points_;
+    	}
+    	ar & removed_count_;
+    }
+
+
+    /**
+     * @brief Perform k-nearest neighbor search
+     * @param[in] queries The query points for which to find the nearest neighbors
+     * @param[out] indices The indices of the nearest neighbors found
+     * @param[out] dists Distances to the nearest neighbors found
+     * @param[in] knn Number of nearest neighbors to return
+     * @param[in] params Search parameters
+     */
+    virtual int knnSearch(const Matrix<ElementType>& queries,
+    		Matrix<size_t>& indices,
+    		Matrix<DistanceType>& dists,
+    		size_t knn,
+    		const SearchParams& params) const
+    {
+    	assert(queries.cols == veclen());
+    	assert(indices.rows >= queries.rows);
+    	assert(dists.rows >= queries.rows);
+    	assert(indices.cols >= knn);
+    	assert(dists.cols >= knn);
+    	bool use_heap;
+
+    	if (params.use_heap==FLANN_Undefined) {
+    		use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
+    	}
+    	else {
+    		use_heap = (params.use_heap==FLANN_True)?true:false;
+    	}
+    	int count = 0;
+
+    	if (use_heap) {
+#pragma omp parallel num_threads(params.cores)
+    		{
+    			KNNResultSet2<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+    			for (int i = 0; i < (int)queries.rows; i++) {
+    				resultSet.clear();
+    				findNeighbors(resultSet, queries[i], params);
+    				size_t n = std::min(resultSet.size(), knn);
+    				resultSet.copy(indices[i], dists[i], n, params.sorted);
+    				indices_to_ids(indices[i], indices[i], n);
+    				count += n;
+    			}
+    		}
+    	}
+    	else {
+#pragma omp parallel num_threads(params.cores)
+    		{
+    			KNNSimpleResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+    			for (int i = 0; i < (int)queries.rows; i++) {
+    				resultSet.clear();
+    				findNeighbors(resultSet, queries[i], params);
+    				size_t n = std::min(resultSet.size(), knn);
+    				resultSet.copy(indices[i], dists[i], n, params.sorted);
+    				indices_to_ids(indices[i], indices[i], n);
+    				count += n;
+    			}
+    		}
+    	}
+    	return count;
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param knn
+     * @param params
+     * @return
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 Matrix<int>& indices,
+                                 Matrix<DistanceType>& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	flann::Matrix<size_t> indices_(new size_t[indices.rows*indices.cols], indices.rows, indices.cols);
+    	int result = knnSearch(queries, indices_, dists, knn, params);
+
+    	for (size_t i=0;i<indices.rows;++i) {
+    		for (size_t j=0;j<indices.cols;++j) {
+    			indices[i][j] = indices_[i][j];
+    		}
+    	}
+        delete[] indices_.ptr();
+    	return result;
+    }
+
+
+    /**
+     * @brief Perform k-nearest neighbor search
+     * @param[in] queries The query points for which to find the nearest neighbors
+     * @param[out] indices The indices of the nearest neighbors found
+     * @param[out] dists Distances to the nearest neighbors found
+     * @param[in] knn Number of nearest neighbors to return
+     * @param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+					std::vector< std::vector<size_t> >& indices,
+					std::vector<std::vector<DistanceType> >& dists,
+    				size_t knn,
+    				const SearchParams& params) const
+    {
+        assert(queries.cols == veclen());
+        bool use_heap;
+        if (params.use_heap==FLANN_Undefined) {
+        	use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
+        }
+        else {
+        	use_heap = (params.use_heap==FLANN_True)?true:false;
+        }
+
+        if (indices.size() < queries.rows ) indices.resize(queries.rows);
+		if (dists.size() < queries.rows ) dists.resize(queries.rows);
+
+		int count = 0;
+		if (use_heap) {
+#pragma omp parallel num_threads(params.cores)
+			{
+				KNNResultSet2<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n>0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+		else {
+#pragma omp parallel num_threads(params.cores)
+			{
+				KNNSimpleResultSet<DistanceType> resultSet(knn);
+#pragma omp for schedule(static) reduction(+:count)
+				for (int i = 0; i < (int)queries.rows; i++) {
+					resultSet.clear();
+					findNeighbors(resultSet, queries[i], params);
+					size_t n = std::min(resultSet.size(), knn);
+					indices[i].resize(n);
+					dists[i].resize(n);
+					if (n>0) {
+						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+						indices_to_ids(&indices[i][0], &indices[i][0], n);
+					}
+					count += n;
+				}
+			}
+		}
+
+		return count;
+    }
+
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param knn
+     * @param params
+     * @return
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 std::vector< std::vector<int> >& indices,
+                                 std::vector<std::vector<DistanceType> >& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	std::vector<std::vector<size_t> > indices_;
+    	int result = knnSearch(queries, indices_, dists, knn, params);
+
+    	indices.resize(indices_.size());
+    	for (size_t i=0;i<indices_.size();++i) {
+            indices[i].assign(indices_[i].begin(), indices_[i].end());
+    	}
+    	return result;
+    }
+
+    /**
+     * @brief Perform radius search
+     * @param[in] query The query point
+     * @param[out] indices The indices of the neighbors found within the given radius
+     * @param[out] dists The distances to the nearest neighbors found
+     * @param[in] radius The radius used for search
+     * @param[in] params Search parameters
+     * @return Number of neighbors found
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+    		Matrix<size_t>& indices,
+    		Matrix<DistanceType>& dists,
+    		float radius,
+    		const SearchParams& params) const
+    {
+    	assert(queries.cols == veclen());
+    	int count = 0;
+    	size_t num_neighbors = std::min(indices.cols, dists.cols);
+    	int max_neighbors = params.max_neighbors;
+    	if (max_neighbors<0) max_neighbors = num_neighbors;
+    	else max_neighbors = std::min(max_neighbors,(int)num_neighbors);
+
+    	if (max_neighbors==0) {
+#pragma omp parallel num_threads(params.cores)
+    		{
+    			CountRadiusResultSet<DistanceType> resultSet(radius);
+#pragma omp for schedule(static) reduction(+:count)
+    			for (int i = 0; i < (int)queries.rows; i++) {
+    				resultSet.clear();
+    				findNeighbors(resultSet, queries[i], params);
+    				count += resultSet.size();
+    			}
+    		}
+    	}
+    	else {
+    		// explicitly indicated to use unbounded radius result set
+    		// and we know there'll be enough room for resulting indices and dists
+    		if (params.max_neighbors<0 && (num_neighbors>=size())) {
+#pragma omp parallel num_threads(params.cores)
+    			{
+    				RadiusResultSet<DistanceType> resultSet(radius);
+#pragma omp for schedule(static) reduction(+:count)
+    				for (int i = 0; i < (int)queries.rows; i++) {
+    					resultSet.clear();
+    					findNeighbors(resultSet, queries[i], params);
+    					size_t n = resultSet.size();
+    					count += n;
+    					if (n>num_neighbors) n = num_neighbors;
+    					resultSet.copy(indices[i], dists[i], n, params.sorted);
+
+    					// mark the next element in the output buffers as unused
+    					if (n<indices.cols) indices[i][n] = size_t(-1);
+    					if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
+    					indices_to_ids(indices[i], indices[i], n);
+    				}
+    			}
+    		}
+    		else {
+    			// number of neighbors limited to max_neighbors
+#pragma omp parallel num_threads(params.cores)
+    			{
+    				KNNRadiusResultSet<DistanceType> resultSet(radius, max_neighbors);
+#pragma omp for schedule(static) reduction(+:count)
+    				for (int i = 0; i < (int)queries.rows; i++) {
+    					resultSet.clear();
+    					findNeighbors(resultSet, queries[i], params);
+    					size_t n = resultSet.size();
+    					count += n;
+    					if ((int)n>max_neighbors) n = max_neighbors;
+    					resultSet.copy(indices[i], dists[i], n, params.sorted);
+
+    					// mark the next element in the output buffers as unused
+    					if (n<indices.cols) indices[i][n] = size_t(-1);
+    					if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
+    					indices_to_ids(indices[i], indices[i], n);
+    				}
+    			}
+    		}
+    	}
+        return count;
+    }
+
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param radius
+     * @param params
+     * @return
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    Matrix<int>& indices,
+                                    Matrix<DistanceType>& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	flann::Matrix<size_t> indices_(new size_t[indices.rows*indices.cols], indices.rows, indices.cols);
+    	int result = radiusSearch(queries, indices_, dists, radius, params);
+
+    	for (size_t i=0;i<indices.rows;++i) {
+    		for (size_t j=0;j<indices.cols;++j) {
+    			indices[i][j] = indices_[i][j];
+    		}
+    	}
+        delete[] indices_.ptr();
+    	return result;
+    }
+
+    /**
+     * @brief Perform radius search
+     * @param[in] query The query point
+     * @param[out] indices The indices of the neighbors found within the given radius
+     * @param[out] dists The distances to the nearest neighbors found
+     * @param[in] radius The radius used for search
+     * @param[in] params Search parameters
+     * @return Number of neighbors found
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+    		std::vector< std::vector<size_t> >& indices,
+    		std::vector<std::vector<DistanceType> >& dists,
+    		float radius,
+    		const SearchParams& params) const
+    {
+        assert(queries.cols == veclen());
+    	int count = 0;
+    	// just count neighbors
+    	if (params.max_neighbors==0) {
+#pragma omp parallel num_threads(params.cores)
+    		{
+    			CountRadiusResultSet<DistanceType> resultSet(radius);
+#pragma omp for schedule(static) reduction(+:count)
+    			for (int i = 0; i < (int)queries.rows; i++) {
+    				resultSet.clear();
+    				findNeighbors(resultSet, queries[i], params);
+    				count += resultSet.size();
+    			}
+    		}
+    	}
+    	else {
+    		if (indices.size() < queries.rows ) indices.resize(queries.rows);
+    		if (dists.size() < queries.rows ) dists.resize(queries.rows);
+
+    		if (params.max_neighbors<0) {
+    			// search for all neighbors
+#pragma omp parallel num_threads(params.cores)
+    			{
+    				RadiusResultSet<DistanceType> resultSet(radius);
+#pragma omp for schedule(static) reduction(+:count)
+    				for (int i = 0; i < (int)queries.rows; i++) {
+    					resultSet.clear();
+    					findNeighbors(resultSet, queries[i], params);
+    					size_t n = resultSet.size();
+    					count += n;
+    					indices[i].resize(n);
+    					dists[i].resize(n);
+    					if (n > 0) {
+    						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+        					indices_to_ids(&indices[i][0], &indices[i][0], n);
+    					}
+    				}
+    			}
+    		}
+    		else {
+    			// number of neighbors limited to max_neighbors
+#pragma omp parallel num_threads(params.cores)
+    			{
+    				KNNRadiusResultSet<DistanceType> resultSet(radius, params.max_neighbors);
+#pragma omp for schedule(static) reduction(+:count)
+    				for (int i = 0; i < (int)queries.rows; i++) {
+    					resultSet.clear();
+    					findNeighbors(resultSet, queries[i], params);
+    					size_t n = resultSet.size();
+    					count += n;
+    					if ((int)n>params.max_neighbors) n = params.max_neighbors;
+    					indices[i].resize(n);
+    					dists[i].resize(n);
+    					if (n > 0) {
+    						resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
+        					indices_to_ids(&indices[i][0], &indices[i][0], n);
+    					}
+    				}
+    			}
+    		}
+    	}
+    	return count;
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param radius
+     * @param params
+     * @return
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    std::vector< std::vector<int> >& indices,
+                                    std::vector<std::vector<DistanceType> >& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	std::vector<std::vector<size_t> > indices_;
+    	int result = radiusSearch(queries, indices_, dists, radius, params);
+
+    	indices.resize(indices_.size());
+    	for (size_t i=0;i<indices_.size();++i) {
+            indices[i].assign(indices_[i].begin(), indices_[i].end());
+    	}
+    	return result;
+    }
+
+
+    virtual void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const = 0;
+
+protected:
+
+    virtual void freeIndex() = 0;
+
+    virtual void buildIndexImpl() = 0;
+
+    size_t id_to_index(size_t id)
+    {
+    	if (ids_.size()==0) {
+    		return id;
+    	}
+    	size_t point_index = size_t(-1);
+    	if (id < ids_.size() && ids_[id]==id) {
+    		return id;
+    	}
+    	else {
+    		// binary search
+    		size_t start = 0;
+    		size_t end = ids_.size();
+
+    		while (start<end) {
+    			size_t mid = (start+end)/2;
+    			if (ids_[mid]==id) {
+    				point_index = mid;
+    				break;
+    			}
+    			else if (ids_[mid]<id) {
+    				start = mid + 1;
+    			}
+    			else {
+    				end = mid;
+    			}
+    		}
+    	}
+    	return point_index;
+    }
+
+
+    void indices_to_ids(const size_t* in, size_t* out, size_t size) const
+    {
+		if (removed_) {
+			for (size_t i=0;i<size;++i) {
+				out[i] = ids_[in[i]];
+			}
+		}
+    }
+
+    void setDataset(const Matrix<ElementType>& dataset)
+    {
+    	size_ = dataset.rows;
+    	veclen_ = dataset.cols;
+    	last_id_ = 0;
+
+    	ids_.clear();
+    	removed_points_.clear();
+    	removed_ = false;
+    	removed_count_ = 0;
+
+    	points_.resize(size_);
+    	for (size_t i=0;i<size_;++i) {
+    		points_[i] = dataset[i];
+    	}
+    }
+
+    void extendDataset(const Matrix<ElementType>& new_points)
+    {
+    	size_t new_size = size_ + new_points.rows;
+    	if (removed_) {
+    		removed_points_.resize(new_size);
+    		ids_.resize(new_size);
+    	}
+    	points_.resize(new_size);
+    	for (size_t i=size_;i<new_size;++i) {
+    		points_[i] = new_points[i-size_];
+    		if (removed_) {
+    			ids_[i] = last_id_++;
+    			removed_points_.reset(i);
+    		}
+    	}
+    	size_ = new_size;
+    }
+
+
+    void cleanRemovedPoints()
+    {
+    	if (!removed_) return;
+
+    	size_t last_idx = 0;
+    	for (size_t i=0;i<size_;++i) {
+    		if (!removed_points_.test(i)) {
+    			points_[last_idx] = points_[i];
+    			ids_[last_idx] = ids_[i];
+    			removed_points_.reset(last_idx);
+    			++last_idx;
+    		}
+    	}
+    	points_.resize(last_idx);
+    	ids_.resize(last_idx);
+    	removed_points_.resize(last_idx);
+    	size_ = last_idx;
+    	removed_count_ = 0;
+    }
+
+    void swap(NNIndex& other)
+    {
+    	std::swap(distance_, other.distance_);
+    	std::swap(last_id_, other.last_id_);
+    	std::swap(size_, other.size_);
+    	std::swap(size_at_build_, other.size_at_build_);
+    	std::swap(veclen_, other.veclen_);
+    	std::swap(index_params_, other.index_params_);
+    	std::swap(removed_, other.removed_);
+    	std::swap(removed_points_, other.removed_points_);
+    	std::swap(removed_count_, other.removed_count_);
+    	std::swap(ids_, other.ids_);
+    	std::swap(points_, other.points_);
+    	std::swap(data_ptr_, other.data_ptr_);
+    }
+
+protected:
+
+    /**
+     * The distance functor
+     */
+    Distance distance_;
+
+
+    /**
+     * Each index point has an associated ID. IDs are assigned sequentially in
+     * increasing order. This indicates the ID assigned to the last point added to the
+     * index.
+     */
+    size_t last_id_;
+
+    /**
+     * Number of points in the index (and database)
+     */
+    size_t size_;
+
+    /**
+     * Number of features in the dataset when the index was last built.
+     */
+    size_t size_at_build_;
+
+    /**
+     * Size of one point in the index (and database)
+     */
+    size_t veclen_;
+
+    /**
+     * Parameters of the index.
+     */
+    IndexParams index_params_;
+
+    /**
+     * Flag indicating if at least a point was removed from the index
+     */
+    bool removed_;
+
+    /**
+     * Array used to mark points removed from the index
+     */
+    DynamicBitset removed_points_;
+
+    /**
+     * Number of points removed from the index
+     */
+    size_t removed_count_;
+
+    /**
+     * Array of point IDs, returned by nearest-neighbour operations
+     */
+    std::vector<size_t> ids_;
+
+    /**
+     * Point data
+     */
+    std::vector<ElementType*> points_;
+
+    /**
+     * Pointer to dataset memory if allocated by this index, otherwise NULL
+     */
+    ElementType* data_ptr_;
+
+
+};
+
+
+#define USING_BASECLASS_SYMBOLS \
+		using NNIndex<Distance>::distance_;\
+		using NNIndex<Distance>::size_;\
+		using NNIndex<Distance>::size_at_build_;\
+		using NNIndex<Distance>::veclen_;\
+		using NNIndex<Distance>::index_params_;\
+		using NNIndex<Distance>::removed_points_;\
+		using NNIndex<Distance>::ids_;\
+		using NNIndex<Distance>::removed_;\
+		using NNIndex<Distance>::points_;\
+		using NNIndex<Distance>::extendDataset;\
+		using NNIndex<Distance>::setDataset;\
+		using NNIndex<Distance>::cleanRemovedPoints;\
+		using NNIndex<Distance>::indices_to_ids;
+
+
+
+}
+
+
+#endif //FLANN_NNINDEX_H

corelib/src/flann/config.h

@@ -0,0 +1,38 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * 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 FLANN_CONFIG_H_
+#define FLANN_CONFIG_H_
+
+#ifdef FLANN_VERSION_
+#undef FLANN_VERSION_
+#endif
+#define FLANN_VERSION_ "1.8.4"
+
+#endif /* FLANN_CONFIG_H_ */

corelib/src/flann/config.h.in

@@ -0,0 +1,38 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * 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 FLANN_CONFIG_H_
+#define FLANN_CONFIG_H_
+
+#ifdef FLANN_VERSION_
+#undef FLANN_VERSION_
+#endif
+#define FLANN_VERSION_ "${FLANN_VERSION}"
+
+#endif /* FLANN_CONFIG_H_ */

corelib/src/flann/defines.h

@@ -0,0 +1,156 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * 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 FLANN_DEFINES_H_
+#define FLANN_DEFINES_H_
+
+#include "config.h"
+
+#ifdef FLANN_EXPORT
+#undef FLANN_EXPORT
+#endif
+#ifdef WIN32
+/* win32 dll export/import directives */
+ #ifdef FLANN_EXPORTS
+  #define FLANN_EXPORT __declspec(dllexport)
+ #elif defined(FLANN_STATIC)
+  #define FLANN_EXPORT
+ #else
+  #define FLANN_EXPORT __declspec(dllimport)
+ #endif
+#else
+/* unix needs nothing */
+ #define FLANN_EXPORT
+#endif
+
+#ifdef FLANN_DEPRECATED
+#undef FLANN_DEPRECATED
+#endif
+#ifdef __GNUC__
+#define FLANN_DEPRECATED __attribute__ ((deprecated))
+#elif defined(_MSC_VER)
+#define FLANN_DEPRECATED __declspec(deprecated)
+#else
+#pragma message("WARNING: You need to implement FLANN_DEPRECATED for this compiler")
+#define FLANN_DEPRECATED
+#endif
+
+#undef FLANN_PLATFORM_64_BIT
+#undef FLANN_PLATFORM_32_BIT
+#if __amd64__ || __x86_64__ || _WIN64 || _M_X64
+#define FLANN_PLATFORM_64_BIT
+#else
+#define FLANN_PLATFORM_32_BIT
+#endif
+
+#undef FLANN_ARRAY_LEN
+#define FLANN_ARRAY_LEN(a) (sizeof(a)/sizeof(a[0]))
+
+#ifdef __cplusplus
+namespace flann {
+#endif
+
+/* Nearest neighbour index algorithms */
+enum flann_algorithm_t
+{
+    FLANN_INDEX_LINEAR 			= 0,
+    FLANN_INDEX_KDTREE 			= 1,
+    FLANN_INDEX_KMEANS 			= 2,
+    FLANN_INDEX_COMPOSITE 		= 3,
+    FLANN_INDEX_KDTREE_SINGLE 	= 4,
+    FLANN_INDEX_HIERARCHICAL 	= 5,
+    FLANN_INDEX_LSH 			= 6,
+#ifdef FLANN_USE_CUDA
+    FLANN_INDEX_KDTREE_CUDA 	= 7,
+#endif
+    FLANN_INDEX_SAVED 			= 254,
+    FLANN_INDEX_AUTOTUNED 		= 255,
+};
+
+enum flann_centers_init_t
+{
+    FLANN_CENTERS_RANDOM = 0,
+    FLANN_CENTERS_GONZALES = 1,
+    FLANN_CENTERS_KMEANSPP = 2,
+    FLANN_CENTERS_GROUPWISE = 3,
+};
+
+enum flann_log_level_t
+{
+    FLANN_LOG_NONE = 0,
+    FLANN_LOG_FATAL = 1,
+    FLANN_LOG_ERROR = 2,
+    FLANN_LOG_WARN = 3,
+    FLANN_LOG_INFO = 4,
+    FLANN_LOG_DEBUG = 5
+};
+
+enum flann_distance_t
+{
+    FLANN_DIST_EUCLIDEAN 			= 1,
+    FLANN_DIST_L2 					= 1,
+    FLANN_DIST_MANHATTAN 			= 2,
+    FLANN_DIST_L1 					= 2,
+    FLANN_DIST_MINKOWSKI 			= 3,
+    FLANN_DIST_MAX   				= 4,
+    FLANN_DIST_HIST_INTERSECT  		= 5,
+    FLANN_DIST_HELLINGER 			= 6,
+    FLANN_DIST_CHI_SQUARE		 	= 7,
+    FLANN_DIST_KULLBACK_LEIBLER  	= 8,
+    FLANN_DIST_HAMMING         		= 9,
+    FLANN_DIST_HAMMING_LUT			= 10,
+    FLANN_DIST_HAMMING_POPCNT   	= 11,
+    FLANN_DIST_L2_SIMPLE	   		= 12,
+};
+
+enum flann_datatype_t
+{
+    FLANN_NONE 		= -1,
+    FLANN_INT8 		= 0,
+    FLANN_INT16 	= 1,
+    FLANN_INT32 	= 2,
+    FLANN_INT64 	= 3,
+    FLANN_UINT8 	= 4,
+    FLANN_UINT16 	= 5,
+    FLANN_UINT32 	= 6,
+    FLANN_UINT64 	= 7,
+    FLANN_FLOAT32 	= 8,
+    FLANN_FLOAT64 	= 9
+};
+
+enum flann_checks_t {
+    FLANN_CHECKS_UNLIMITED = -1,
+    FLANN_CHECKS_AUTOTUNED = -2,
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif /* FLANN_DEFINES_H_ */

corelib/src/flann/ext/lz4.h

@@ -0,0 +1,360 @@
+/*
+   LZ4 - Fast LZ compression algorithm
+   Header File
+   Copyright (C) 2011-2015, Yann Collet.
+
+   BSD 2-Clause License (http://www.opensource.org/licenses/bsd-license.php)
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are
+   met:
+
+       * Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+       * 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 COPYRIGHT HOLDERS AND CONTRIBUTORS
+   "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 COPYRIGHT
+   OWNER OR CONTRIBUTORS 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.
+
+   You can contact the author at :
+   - LZ4 source repository : https://github.com/Cyan4973/lz4
+   - LZ4 public forum : https://groups.google.com/forum/#!forum/lz4c
+*/
+#pragma once
+
+#if defined (__cplusplus)
+extern "C" {
+#endif
+
+/*
+ * lz4.h provides block compression functions, and gives full buffer control to programmer.
+ * If you need to generate inter-operable compressed data (respecting LZ4 frame specification),
+ * and can let the library handle its own memory, please use lz4frame.h instead.
+*/
+
+/**************************************
+*  Version
+**************************************/
+#define LZ4_VERSION_MAJOR    1    /* for breaking interface changes  */
+#define LZ4_VERSION_MINOR    7    /* for new (non-breaking) interface capabilities */
+#define LZ4_VERSION_RELEASE  1    /* for tweaks, bug-fixes, or development */
+#define LZ4_VERSION_NUMBER (LZ4_VERSION_MAJOR *100*100 + LZ4_VERSION_MINOR *100 + LZ4_VERSION_RELEASE)
+int LZ4_versionNumber (void);
+
+/**************************************
+*  Tuning parameter
+**************************************/
+/*
+ * LZ4_MEMORY_USAGE :
+ * Memory usage formula : N->2^N Bytes (examples : 10 -> 1KB; 12 -> 4KB ; 16 -> 64KB; 20 -> 1MB; etc.)
+ * Increasing memory usage improves compression ratio
+ * Reduced memory usage can improve speed, due to cache effect
+ * Default value is 14, for 16KB, which nicely fits into Intel x86 L1 cache
+ */
+#define LZ4_MEMORY_USAGE 14
+
+
+/**************************************
+*  Simple Functions
+**************************************/
+
+int LZ4_compress_default(const char* source, char* dest, int sourceSize, int maxDestSize);
+int LZ4_decompress_safe (const char* source, char* dest, int compressedSize, int maxDecompressedSize);
+
+/*
+LZ4_compress_default() :
+    Compresses 'sourceSize' bytes from buffer 'source'
+    into already allocated 'dest' buffer of size 'maxDestSize'.
+    Compression is guaranteed to succeed if 'maxDestSize' >= LZ4_compressBound(sourceSize).
+    It also runs faster, so it's a recommended setting.
+    If the function cannot compress 'source' into a more limited 'dest' budget,
+    compression stops *immediately*, and the function result is zero.
+    As a consequence, 'dest' content is not valid.
+    This function never writes outside 'dest' buffer, nor read outside 'source' buffer.
+        sourceSize  : Max supported value is LZ4_MAX_INPUT_VALUE
+        maxDestSize : full or partial size of buffer 'dest' (which must be already allocated)
+        return : the number of bytes written into buffer 'dest' (necessarily <= maxOutputSize)
+              or 0 if compression fails
+
+LZ4_decompress_safe() :
+    compressedSize : is the precise full size of the compressed block.
+    maxDecompressedSize : is the size of destination buffer, which must be already allocated.
+    return : the number of bytes decompressed into destination buffer (necessarily <= maxDecompressedSize)
+             If destination buffer is not large enough, decoding will stop and output an error code (<0).
+             If the source stream is detected malformed, the function will stop decoding and return a negative result.
+             This function is protected against buffer overflow exploits, including malicious data packets.
+             It never writes outside output buffer, nor reads outside input buffer.
+*/
+
+
+/**************************************
+*  Advanced Functions
+**************************************/
+#define LZ4_MAX_INPUT_SIZE        0x7E000000   /* 2 113 929 216 bytes */
+#define LZ4_COMPRESSBOUND(isize)  ((unsigned)(isize) > (unsigned)LZ4_MAX_INPUT_SIZE ? 0 : (isize) + ((isize)/255) + 16)
+
+/*
+LZ4_compressBound() :
+    Provides the maximum size that LZ4 compression may output in a "worst case" scenario (input data not compressible)
+    This function is primarily useful for memory allocation purposes (destination buffer size).
+    Macro LZ4_COMPRESSBOUND() is also provided for compilation-time evaluation (stack memory allocation for example).
+    Note that LZ4_compress_default() compress faster when dest buffer size is >= LZ4_compressBound(srcSize)
+        inputSize  : max supported value is LZ4_MAX_INPUT_SIZE
+        return : maximum output size in a "worst case" scenario
+              or 0, if input size is too large ( > LZ4_MAX_INPUT_SIZE)
+*/
+int LZ4_compressBound(int inputSize);
+
+/*
+LZ4_compress_fast() :
+    Same as LZ4_compress_default(), but allows to select an "acceleration" factor.
+    The larger the acceleration value, the faster the algorithm, but also the lesser the compression.
+    It's a trade-off. It can be fine tuned, with each successive value providing roughly +~3% to speed.
+    An acceleration value of "1" is the same as regular LZ4_compress_default()
+    Values <= 0 will be replaced by ACCELERATION_DEFAULT (see lz4.c), which is 1.
+*/
+int LZ4_compress_fast (const char* source, char* dest, int sourceSize, int maxDestSize, int acceleration);
+
+
+/*
+LZ4_compress_fast_extState() :
+    Same compression function, just using an externally allocated memory space to store compression state.
+    Use LZ4_sizeofState() to know how much memory must be allocated,
+    and allocate it on 8-bytes boundaries (using malloc() typically).
+    Then, provide it as 'void* state' to compression function.
+*/
+int LZ4_sizeofState(void);
+int LZ4_compress_fast_extState (void* state, const char* source, char* dest, int inputSize, int maxDestSize, int acceleration);
+
+
+/*
+LZ4_compress_destSize() :
+    Reverse the logic, by compressing as much data as possible from 'source' buffer
+    into already allocated buffer 'dest' of size 'targetDestSize'.
+    This function either compresses the entire 'source' content into 'dest' if it's large enough,
+    or fill 'dest' buffer completely with as much data as possible from 'source'.
+        *sourceSizePtr : will be modified to indicate how many bytes where read from 'source' to fill 'dest'.
+                         New value is necessarily <= old value.
+        return : Nb bytes written into 'dest' (necessarily <= targetDestSize)
+              or 0 if compression fails
+*/
+int LZ4_compress_destSize (const char* source, char* dest, int* sourceSizePtr, int targetDestSize);
+
+
+/*
+LZ4_decompress_fast() :
+    originalSize : is the original and therefore uncompressed size
+    return : the number of bytes read from the source buffer (in other words, the compressed size)
+             If the source stream is detected malformed, the function will stop decoding and return a negative result.
+             Destination buffer must be already allocated. Its size must be a minimum of 'originalSize' bytes.
+    note : This function fully respect memory boundaries for properly formed compressed data.
+           It is a bit faster than LZ4_decompress_safe().
+           However, it does not provide any protection against intentionally modified data stream (malicious input).
+           Use this function in trusted environment only (data to decode comes from a trusted source).
+*/
+int LZ4_decompress_fast (const char* source, char* dest, int originalSize);
+
+/*
+LZ4_decompress_safe_partial() :
+    This function decompress a compressed block of size 'compressedSize' at position 'source'
+    into destination buffer 'dest' of size 'maxDecompressedSize'.
+    The function tries to stop decompressing operation as soon as 'targetOutputSize' has been reached,
+    reducing decompression time.
+    return : the number of bytes decoded in the destination buffer (necessarily <= maxDecompressedSize)
+       Note : this number can be < 'targetOutputSize' should the compressed block to decode be smaller.
+             Always control how many bytes were decoded.
+             If the source stream is detected malformed, the function will stop decoding and return a negative result.
+             This function never writes outside of output buffer, and never reads outside of input buffer. It is therefore protected against malicious data packets
+*/
+int LZ4_decompress_safe_partial (const char* source, char* dest, int compressedSize, int targetOutputSize, int maxDecompressedSize);
+
+
+/***********************************************
+*  Streaming Compression Functions
+***********************************************/
+#define LZ4_STREAMSIZE_U64 ((1 << (LZ4_MEMORY_USAGE-3)) + 4)
+#define LZ4_STREAMSIZE     (LZ4_STREAMSIZE_U64 * sizeof(long long))
+/*
+ * LZ4_stream_t
+ * information structure to track an LZ4 stream.
+ * important : init this structure content before first use !
+ * note : only allocated directly the structure if you are statically linking LZ4
+ *        If you are using liblz4 as a DLL, please use below construction methods instead.
+ */
+typedef struct { long long table[LZ4_STREAMSIZE_U64]; } LZ4_stream_t;
+
+/*
+ * LZ4_resetStream
+ * Use this function to init an allocated LZ4_stream_t structure
+ */
+void LZ4_resetStream (LZ4_stream_t* streamPtr);
+
+/*
+ * LZ4_createStream will allocate and initialize an LZ4_stream_t structure
+ * LZ4_freeStream releases its memory.
+ * In the context of a DLL (liblz4), please use these methods rather than the static struct.
+ * They are more future proof, in case of a change of LZ4_stream_t size.
+ */
+LZ4_stream_t* LZ4_createStream(void);
+int           LZ4_freeStream (LZ4_stream_t* streamPtr);
+
+/*
+ * LZ4_loadDict
+ * Use this function to load a static dictionary into LZ4_stream.
+ * Any previous data will be forgotten, only 'dictionary' will remain in memory.
+ * Loading a size of 0 is allowed.
+ * Return : dictionary size, in bytes (necessarily <= 64 KB)
+ */
+int LZ4_loadDict (LZ4_stream_t* streamPtr, const char* dictionary, int dictSize);
+
+/*
+ * LZ4_compress_fast_continue
+ * Compress buffer content 'src', using data from previously compressed blocks as dictionary to improve compression ratio.
+ * Important : Previous data blocks are assumed to still be present and unmodified !
+ * 'dst' buffer must be already allocated.
+ * If maxDstSize >= LZ4_compressBound(srcSize), compression is guaranteed to succeed, and runs faster.
+ * If not, and if compressed data cannot fit into 'dst' buffer size, compression stops, and function returns a zero.
+ */
+int LZ4_compress_fast_continue (LZ4_stream_t* streamPtr, const char* src, char* dst, int srcSize, int maxDstSize, int acceleration);
+
+/*
+ * LZ4_saveDict
+ * If previously compressed data block is not guaranteed to remain available at its memory location
+ * save it into a safer place (char* safeBuffer)
+ * Note : you don't need to call LZ4_loadDict() afterwards,
+ *        dictionary is immediately usable, you can therefore call LZ4_compress_fast_continue()
+ * Return : saved dictionary size in bytes (necessarily <= dictSize), or 0 if error
+ */
+int LZ4_saveDict (LZ4_stream_t* streamPtr, char* safeBuffer, int dictSize);
+
+
+/************************************************
+*  Streaming Decompression Functions
+************************************************/
+
+#define LZ4_STREAMDECODESIZE_U64  4
+#define LZ4_STREAMDECODESIZE     (LZ4_STREAMDECODESIZE_U64 * sizeof(unsigned long long))
+typedef struct { unsigned long long table[LZ4_STREAMDECODESIZE_U64]; } LZ4_streamDecode_t;
+/*
+ * LZ4_streamDecode_t
+ * information structure to track an LZ4 stream.
+ * init this structure content using LZ4_setStreamDecode or memset() before first use !
+ *
+ * In the context of a DLL (liblz4) please prefer usage of construction methods below.
+ * They are more future proof, in case of a change of LZ4_streamDecode_t size in the future.
+ * LZ4_createStreamDecode will allocate and initialize an LZ4_streamDecode_t structure
+ * LZ4_freeStreamDecode releases its memory.
+ */
+LZ4_streamDecode_t* LZ4_createStreamDecode(void);
+int                 LZ4_freeStreamDecode (LZ4_streamDecode_t* LZ4_stream);
+
+/*
+ * LZ4_setStreamDecode
+ * Use this function to instruct where to find the dictionary.
+ * Setting a size of 0 is allowed (same effect as reset).
+ * Return : 1 if OK, 0 if error
+ */
+int LZ4_setStreamDecode (LZ4_streamDecode_t* LZ4_streamDecode, const char* dictionary, int dictSize);
+
+/*
+*_continue() :
+    These decoding functions allow decompression of multiple blocks in "streaming" mode.
+    Previously decoded blocks *must* remain available at the memory position where they were decoded (up to 64 KB)
+    In the case of a ring buffers, decoding buffer must be either :
+    - Exactly same size as encoding buffer, with same update rule (block boundaries at same positions)
+      In which case, the decoding & encoding ring buffer can have any size, including very small ones ( < 64 KB).
+    - Larger than encoding buffer, by a minimum of maxBlockSize more bytes.
+      maxBlockSize is implementation dependent. It's the maximum size you intend to compress into a single block.
+      In which case, encoding and decoding buffers do not need to be synchronized,
+      and encoding ring buffer can have any size, including small ones ( < 64 KB).
+    - _At least_ 64 KB + 8 bytes + maxBlockSize.
+      In which case, encoding and decoding buffers do not need to be synchronized,
+      and encoding ring buffer can have any size, including larger than decoding buffer.
+    Whenever these conditions are not possible, save the last 64KB of decoded data into a safe buffer,
+    and indicate where it is saved using LZ4_setStreamDecode()
+*/
+int LZ4_decompress_safe_continue (LZ4_streamDecode_t* LZ4_streamDecode, const char* source, char* dest, int compressedSize, int maxDecompressedSize);
+int LZ4_decompress_fast_continue (LZ4_streamDecode_t* LZ4_streamDecode, const char* source, char* dest, int originalSize);
+
+
+/*
+Advanced decoding functions :
+*_usingDict() :
+    These decoding functions work the same as
+    a combination of LZ4_setStreamDecode() followed by LZ4_decompress_x_continue()
+    They are stand-alone. They don't need nor update an LZ4_streamDecode_t structure.
+*/
+int LZ4_decompress_safe_usingDict (const char* source, char* dest, int compressedSize, int maxDecompressedSize, const char* dictStart, int dictSize);
+int LZ4_decompress_fast_usingDict (const char* source, char* dest, int originalSize, const char* dictStart, int dictSize);
+
+
+
+/**************************************
+*  Obsolete Functions
+**************************************/
+/* Deprecate Warnings */
+/* Should these warnings messages be a problem,
+   it is generally possible to disable them,
+   with -Wno-deprecated-declarations for gcc
+   or _CRT_SECURE_NO_WARNINGS in Visual for example.
+   You can also define LZ4_DEPRECATE_WARNING_DEFBLOCK. */
+#ifndef LZ4_DEPRECATE_WARNING_DEFBLOCK
+#  define LZ4_DEPRECATE_WARNING_DEFBLOCK
+#  define LZ4_GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__)
+#  if (LZ4_GCC_VERSION >= 405) || defined(__clang__)
+#    define LZ4_DEPRECATED(message) __attribute__((deprecated(message)))
+#  elif (LZ4_GCC_VERSION >= 301)
+#    define LZ4_DEPRECATED(message) __attribute__((deprecated))
+#  elif defined(_MSC_VER)
+#    define LZ4_DEPRECATED(message) __declspec(deprecated(message))
+#  else
+#    pragma message("WARNING: You need to implement LZ4_DEPRECATED for this compiler")
+#    define LZ4_DEPRECATED(message)
+#  endif
+#endif /* LZ4_DEPRECATE_WARNING_DEFBLOCK */
+
+/* Obsolete compression functions */
+/* These functions are planned to start generate warnings by r131 approximately */
+int LZ4_compress               (const char* source, char* dest, int sourceSize);
+int LZ4_compress_limitedOutput (const char* source, char* dest, int sourceSize, int maxOutputSize);
+int LZ4_compress_withState               (void* state, const char* source, char* dest, int inputSize);
+int LZ4_compress_limitedOutput_withState (void* state, const char* source, char* dest, int inputSize, int maxOutputSize);
+int LZ4_compress_continue                (LZ4_stream_t* LZ4_streamPtr, const char* source, char* dest, int inputSize);
+int LZ4_compress_limitedOutput_continue  (LZ4_stream_t* LZ4_streamPtr, const char* source, char* dest, int inputSize, int maxOutputSize);
+
+/* Obsolete decompression functions */
+/* These function names are completely deprecated and must no longer be used.
+   They are only provided here for compatibility with older programs.
+    - LZ4_uncompress is the same as LZ4_decompress_fast
+    - LZ4_uncompress_unknownOutputSize is the same as LZ4_decompress_safe
+   These function prototypes are now disabled; uncomment them only if you really need them.
+   It is highly recommended to stop using these prototypes and migrate to maintained ones */
+/* int LZ4_uncompress (const char* source, char* dest, int outputSize); */
+/* int LZ4_uncompress_unknownOutputSize (const char* source, char* dest, int isize, int maxOutputSize); */
+
+/* Obsolete streaming functions; use new streaming interface whenever possible */
+LZ4_DEPRECATED("use LZ4_createStream() instead") void* LZ4_create (char* inputBuffer);
+LZ4_DEPRECATED("use LZ4_createStream() instead") int   LZ4_sizeofStreamState(void);
+LZ4_DEPRECATED("use LZ4_resetStream() instead")  int   LZ4_resetStreamState(void* state, char* inputBuffer);
+LZ4_DEPRECATED("use LZ4_saveDict() instead")     char* LZ4_slideInputBuffer (void* state);
+
+/* Obsolete streaming decoding functions */
+LZ4_DEPRECATED("use LZ4_decompress_safe_usingDict() instead") int LZ4_decompress_safe_withPrefix64k (const char* src, char* dst, int compressedSize, int maxDstSize);
+LZ4_DEPRECATED("use LZ4_decompress_fast_usingDict() instead") int LZ4_decompress_fast_withPrefix64k (const char* src, char* dst, int originalSize);
+
+
+#if defined (__cplusplus)
+}
+#endif

corelib/src/flann/ext/lz4hc.c

@@ -0,0 +1,731 @@
+/*
+    LZ4 HC - High Compression Mode of LZ4
+    Copyright (C) 2011-2015, Yann Collet.
+
+    BSD 2-Clause License (http://www.opensource.org/licenses/bsd-license.php)
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are
+    met:
+
+    * Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+    * 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 COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "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 COPYRIGHT
+    OWNER OR CONTRIBUTORS 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.
+
+    You can contact the author at :
+       - LZ4 source repository : https://github.com/Cyan4973/lz4
+       - LZ4 public forum : https://groups.google.com/forum/#!forum/lz4c
+*/
+
+
+
+/**************************************
+*  Tuning Parameter
+**************************************/
+static const int LZ4HC_compressionLevel_default = 9;
+
+
+/**************************************
+*  Includes
+**************************************/
+#include "lz4hc.h"
+
+
+/**************************************
+*  Local Compiler Options
+**************************************/
+#if defined(__GNUC__)
+#  pragma GCC diagnostic ignored "-Wunused-function"
+#endif
+
+#if defined (__clang__)
+#  pragma clang diagnostic ignored "-Wunused-function"
+#endif
+
+
+/**************************************
+*  Common LZ4 definition
+**************************************/
+#define LZ4_COMMONDEFS_ONLY
+#include "lz4.c"
+
+
+/**************************************
+*  Local Constants
+**************************************/
+#define DICTIONARY_LOGSIZE 16
+#define MAXD (1<<DICTIONARY_LOGSIZE)
+#define MAXD_MASK (MAXD - 1)
+
+#define HASH_LOG (DICTIONARY_LOGSIZE-1)
+#define HASHTABLESIZE (1 << HASH_LOG)
+#define HASH_MASK (HASHTABLESIZE - 1)
+
+#define OPTIMAL_ML (int)((ML_MASK-1)+MINMATCH)
+
+static const int g_maxCompressionLevel = 16;
+
+
+/**************************************
+*  Local Types
+**************************************/
+typedef struct
+{
+    U32   hashTable[HASHTABLESIZE];
+    U16   chainTable[MAXD];
+    const BYTE* end;        /* next block here to continue on current prefix */
+    const BYTE* base;       /* All index relative to this position */
+    const BYTE* dictBase;   /* alternate base for extDict */
+    BYTE* inputBuffer;      /* deprecated */
+    U32   dictLimit;        /* below that point, need extDict */
+    U32   lowLimit;         /* below that point, no more dict */
+    U32   nextToUpdate;     /* index from which to continue dictionary update */
+    U32   compressionLevel;
+} LZ4HC_Data_Structure;
+
+
+/**************************************
+*  Local Macros
+**************************************/
+#define HASH_FUNCTION(i)       (((i) * 2654435761U) >> ((MINMATCH*8)-HASH_LOG))
+//#define DELTANEXTU16(p)        chainTable[(p) & MAXD_MASK]   /* flexible, MAXD dependent */
+#define DELTANEXTU16(p)        chainTable[(U16)(p)]   /* faster */
+
+static U32 LZ4HC_hashPtr(const void* ptr) { return HASH_FUNCTION(LZ4_read32(ptr)); }
+
+
+
+/**************************************
+*  HC Compression
+**************************************/
+static void LZ4HC_init (LZ4HC_Data_Structure* hc4, const BYTE* start)
+{
+    MEM_INIT((void*)hc4->hashTable, 0, sizeof(hc4->hashTable));
+    MEM_INIT(hc4->chainTable, 0xFF, sizeof(hc4->chainTable));
+    hc4->nextToUpdate = 64 KB;
+    hc4->base = start - 64 KB;
+    hc4->end = start;
+    hc4->dictBase = start - 64 KB;
+    hc4->dictLimit = 64 KB;
+    hc4->lowLimit = 64 KB;
+}
+
+
+/* Update chains up to ip (excluded) */
+FORCE_INLINE void LZ4HC_Insert (LZ4HC_Data_Structure* hc4, const BYTE* ip)
+{
+    U16* chainTable = hc4->chainTable;
+    U32* HashTable  = hc4->hashTable;
+    const BYTE* const base = hc4->base;
+    const U32 target = (U32)(ip - base);
+    U32 idx = hc4->nextToUpdate;
+
+    while(idx < target)
+    {
+        U32 h = LZ4HC_hashPtr(base+idx);
+        size_t delta = idx - HashTable[h];
+        if (delta>MAX_DISTANCE) delta = MAX_DISTANCE;
+        DELTANEXTU16(idx) = (U16)delta;
+        HashTable[h] = idx;
+        idx++;
+    }
+
+    hc4->nextToUpdate = target;
+}
+
+
+FORCE_INLINE int LZ4HC_InsertAndFindBestMatch (LZ4HC_Data_Structure* hc4,   /* Index table will be updated */
+                                               const BYTE* ip, const BYTE* const iLimit,
+                                               const BYTE** matchpos,
+                                               const int maxNbAttempts)
+{
+    U16* const chainTable = hc4->chainTable;
+    U32* const HashTable = hc4->hashTable;
+    const BYTE* const base = hc4->base;
+    const BYTE* const dictBase = hc4->dictBase;
+    const U32 dictLimit = hc4->dictLimit;
+    const U32 lowLimit = (hc4->lowLimit + 64 KB > (U32)(ip-base)) ? hc4->lowLimit : (U32)(ip - base) - (64 KB - 1);
+    U32 matchIndex;
+    const BYTE* match;
+    int nbAttempts=maxNbAttempts;
+    size_t ml=0;
+
+    /* HC4 match finder */
+    LZ4HC_Insert(hc4, ip);
+    matchIndex = HashTable[LZ4HC_hashPtr(ip)];
+
+    while ((matchIndex>=lowLimit) && (nbAttempts))
+    {
+        nbAttempts--;
+        if (matchIndex >= dictLimit)
+        {
+            match = base + matchIndex;
+            if (*(match+ml) == *(ip+ml)
+                && (LZ4_read32(match) == LZ4_read32(ip)))
+            {
+                size_t mlt = LZ4_count(ip+MINMATCH, match+MINMATCH, iLimit) + MINMATCH;
+                if (mlt > ml) { ml = mlt; *matchpos = match; }
+            }
+        }
+        else
+        {
+            match = dictBase + matchIndex;
+            if (LZ4_read32(match) == LZ4_read32(ip))
+            {
+                size_t mlt;
+                const BYTE* vLimit = ip + (dictLimit - matchIndex);
+                if (vLimit > iLimit) vLimit = iLimit;
+                mlt = LZ4_count(ip+MINMATCH, match+MINMATCH, vLimit) + MINMATCH;
+                if ((ip+mlt == vLimit) && (vLimit < iLimit))
+                    mlt += LZ4_count(ip+mlt, base+dictLimit, iLimit);
+                if (mlt > ml) { ml = mlt; *matchpos = base + matchIndex; }   /* virtual matchpos */
+            }
+        }
+        matchIndex -= DELTANEXTU16(matchIndex);
+    }
+
+    return (int)ml;
+}
+
+
+FORCE_INLINE int LZ4HC_InsertAndGetWiderMatch (
+    LZ4HC_Data_Structure* hc4,
+    const BYTE* const ip,
+    const BYTE* const iLowLimit,
+    const BYTE* const iHighLimit,
+    int longest,
+    const BYTE** matchpos,
+    const BYTE** startpos,
+    const int maxNbAttempts)
+{
+    U16* const chainTable = hc4->chainTable;
+    U32* const HashTable = hc4->hashTable;
+    const BYTE* const base = hc4->base;
+    const U32 dictLimit = hc4->dictLimit;
+    const BYTE* const lowPrefixPtr = base + dictLimit;
+    const U32 lowLimit = (hc4->lowLimit + 64 KB > (U32)(ip-base)) ? hc4->lowLimit : (U32)(ip - base) - (64 KB - 1);
+    const BYTE* const dictBase = hc4->dictBase;
+    U32   matchIndex;
+    int nbAttempts = maxNbAttempts;
+    int delta = (int)(ip-iLowLimit);
+
+
+    /* First Match */
+    LZ4HC_Insert(hc4, ip);
+    matchIndex = HashTable[LZ4HC_hashPtr(ip)];
+
+    while ((matchIndex>=lowLimit) && (nbAttempts))
+    {
+        nbAttempts--;
+        if (matchIndex >= dictLimit)
+        {
+            const BYTE* matchPtr = base + matchIndex;
+            if (*(iLowLimit + longest) == *(matchPtr - delta + longest))
+                if (LZ4_read32(matchPtr) == LZ4_read32(ip))
+                {
+                    int mlt = MINMATCH + LZ4_count(ip+MINMATCH, matchPtr+MINMATCH, iHighLimit);
+                    int back = 0;
+
+                    while ((ip+back>iLowLimit)
+                           && (matchPtr+back > lowPrefixPtr)
+                           && (ip[back-1] == matchPtr[back-1]))
+                            back--;
+
+                    mlt -= back;
+
+                    if (mlt > longest)
+                    {
+                        longest = (int)mlt;
+                        *matchpos = matchPtr+back;
+                        *startpos = ip+back;
+                    }
+                }
+        }
+        else
+        {
+            const BYTE* matchPtr = dictBase + matchIndex;
+            if (LZ4_read32(matchPtr) == LZ4_read32(ip))
+            {
+                size_t mlt;
+                int back=0;
+                const BYTE* vLimit = ip + (dictLimit - matchIndex);
+                if (vLimit > iHighLimit) vLimit = iHighLimit;
+                mlt = LZ4_count(ip+MINMATCH, matchPtr+MINMATCH, vLimit) + MINMATCH;
+                if ((ip+mlt == vLimit) && (vLimit < iHighLimit))
+                    mlt += LZ4_count(ip+mlt, base+dictLimit, iHighLimit);
+                while ((ip+back > iLowLimit) && (matchIndex+back > lowLimit) && (ip[back-1] == matchPtr[back-1])) back--;
+                mlt -= back;
+                if ((int)mlt > longest) { longest = (int)mlt; *matchpos = base + matchIndex + back; *startpos = ip+back; }
+            }
+        }
+        matchIndex -= DELTANEXTU16(matchIndex);
+    }
+
+    return longest;
+}
+
+
+typedef enum { noLimit = 0, limitedOutput = 1 } limitedOutput_directive;
+
+#define LZ4HC_DEBUG 0
+#if LZ4HC_DEBUG
+static unsigned debug = 0;
+#endif
+
+FORCE_INLINE int LZ4HC_encodeSequence (
+    const BYTE** ip,
+    BYTE** op,
+    const BYTE** anchor,
+    int matchLength,
+    const BYTE* const match,
+    limitedOutput_directive limitedOutputBuffer,
+    BYTE* oend)
+{
+    int length;
+    BYTE* token;
+
+#if LZ4HC_DEBUG
+    if (debug) printf("literal : %u  --  match : %u  --  offset : %u\n", (U32)(*ip - *anchor), (U32)matchLength, (U32)(*ip-match));
+#endif
+
+    /* Encode Literal length */
+    length = (int)(*ip - *anchor);
+    token = (*op)++;
+    if ((limitedOutputBuffer) && ((*op + (length>>8) + length + (2 + 1 + LASTLITERALS)) > oend)) return 1;   /* Check output limit */
+    if (length>=(int)RUN_MASK) { int len; *token=(RUN_MASK<<ML_BITS); len = length-RUN_MASK; for(; len > 254 ; len-=255) *(*op)++ = 255;  *(*op)++ = (BYTE)len; }
+    else *token = (BYTE)(length<<ML_BITS);
+
+    /* Copy Literals */
+    LZ4_wildCopy(*op, *anchor, (*op) + length);
+    *op += length;
+
+    /* Encode Offset */
+    LZ4_writeLE16(*op, (U16)(*ip-match)); *op += 2;
+
+    /* Encode MatchLength */
+    length = (int)(matchLength-MINMATCH);
+    if ((limitedOutputBuffer) && (*op + (length>>8) + (1 + LASTLITERALS) > oend)) return 1;   /* Check output limit */
+    if (length>=(int)ML_MASK) { *token+=ML_MASK; length-=ML_MASK; for(; length > 509 ; length-=510) { *(*op)++ = 255; *(*op)++ = 255; } if (length > 254) { length-=255; *(*op)++ = 255; } *(*op)++ = (BYTE)length; }
+    else *token += (BYTE)(length);
+
+    /* Prepare next loop */
+    *ip += matchLength;
+    *anchor = *ip;
+
+    return 0;
+}
+
+
+static int LZ4HC_compress_generic (
+    void* ctxvoid,
+    const char* source,
+    char* dest,
+    int inputSize,
+    int maxOutputSize,
+    int compressionLevel,
+    limitedOutput_directive limit
+    )
+{
+    LZ4HC_Data_Structure* ctx = (LZ4HC_Data_Structure*) ctxvoid;
+    const BYTE* ip = (const BYTE*) source;
+    const BYTE* anchor = ip;
+    const BYTE* const iend = ip + inputSize;
+    const BYTE* const mflimit = iend - MFLIMIT;
+    const BYTE* const matchlimit = (iend - LASTLITERALS);
+
+    BYTE* op = (BYTE*) dest;
+    BYTE* const oend = op + maxOutputSize;
+
+    unsigned maxNbAttempts;
+    int   ml, ml2, ml3, ml0;
+    const BYTE* ref=NULL;
+    const BYTE* start2=NULL;
+    const BYTE* ref2=NULL;
+    const BYTE* start3=NULL;
+    const BYTE* ref3=NULL;
+    const BYTE* start0;
+    const BYTE* ref0;
+
+
+    /* init */
+    if (compressionLevel > g_maxCompressionLevel) compressionLevel = g_maxCompressionLevel;
+    if (compressionLevel < 1) compressionLevel = LZ4HC_compressionLevel_default;
+    maxNbAttempts = 1 << (compressionLevel-1);
+    ctx->end += inputSize;
+
+    ip++;
+
+    /* Main Loop */
+    while (ip < mflimit)
+    {
+        ml = LZ4HC_InsertAndFindBestMatch (ctx, ip, matchlimit, (&ref), maxNbAttempts);
+        if (!ml) { ip++; continue; }
+
+        /* saved, in case we would skip too much */
+        start0 = ip;
+        ref0 = ref;
+        ml0 = ml;
+
+_Search2:
+        if (ip+ml < mflimit)
+            ml2 = LZ4HC_InsertAndGetWiderMatch(ctx, ip + ml - 2, ip + 1, matchlimit, ml, &ref2, &start2, maxNbAttempts);
+        else ml2 = ml;
+
+        if (ml2 == ml)  /* No better match */
+        {
+            if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml, ref, limit, oend)) return 0;
+            continue;
+        }
+
+        if (start0 < ip)
+        {
+            if (start2 < ip + ml0)   /* empirical */
+            {
+                ip = start0;
+                ref = ref0;
+                ml = ml0;
+            }
+        }
+
+        /* Here, start0==ip */
+        if ((start2 - ip) < 3)   /* First Match too small : removed */
+        {
+            ml = ml2;
+            ip = start2;
+            ref =ref2;
+            goto _Search2;
+        }
+
+_Search3:
+        /*
+        * Currently we have :
+        * ml2 > ml1, and
+        * ip1+3 <= ip2 (usually < ip1+ml1)
+        */
+        if ((start2 - ip) < OPTIMAL_ML)
+        {
+            int correction;
+            int new_ml = ml;
+            if (new_ml > OPTIMAL_ML) new_ml = OPTIMAL_ML;
+            if (ip+new_ml > start2 + ml2 - MINMATCH) new_ml = (int)(start2 - ip) + ml2 - MINMATCH;
+            correction = new_ml - (int)(start2 - ip);
+            if (correction > 0)
+            {
+                start2 += correction;
+                ref2 += correction;
+                ml2 -= correction;
+            }
+        }
+        /* Now, we have start2 = ip+new_ml, with new_ml = min(ml, OPTIMAL_ML=18) */
+
+        if (start2 + ml2 < mflimit)
+            ml3 = LZ4HC_InsertAndGetWiderMatch(ctx, start2 + ml2 - 3, start2, matchlimit, ml2, &ref3, &start3, maxNbAttempts);
+        else ml3 = ml2;
+
+        if (ml3 == ml2) /* No better match : 2 sequences to encode */
+        {
+            /* ip & ref are known; Now for ml */
+            if (start2 < ip+ml)  ml = (int)(start2 - ip);
+            /* Now, encode 2 sequences */
+            if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml, ref, limit, oend)) return 0;
+            ip = start2;
+            if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml2, ref2, limit, oend)) return 0;
+            continue;
+        }
+
+        if (start3 < ip+ml+3) /* Not enough space for match 2 : remove it */
+        {
+            if (start3 >= (ip+ml)) /* can write Seq1 immediately ==> Seq2 is removed, so Seq3 becomes Seq1 */
+            {
+                if (start2 < ip+ml)
+                {
+                    int correction = (int)(ip+ml - start2);
+                    start2 += correction;
+                    ref2 += correction;
+                    ml2 -= correction;
+                    if (ml2 < MINMATCH)
+                    {
+                        start2 = start3;
+                        ref2 = ref3;
+                        ml2 = ml3;
+                    }
+                }
+
+                if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml, ref, limit, oend)) return 0;
+                ip  = start3;
+                ref = ref3;
+                ml  = ml3;
+
+                start0 = start2;
+                ref0 = ref2;
+                ml0 = ml2;
+                goto _Search2;
+            }
+
+            start2 = start3;
+            ref2 = ref3;
+            ml2 = ml3;
+            goto _Search3;
+        }
+
+        /*
+        * OK, now we have 3 ascending matches; let's write at least the first one
+        * ip & ref are known; Now for ml
+        */
+        if (start2 < ip+ml)
+        {
+            if ((start2 - ip) < (int)ML_MASK)
+            {
+                int correction;
+                if (ml > OPTIMAL_ML) ml = OPTIMAL_ML;
+                if (ip + ml > start2 + ml2 - MINMATCH) ml = (int)(start2 - ip) + ml2 - MINMATCH;
+                correction = ml - (int)(start2 - ip);
+                if (correction > 0)
+                {
+                    start2 += correction;
+                    ref2 += correction;
+                    ml2 -= correction;
+                }
+            }
+            else
+            {
+                ml = (int)(start2 - ip);
+            }
+        }
+        if (LZ4HC_encodeSequence(&ip, &op, &anchor, ml, ref, limit, oend)) return 0;
+
+        ip = start2;
+        ref = ref2;
+        ml = ml2;
+
+        start2 = start3;
+        ref2 = ref3;
+        ml2 = ml3;
+
+        goto _Search3;
+    }
+
+    /* Encode Last Literals */
+    {
+        int lastRun = (int)(iend - anchor);
+        if ((limit) && (((char*)op - dest) + lastRun + 1 + ((lastRun+255-RUN_MASK)/255) > (U32)maxOutputSize)) return 0;  /* Check output limit */
+        if (lastRun>=(int)RUN_MASK) { *op++=(RUN_MASK<<ML_BITS); lastRun-=RUN_MASK; for(; lastRun > 254 ; lastRun-=255) *op++ = 255; *op++ = (BYTE) lastRun; }
+        else *op++ = (BYTE)(lastRun<<ML_BITS);
+        memcpy(op, anchor, iend - anchor);
+        op += iend-anchor;
+    }
+
+    /* End */
+    return (int) (((char*)op)-dest);
+}
+
+
+int LZ4_sizeofStateHC(void) { return sizeof(LZ4HC_Data_Structure); }
+
+int LZ4_compress_HC_extStateHC (void* state, const char* src, char* dst, int srcSize, int maxDstSize, int compressionLevel)
+{
+    if (((size_t)(state)&(sizeof(void*)-1)) != 0) return 0;   /* Error : state is not aligned for pointers (32 or 64 bits) */
+    LZ4HC_init ((LZ4HC_Data_Structure*)state, (const BYTE*)src);
+    if (maxDstSize < LZ4_compressBound(srcSize))
+        return LZ4HC_compress_generic (state, src, dst, srcSize, maxDstSize, compressionLevel, limitedOutput);
+    else
+        return LZ4HC_compress_generic (state, src, dst, srcSize, maxDstSize, compressionLevel, noLimit);
+}
+
+int LZ4_compress_HC(const char* src, char* dst, int srcSize, int maxDstSize, int compressionLevel)
+{
+    LZ4HC_Data_Structure state;
+    return LZ4_compress_HC_extStateHC(&state, src, dst, srcSize, maxDstSize, compressionLevel);
+}
+
+
+
+/**************************************
+*  Streaming Functions
+**************************************/
+/* allocation */
+LZ4_streamHC_t* LZ4_createStreamHC(void) { return (LZ4_streamHC_t*)malloc(sizeof(LZ4_streamHC_t)); }
+int             LZ4_freeStreamHC (LZ4_streamHC_t* LZ4_streamHCPtr) { free(LZ4_streamHCPtr); return 0; }
+
+
+/* initialization */
+void LZ4_resetStreamHC (LZ4_streamHC_t* LZ4_streamHCPtr, int compressionLevel)
+{
+    LZ4_STATIC_ASSERT(sizeof(LZ4HC_Data_Structure) <= sizeof(LZ4_streamHC_t));   /* if compilation fails here, LZ4_STREAMHCSIZE must be increased */
+    ((LZ4HC_Data_Structure*)LZ4_streamHCPtr)->base = NULL;
+    ((LZ4HC_Data_Structure*)LZ4_streamHCPtr)->compressionLevel = (unsigned)compressionLevel;
+}
+
+int LZ4_loadDictHC (LZ4_streamHC_t* LZ4_streamHCPtr, const char* dictionary, int dictSize)
+{
+    LZ4HC_Data_Structure* ctxPtr = (LZ4HC_Data_Structure*) LZ4_streamHCPtr;
+    if (dictSize > 64 KB)
+    {
+        dictionary += dictSize - 64 KB;
+        dictSize = 64 KB;
+    }
+    LZ4HC_init (ctxPtr, (const BYTE*)dictionary);
+    if (dictSize >= 4) LZ4HC_Insert (ctxPtr, (const BYTE*)dictionary +(dictSize-3));
+    ctxPtr->end = (const BYTE*)dictionary + dictSize;
+    return dictSize;
+}
+
+
+/* compression */
+
+static void LZ4HC_setExternalDict(LZ4HC_Data_Structure* ctxPtr, const BYTE* newBlock)
+{
+    if (ctxPtr->end >= ctxPtr->base + 4)
+        LZ4HC_Insert (ctxPtr, ctxPtr->end-3);   /* Referencing remaining dictionary content */
+    /* Only one memory segment for extDict, so any previous extDict is lost at this stage */
+    ctxPtr->lowLimit  = ctxPtr->dictLimit;
+    ctxPtr->dictLimit = (U32)(ctxPtr->end - ctxPtr->base);
+    ctxPtr->dictBase  = ctxPtr->base;
+    ctxPtr->base = newBlock - ctxPtr->dictLimit;
+    ctxPtr->end  = newBlock;
+    ctxPtr->nextToUpdate = ctxPtr->dictLimit;   /* match referencing will resume from there */
+}
+
+static int LZ4_compressHC_continue_generic (LZ4HC_Data_Structure* ctxPtr,
+                                            const char* source, char* dest,
+                                            int inputSize, int maxOutputSize, limitedOutput_directive limit)
+{
+    /* auto-init if forgotten */
+    if (ctxPtr->base == NULL)
+        LZ4HC_init (ctxPtr, (const BYTE*) source);
+
+    /* Check overflow */
+    if ((size_t)(ctxPtr->end - ctxPtr->base) > 2 GB)
+    {
+        size_t dictSize = (size_t)(ctxPtr->end - ctxPtr->base) - ctxPtr->dictLimit;
+        if (dictSize > 64 KB) dictSize = 64 KB;
+
+        LZ4_loadDictHC((LZ4_streamHC_t*)ctxPtr, (const char*)(ctxPtr->end) - dictSize, (int)dictSize);
+    }
+
+    /* Check if blocks follow each other */
+    if ((const BYTE*)source != ctxPtr->end)
+        LZ4HC_setExternalDict(ctxPtr, (const BYTE*)source);
+
+    /* Check overlapping input/dictionary space */
+    {
+        const BYTE* sourceEnd = (const BYTE*) source + inputSize;
+        const BYTE* dictBegin = ctxPtr->dictBase + ctxPtr->lowLimit;
+        const BYTE* dictEnd   = ctxPtr->dictBase + ctxPtr->dictLimit;
+        if ((sourceEnd > dictBegin) && ((const BYTE*)source < dictEnd))
+        {
+            if (sourceEnd > dictEnd) sourceEnd = dictEnd;
+            ctxPtr->lowLimit = (U32)(sourceEnd - ctxPtr->dictBase);
+            if (ctxPtr->dictLimit - ctxPtr->lowLimit < 4) ctxPtr->lowLimit = ctxPtr->dictLimit;
+        }
+    }
+
+    return LZ4HC_compress_generic (ctxPtr, source, dest, inputSize, maxOutputSize, ctxPtr->compressionLevel, limit);
+}
+
+int LZ4_compress_HC_continue (LZ4_streamHC_t* LZ4_streamHCPtr, const char* source, char* dest, int inputSize, int maxOutputSize)
+{
+    if (maxOutputSize < LZ4_compressBound(inputSize))
+        return LZ4_compressHC_continue_generic ((LZ4HC_Data_Structure*)LZ4_streamHCPtr, source, dest, inputSize, maxOutputSize, limitedOutput);
+    else
+        return LZ4_compressHC_continue_generic ((LZ4HC_Data_Structure*)LZ4_streamHCPtr, source, dest, inputSize, maxOutputSize, noLimit);
+}
+
+
+/* dictionary saving */
+
+int LZ4_saveDictHC (LZ4_streamHC_t* LZ4_streamHCPtr, char* safeBuffer, int dictSize)
+{
+    LZ4HC_Data_Structure* streamPtr = (LZ4HC_Data_Structure*)LZ4_streamHCPtr;
+    int prefixSize = (int)(streamPtr->end - (streamPtr->base + streamPtr->dictLimit));
+    if (dictSize > 64 KB) dictSize = 64 KB;
+    if (dictSize < 4) dictSize = 0;
+    if (dictSize > prefixSize) dictSize = prefixSize;
+    memmove(safeBuffer, streamPtr->end - dictSize, dictSize);
+    {
+        U32 endIndex = (U32)(streamPtr->end - streamPtr->base);
+        streamPtr->end = (const BYTE*)safeBuffer + dictSize;
+        streamPtr->base = streamPtr->end - endIndex;
+        streamPtr->dictLimit = endIndex - dictSize;
+        streamPtr->lowLimit = endIndex - dictSize;
+        if (streamPtr->nextToUpdate < streamPtr->dictLimit) streamPtr->nextToUpdate = streamPtr->dictLimit;
+    }
+    return dictSize;
+}
+
+
+/***********************************
+*  Deprecated Functions
+***********************************/
+/* Deprecated compression functions */
+/* These functions are planned to start generate warnings by r131 approximately */
+int LZ4_compressHC(const char* src, char* dst, int srcSize) { return LZ4_compress_HC (src, dst, srcSize, LZ4_compressBound(srcSize), 0); }
+int LZ4_compressHC_limitedOutput(const char* src, char* dst, int srcSize, int maxDstSize) { return LZ4_compress_HC(src, dst, srcSize, maxDstSize, 0); }
+int LZ4_compressHC2(const char* src, char* dst, int srcSize, int cLevel) { return LZ4_compress_HC (src, dst, srcSize, LZ4_compressBound(srcSize), cLevel); }
+int LZ4_compressHC2_limitedOutput(const char* src, char* dst, int srcSize, int maxDstSize, int cLevel) { return LZ4_compress_HC(src, dst, srcSize, maxDstSize, cLevel); }
+int LZ4_compressHC_withStateHC (void* state, const char* src, char* dst, int srcSize) { return LZ4_compress_HC_extStateHC (state, src, dst, srcSize, LZ4_compressBound(srcSize), 0); }
+int LZ4_compressHC_limitedOutput_withStateHC (void* state, const char* src, char* dst, int srcSize, int maxDstSize) { return LZ4_compress_HC_extStateHC (state, src, dst, srcSize, maxDstSize, 0); }
+int LZ4_compressHC2_withStateHC (void* state, const char* src, char* dst, int srcSize, int cLevel) { return LZ4_compress_HC_extStateHC(state, src, dst, srcSize, LZ4_compressBound(srcSize), cLevel); }
+int LZ4_compressHC2_limitedOutput_withStateHC (void* state, const char* src, char* dst, int srcSize, int maxDstSize, int cLevel) { return LZ4_compress_HC_extStateHC(state, src, dst, srcSize, maxDstSize, cLevel); }
+int LZ4_compressHC_continue (LZ4_streamHC_t* ctx, const char* src, char* dst, int srcSize) { return LZ4_compress_HC_continue (ctx, src, dst, srcSize, LZ4_compressBound(srcSize)); }
+int LZ4_compressHC_limitedOutput_continue (LZ4_streamHC_t* ctx, const char* src, char* dst, int srcSize, int maxDstSize) { return LZ4_compress_HC_continue (ctx, src, dst, srcSize, maxDstSize); }
+
+
+/* Deprecated streaming functions */
+/* These functions currently generate deprecation warnings */
+int LZ4_sizeofStreamStateHC(void) { return LZ4_STREAMHCSIZE; }
+
+int LZ4_resetStreamStateHC(void* state, char* inputBuffer)
+{
+    if ((((size_t)state) & (sizeof(void*)-1)) != 0) return 1;   /* Error : pointer is not aligned for pointer (32 or 64 bits) */
+    LZ4HC_init((LZ4HC_Data_Structure*)state, (const BYTE*)inputBuffer);
+    ((LZ4HC_Data_Structure*)state)->inputBuffer = (BYTE*)inputBuffer;
+    return 0;
+}
+
+void* LZ4_createHC (char* inputBuffer)
+{
+    void* hc4 = ALLOCATOR(1, sizeof(LZ4HC_Data_Structure));
+    if (hc4 == NULL) return NULL;   /* not enough memory */
+    LZ4HC_init ((LZ4HC_Data_Structure*)hc4, (const BYTE*)inputBuffer);
+    ((LZ4HC_Data_Structure*)hc4)->inputBuffer = (BYTE*)inputBuffer;
+    return hc4;
+}
+
+int LZ4_freeHC (void* LZ4HC_Data)
+{
+    FREEMEM(LZ4HC_Data);
+    return (0);
+}
+
+int LZ4_compressHC2_continue (void* LZ4HC_Data, const char* source, char* dest, int inputSize, int compressionLevel)
+{
+    return LZ4HC_compress_generic (LZ4HC_Data, source, dest, inputSize, 0, compressionLevel, noLimit);
+}
+
+int LZ4_compressHC2_limitedOutput_continue (void* LZ4HC_Data, const char* source, char* dest, int inputSize, int maxOutputSize, int compressionLevel)
+{
+    return LZ4HC_compress_generic (LZ4HC_Data, source, dest, inputSize, maxOutputSize, compressionLevel, limitedOutput);
+}
+
+char* LZ4_slideInputBufferHC(void* LZ4HC_Data)
+{
+    LZ4HC_Data_Structure* hc4 = (LZ4HC_Data_Structure*)LZ4HC_Data;
+    int dictSize = LZ4_saveDictHC((LZ4_streamHC_t*)LZ4HC_Data, (char*)(hc4->inputBuffer), 64 KB);
+    return (char*)(hc4->inputBuffer + dictSize);
+}

corelib/src/flann/ext/lz4hc.h

@@ -0,0 +1,189 @@
+/*
+   LZ4 HC - High Compression Mode of LZ4
+   Header File
+   Copyright (C) 2011-2015, Yann Collet.
+   BSD 2-Clause License (http://www.opensource.org/licenses/bsd-license.php)
+
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are
+   met:
+
+       * Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+       * 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 COPYRIGHT HOLDERS AND CONTRIBUTORS
+   "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 COPYRIGHT
+   OWNER OR CONTRIBUTORS 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.
+
+   You can contact the author at :
+   - LZ4 source repository : https://github.com/Cyan4973/lz4
+   - LZ4 public forum : https://groups.google.com/forum/#!forum/lz4c
+*/
+#pragma once
+
+
+#if defined (__cplusplus)
+extern "C" {
+#endif
+
+/*****************************
+*  Includes
+*****************************/
+#include <stddef.h>   /* size_t */
+
+
+/**************************************
+*  Block Compression
+**************************************/
+int LZ4_compress_HC (const char* src, char* dst, int srcSize, int maxDstSize, int compressionLevel);
+/*
+LZ4_compress_HC :
+    Destination buffer 'dst' must be already allocated.
+    Compression completion is guaranteed if 'dst' buffer is sized to handle worst circumstances (data not compressible)
+    Worst size evaluation is provided by function LZ4_compressBound() (see "lz4.h")
+      srcSize  : Max supported value is LZ4_MAX_INPUT_SIZE (see "lz4.h")
+      compressionLevel : Recommended values are between 4 and 9, although any value between 0 and 16 will work.
+                         0 means "use default value" (see lz4hc.c).
+                         Values >16 behave the same as 16.
+      return : the number of bytes written into buffer 'dst'
+            or 0 if compression fails.
+*/
+
+
+/* Note :
+   Decompression functions are provided within LZ4 source code (see "lz4.h") (BSD license)
+*/
+
+
+int LZ4_sizeofStateHC(void);
+int LZ4_compress_HC_extStateHC(void* state, const char* src, char* dst, int srcSize, int maxDstSize, int compressionLevel);
+/*
+LZ4_compress_HC_extStateHC() :
+   Use this function if you prefer to manually allocate memory for compression tables.
+   To know how much memory must be allocated for the compression tables, use :
+      int LZ4_sizeofStateHC();
+
+   Allocated memory must be aligned on 8-bytes boundaries (which a normal malloc() will do properly).
+
+   The allocated memory can then be provided to the compression functions using 'void* state' parameter.
+   LZ4_compress_HC_extStateHC() is equivalent to previously described function.
+   It just uses externally allocated memory for stateHC.
+*/
+
+
+/**************************************
+*  Streaming Compression
+**************************************/
+#define LZ4_STREAMHCSIZE        262192
+#define LZ4_STREAMHCSIZE_SIZET (LZ4_STREAMHCSIZE / sizeof(size_t))
+typedef struct { size_t table[LZ4_STREAMHCSIZE_SIZET]; } LZ4_streamHC_t;
+/*
+  LZ4_streamHC_t
+  This structure allows static allocation of LZ4 HC streaming state.
+  State must then be initialized using LZ4_resetStreamHC() before first use.
+
+  Static allocation should only be used in combination with static linking.
+  If you want to use LZ4 as a DLL, please use construction functions below, which are future-proof.
+*/
+
+
+LZ4_streamHC_t* LZ4_createStreamHC(void);
+int             LZ4_freeStreamHC (LZ4_streamHC_t* streamHCPtr);
+/*
+  These functions create and release memory for LZ4 HC streaming state.
+  Newly created states are already initialized.
+  Existing state space can be re-used anytime using LZ4_resetStreamHC().
+  If you use LZ4 as a DLL, use these functions instead of static structure allocation,
+  to avoid size mismatch between different versions.
+*/
+
+void LZ4_resetStreamHC (LZ4_streamHC_t* streamHCPtr, int compressionLevel);
+int  LZ4_loadDictHC (LZ4_streamHC_t* streamHCPtr, const char* dictionary, int dictSize);
+
+int LZ4_compress_HC_continue (LZ4_streamHC_t* streamHCPtr, const char* src, char* dst, int srcSize, int maxDstSize);
+
+int LZ4_saveDictHC (LZ4_streamHC_t* streamHCPtr, char* safeBuffer, int maxDictSize);
+
+/*
+  These functions compress data in successive blocks of any size, using previous blocks as dictionary.
+  One key assumption is that previous blocks (up to 64 KB) remain read-accessible while compressing next blocks.
+  There is an exception for ring buffers, which can be smaller 64 KB.
+  Such case is automatically detected and correctly handled by LZ4_compress_HC_continue().
+
+  Before starting compression, state must be properly initialized, using LZ4_resetStreamHC().
+  A first "fictional block" can then be designated as initial dictionary, using LZ4_loadDictHC() (Optional).
+
+  Then, use LZ4_compress_HC_continue() to compress each successive block.
+  It works like LZ4_compress_HC(), but use previous memory blocks as dictionary to improve compression.
+  Previous memory blocks (including initial dictionary when present) must remain accessible and unmodified during compression.
+  As a reminder, size 'dst' buffer to handle worst cases, using LZ4_compressBound(), to ensure success of compression operation.
+
+  If, for any reason, previous data blocks can't be preserved unmodified in memory during next compression block,
+  you must save it to a safer memory space, using LZ4_saveDictHC().
+  Return value of LZ4_saveDictHC() is the size of dictionary effectively saved into 'safeBuffer'.
+*/
+
+
+
+/**************************************
+*  Deprecated Functions
+**************************************/
+/* Deprecate Warnings */
+/* Should these warnings messages be a problem,
+   it is generally possible to disable them,
+   with -Wno-deprecated-declarations for gcc
+   or _CRT_SECURE_NO_WARNINGS in Visual for example.
+   You can also define LZ4_DEPRECATE_WARNING_DEFBLOCK. */
+#ifndef LZ4_DEPRECATE_WARNING_DEFBLOCK
+#  define LZ4_DEPRECATE_WARNING_DEFBLOCK
+#  define LZ4_GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__)
+#  if (LZ4_GCC_VERSION >= 405) || defined(__clang__)
+#    define LZ4_DEPRECATED(message) __attribute__((deprecated(message)))
+#  elif (LZ4_GCC_VERSION >= 301)
+#    define LZ4_DEPRECATED(message) __attribute__((deprecated))
+#  elif defined(_MSC_VER)
+#    define LZ4_DEPRECATED(message) __declspec(deprecated(message))
+#  else
+#    pragma message("WARNING: You need to implement LZ4_DEPRECATED for this compiler")
+#    define LZ4_DEPRECATED(message)
+#  endif
+#endif // LZ4_DEPRECATE_WARNING_DEFBLOCK
+
+/* compression functions */
+/* these functions are planned to trigger warning messages by r131 approximately */
+int LZ4_compressHC                (const char* source, char* dest, int inputSize);
+int LZ4_compressHC_limitedOutput  (const char* source, char* dest, int inputSize, int maxOutputSize);
+int LZ4_compressHC2               (const char* source, char* dest, int inputSize, int compressionLevel);
+int LZ4_compressHC2_limitedOutput (const char* source, char* dest, int inputSize, int maxOutputSize, int compressionLevel);
+int LZ4_compressHC_withStateHC               (void* state, const char* source, char* dest, int inputSize);
+int LZ4_compressHC_limitedOutput_withStateHC (void* state, const char* source, char* dest, int inputSize, int maxOutputSize);
+int LZ4_compressHC2_withStateHC              (void* state, const char* source, char* dest, int inputSize, int compressionLevel);
+int LZ4_compressHC2_limitedOutput_withStateHC(void* state, const char* source, char* dest, int inputSize, int maxOutputSize, int compressionLevel);
+int LZ4_compressHC_continue               (LZ4_streamHC_t* LZ4_streamHCPtr, const char* source, char* dest, int inputSize);
+int LZ4_compressHC_limitedOutput_continue (LZ4_streamHC_t* LZ4_streamHCPtr, const char* source, char* dest, int inputSize, int maxOutputSize);
+
+/* Streaming functions following the older model; should no longer be used */
+LZ4_DEPRECATED("use LZ4_createStreamHC() instead") void* LZ4_createHC (char* inputBuffer);
+LZ4_DEPRECATED("use LZ4_saveDictHC() instead")     char* LZ4_slideInputBufferHC (void* LZ4HC_Data);
+LZ4_DEPRECATED("use LZ4_freeStreamHC() instead")   int   LZ4_freeHC (void* LZ4HC_Data);
+LZ4_DEPRECATED("use LZ4_compress_HC_continue() instead") int   LZ4_compressHC2_continue (void* LZ4HC_Data, const char* source, char* dest, int inputSize, int compressionLevel);
+LZ4_DEPRECATED("use LZ4_compress_HC_continue() instead") int   LZ4_compressHC2_limitedOutput_continue (void* LZ4HC_Data, const char* source, char* dest, int inputSize, int maxOutputSize, int compressionLevel);
+LZ4_DEPRECATED("use LZ4_createStreamHC() instead") int   LZ4_sizeofStreamStateHC(void);
+LZ4_DEPRECATED("use LZ4_resetStreamHC() instead")  int   LZ4_resetStreamStateHC(void* state, char* inputBuffer);
+
+
+#if defined (__cplusplus)
+}
+#endif

corelib/src/flann/flann.h

@@ -0,0 +1,609 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_H_
+#define FLANN_H_
+
+#include "defines.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+    using namespace flann;
+#endif
+
+
+struct FLANNParameters
+{
+    enum flann_algorithm_t algorithm; /* the algorithm to use */
+
+    /* search time parameters */
+    int checks;                /* how many leafs (features) to check in one search */
+    float eps;     /* eps parameter for eps-knn search */
+    int sorted;     /* indicates if results returned by radius search should be sorted or not */
+    int max_neighbors;  /* limits the maximum number of neighbors should be returned by radius search */
+    int cores;      /* number of paralel cores to use for searching */
+
+    /*  kdtree index parameters */
+    int trees;                 /* number of randomized trees to use (for kdtree) */
+    int leaf_max_size;
+
+    /* kmeans index parameters */
+    int branching;             /* branching factor (for kmeans tree) */
+    int iterations;            /* max iterations to perform in one kmeans cluetering (kmeans tree) */
+    enum flann_centers_init_t centers_init;  /* algorithm used for picking the initial cluster centers for kmeans tree */
+    float cb_index;            /* cluster boundary index. Used when searching the kmeans tree */
+
+    /* autotuned index parameters */
+    float target_precision;    /* precision desired (used for autotuning, -1 otherwise) */
+    float build_weight;        /* build tree time weighting factor */
+    float memory_weight;       /* index memory weigthing factor */
+    float sample_fraction;     /* what fraction of the dataset to use for autotuning */
+
+    /* LSH parameters */
+    unsigned int table_number_; /** The number of hash tables to use */
+    unsigned int key_size_;     /** The length of the key in the hash tables */
+    unsigned int multi_probe_level_; /** Number of levels to use in multi-probe LSH, 0 for standard LSH */
+
+    /* other parameters */
+    enum flann_log_level_t log_level;    /* determines the verbosity of each flann function */
+    long random_seed;            /* random seed to use */
+};
+
+
+typedef void* FLANN_INDEX; /* deprecated */
+typedef void* flann_index_t;
+
+FLANN_EXPORT extern struct FLANNParameters DEFAULT_FLANN_PARAMETERS;
+
+/**
+   Sets the log level used for all flann functions (unless
+   specified in FLANNParameters for each call
+
+   Params:
+    level = verbosity level
+ */
+FLANN_EXPORT void flann_log_verbosity(int level);
+
+
+/**
+ * Sets the distance type to use throughout FLANN.
+ * If distance type specified is MINKOWSKI, the second argument
+ * specifies which order the minkowski distance should have.
+ */
+FLANN_EXPORT void flann_set_distance_type(enum flann_distance_t distance_type, int order);
+
+/**
+ * Gets the distance type in use throughout FLANN.
+ */
+FLANN_EXPORT enum flann_distance_t flann_get_distance_type();
+
+/**
+ * Gets the distance order in use throughout FLANN (only applicable if minkowski distance
+ * is in use).
+ */
+FLANN_EXPORT int flann_get_distance_order();
+
+/**
+   Builds and returns an index. It uses autotuning if the target_precision field of index_params
+   is between 0 and 1, or the parameters specified if it's -1.
+
+   Params:
+    dataset = pointer to a data set stored in row major order
+    rows = number of rows (features) in the dataset
+    cols = number of columns in the dataset (feature dimensionality)
+    speedup = speedup over linear search, estimated if using autotuning, output parameter
+    index_params = index related parameters
+    flann_params = generic flann parameters
+
+   Returns: the newly created index or a number <0 for error
+ */
+FLANN_EXPORT flann_index_t flann_build_index(float* dataset,
+                                             int rows,
+                                             int cols,
+                                             float* speedup,
+                                             struct FLANNParameters* flann_params);
+
+FLANN_EXPORT flann_index_t flann_build_index_float(float* dataset,
+                                                   int rows,
+                                                   int cols,
+                                                   float* speedup,
+                                                   struct FLANNParameters* flann_params);
+
+FLANN_EXPORT flann_index_t flann_build_index_double(double* dataset,
+                                                    int rows,
+                                                    int cols,
+                                                    float* speedup,
+                                                    struct FLANNParameters* flann_params);
+
+FLANN_EXPORT flann_index_t flann_build_index_byte(unsigned char* dataset,
+                                                  int rows,
+                                                  int cols,
+                                                  float* speedup,
+                                                  struct FLANNParameters* flann_params);
+
+FLANN_EXPORT flann_index_t flann_build_index_int(int* dataset,
+                                                 int rows,
+                                                 int cols,
+                                                 float* speedup,
+                                                 struct FLANNParameters* flann_params);
+
+/**
+  Adds points to pre-built index.
+
+  Params:
+    index_ptr = pointer to index, must already be built
+    points = pointer to array of points
+    rows = number of points to add
+    columns = feature dimensionality
+    rebuild_threshold = reallocs index when it grows by factor of
+      `rebuild_threshold`. A smaller value results is more space efficient
+      but less computationally efficient. Must be greater than 1.
+
+  Returns: 0 if success otherwise -1
+**/
+FLANN_EXPORT int flann_add_points(flann_index_t index_ptr, float* points,
+                                  int rows, int columns,
+                                  float rebuild_threshold);
+
+FLANN_EXPORT int flann_add_points_float(flann_index_t index_ptr, float* points,
+                                        int rows, int columns,
+                                        float rebuild_threshold);
+
+FLANN_EXPORT int flann_add_points_double(flann_index_t index_ptr,
+                                         double* points, int rows, int columns,
+                                         float rebuild_threshold);
+
+FLANN_EXPORT int flann_add_points_byte(flann_index_t index_ptr,
+                                       unsigned char* points, int rows,
+                                       int columns, float rebuild_threshold);
+
+FLANN_EXPORT int flann_add_points_int(flann_index_t index_ptr, int* points,
+                                      int rows, int columns,
+                                      float rebuild_threshold);
+
+/**
+ * Removes a point from a pre-built index.
+ *
+ * index_ptr = pointer to pre-built index.
+ * point_id = index of datapoint to remove.
+*/
+FLANN_EXPORT int flann_remove_point(flann_index_t index_ptr,
+                                    unsigned int point_id);
+
+FLANN_EXPORT int flann_remove_point_float(flann_index_t index_ptr,
+                                          unsigned int point_id);
+
+FLANN_EXPORT int flann_remove_point_double(flann_index_t index_ptr,
+                                           unsigned int point_id);
+
+FLANN_EXPORT int flann_remove_point_byte(flann_index_t index_ptr,
+                                         unsigned int point_id);
+
+FLANN_EXPORT int flann_remove_point_int(flann_index_t index_ptr,
+                                        unsigned int point_id);
+
+/**
+ * Gets a point from a given index position.
+ *
+ * index_ptr = pointer to pre-built index.
+ * point_id = index of datapoint to get.
+ *
+ * Returns: pointer to datapoint or NULL on miss
+*/
+FLANN_EXPORT float* flann_get_point(flann_index_t index_ptr,
+                                    unsigned int point_id);
+
+FLANN_EXPORT float* flann_get_point_float(flann_index_t index_ptr,
+                                          unsigned int point_id);
+
+FLANN_EXPORT double* flann_get_point_double(flann_index_t index_ptr,
+                                            unsigned int point_id);
+
+FLANN_EXPORT unsigned char* flann_get_point_byte(flann_index_t index_ptr,
+                                                 unsigned int point_id);
+
+FLANN_EXPORT int* flann_get_point_int(flann_index_t index_ptr,
+                                      unsigned int point_id);
+
+/**
+ * Returns the number of datapoints stored in index.
+ *
+ * index_ptr = pointer to pre-built index.
+ *
+*/
+FLANN_EXPORT unsigned int flann_veclen(flann_index_t index_ptr);
+
+FLANN_EXPORT unsigned int flann_veclen_float(flann_index_t index_ptr);
+
+FLANN_EXPORT unsigned int flann_veclen_double(flann_index_t index_ptr);
+
+FLANN_EXPORT unsigned int flann_veclen_byte(flann_index_t index_ptr);
+
+FLANN_EXPORT unsigned int flann_veclen_int(flann_index_t index_ptr);
+
+/**
+ * Returns the dimensionality of datapoints stored in index.
+ *
+ * index_ptr = pointer to pre-built index.
+ *
+*/
+FLANN_EXPORT unsigned int flann_size(flann_index_t index_ptr);
+
+FLANN_EXPORT unsigned int flann_size_float(flann_index_t index_ptr);
+
+FLANN_EXPORT unsigned int flann_size_double(flann_index_t index_ptr);
+
+FLANN_EXPORT unsigned int flann_size_byte(flann_index_t index_ptr);
+
+FLANN_EXPORT unsigned int flann_size_int(flann_index_t index_ptr);
+
+/**
+ * Returns the number of bytes consumed by the index.
+ *
+ * index_ptr = pointer to pre-built index.
+ *
+*/
+FLANN_EXPORT int flann_used_memory(flann_index_t index_ptr);
+
+FLANN_EXPORT int flann_used_memory_float(flann_index_t index_ptr);
+
+FLANN_EXPORT int flann_used_memory_double(flann_index_t index_ptr);
+
+FLANN_EXPORT int flann_used_memory_byte(flann_index_t index_ptr);
+
+FLANN_EXPORT int flann_used_memory_int(flann_index_t index_ptr);
+
+
+/**
+ * Saves the index to a file. Only the index is saved into the file, the dataset corresponding to the index is not saved.
+ *
+ * @param index_id The index that should be saved
+ * @param filename The filename the index should be saved to
+ * @return Returns 0 on success, negative value on error.
+ */
+FLANN_EXPORT int flann_save_index(flann_index_t index_id,
+                                  char* filename);
+
+FLANN_EXPORT int flann_save_index_float(flann_index_t index_id,
+                                        char* filename);
+
+FLANN_EXPORT int flann_save_index_double(flann_index_t index_id,
+                                         char* filename);
+
+FLANN_EXPORT int flann_save_index_byte(flann_index_t index_id,
+                                       char* filename);
+
+FLANN_EXPORT int flann_save_index_int(flann_index_t index_id,
+                                      char* filename);
+
+/**
+ * Loads an index from a file.
+ *
+ * @param filename File to load the index from.
+ * @param dataset The dataset corresponding to the index.
+ * @param rows Dataset tors
+ * @param cols Dataset columns
+ * @return
+ */
+FLANN_EXPORT flann_index_t flann_load_index(char* filename,
+                                            float* dataset,
+                                            int rows,
+                                            int cols);
+
+FLANN_EXPORT flann_index_t flann_load_index_float(char* filename,
+                                                  float* dataset,
+                                                  int rows,
+                                                  int cols);
+
+FLANN_EXPORT flann_index_t flann_load_index_double(char* filename,
+                                                   double* dataset,
+                                                   int rows,
+                                                   int cols);
+
+FLANN_EXPORT flann_index_t flann_load_index_byte(char* filename,
+                                                 unsigned char* dataset,
+                                                 int rows,
+                                                 int cols);
+
+FLANN_EXPORT flann_index_t flann_load_index_int(char* filename,
+                                                int* dataset,
+                                                int rows,
+                                                int cols);
+
+
+/**
+   Builds an index and uses it to find nearest neighbors.
+
+   Params:
+    dataset = pointer to a data set stored in row major order
+    rows = number of rows (features) in the dataset
+    cols = number of columns in the dataset (feature dimensionality)
+    testset = pointer to a query set stored in row major order
+    trows = number of rows (features) in the query dataset (same dimensionality as features in the dataset)
+    indices = pointer to matrix for the indices of the nearest neighbors of the testset features in the dataset
+            (must have trows number of rows and nn number of columns)
+    nn = how many nearest neighbors to return
+    flann_params = generic flann parameters
+
+   Returns: zero or -1 for error
+ */
+FLANN_EXPORT int flann_find_nearest_neighbors(float* dataset,
+                                              int rows,
+                                              int cols,
+                                              float* testset,
+                                              int trows,
+                                              int* indices,
+                                              float* dists,
+                                              int nn,
+                                              struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_find_nearest_neighbors_float(float* dataset,
+                                                    int rows,
+                                                    int cols,
+                                                    float* testset,
+                                                    int trows,
+                                                    int* indices,
+                                                    float* dists,
+                                                    int nn,
+                                                    struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_find_nearest_neighbors_double(double* dataset,
+                                                     int rows,
+                                                     int cols,
+                                                     double* testset,
+                                                     int trows,
+                                                     int* indices,
+                                                     double* dists,
+                                                     int nn,
+                                                     struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_find_nearest_neighbors_byte(unsigned char* dataset,
+                                                   int rows,
+                                                   int cols,
+                                                   unsigned char* testset,
+                                                   int trows,
+                                                   int* indices,
+                                                   float* dists,
+                                                   int nn,
+                                                   struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_find_nearest_neighbors_int(int* dataset,
+                                                  int rows,
+                                                  int cols,
+                                                  int* testset,
+                                                  int trows,
+                                                  int* indices,
+                                                  float* dists,
+                                                  int nn,
+                                                  struct FLANNParameters* flann_params);
+
+
+/**
+   Searches for nearest neighbors using the index provided
+
+   Params:
+    index_id = the index (constructed previously using flann_build_index).
+    testset = pointer to a query set stored in row major order
+    trows = number of rows (features) in the query dataset (same dimensionality as features in the dataset)
+    indices = pointer to matrix for the indices of the nearest neighbors of the testset features in the dataset
+            (must have trows number of rows and nn number of columns)
+    dists = pointer to matrix for the distances of the nearest neighbors of the testset features in the dataset
+            (must have trows number of rows and 1 column)
+    nn = how many nearest neighbors to return
+    flann_params = generic flann parameters
+
+   Returns: zero or a number <0 for error
+ */
+FLANN_EXPORT int flann_find_nearest_neighbors_index(flann_index_t index_id,
+                                                    float* testset,
+                                                    int trows,
+                                                    int* indices,
+                                                    float* dists,
+                                                    int nn,
+                                                    struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_find_nearest_neighbors_index_float(flann_index_t index_id,
+                                                          float* testset,
+                                                          int trows,
+                                                          int* indices,
+                                                          float* dists,
+                                                          int nn,
+                                                          struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_find_nearest_neighbors_index_double(flann_index_t index_id,
+                                                           double* testset,
+                                                           int trows,
+                                                           int* indices,
+                                                           double* dists,
+                                                           int nn,
+                                                           struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_find_nearest_neighbors_index_byte(flann_index_t index_id,
+                                                         unsigned char* testset,
+                                                         int trows,
+                                                         int* indices,
+                                                         float* dists,
+                                                         int nn,
+                                                         struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_find_nearest_neighbors_index_int(flann_index_t index_id,
+                                                        int* testset,
+                                                        int trows,
+                                                        int* indices,
+                                                        float* dists,
+                                                        int nn,
+                                                        struct FLANNParameters* flann_params);
+
+
+/**
+ * Performs an radius search using an already constructed index.
+ *
+ * In case of radius search, instead of always returning a predetermined
+ * number of nearest neighbours (for example the 10 nearest neighbours), the
+ * search will return all the neighbours found within a search radius
+ * of the query point.
+ *
+ * The check parameter in the FLANNParameters below sets the level of approximation
+ * for the search by only visiting "checks" number of features in the index
+ * (the same way as for the KNN search). A lower value for checks will give
+ * a higher search speedup at the cost of potentially not returning all the
+ * neighbours in the specified radius.
+ */
+FLANN_EXPORT int flann_radius_search(flann_index_t index_ptr, /* the index */
+                                     float* query, /* query point */
+                                     int* indices, /* array for storing the indices found (will be modified) */
+                                     float* dists, /* similar, but for storing distances */
+                                     int max_nn,  /* size of arrays indices and dists */
+                                     float radius, /* search radius (squared radius for euclidian metric) */
+                                     struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_radius_search_float(flann_index_t index_ptr, /* the index */
+                                           float* query, /* query point */
+                                           int* indices, /* array for storing the indices found (will be modified) */
+                                           float* dists, /* similar, but for storing distances */
+                                           int max_nn,  /* size of arrays indices and dists */
+                                           float radius, /* search radius (squared radius for euclidian metric) */
+                                           struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_radius_search_double(flann_index_t index_ptr, /* the index */
+                                            double* query, /* query point */
+                                            int* indices, /* array for storing the indices found (will be modified) */
+                                            double* dists, /* similar, but for storing distances */
+                                            int max_nn,  /* size of arrays indices and dists */
+                                            float radius, /* search radius (squared radius for euclidian metric) */
+                                            struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_radius_search_byte(flann_index_t index_ptr, /* the index */
+                                          unsigned char* query, /* query point */
+                                          int* indices, /* array for storing the indices found (will be modified) */
+                                          float* dists, /* similar, but for storing distances */
+                                          int max_nn,  /* size of arrays indices and dists */
+                                          float radius, /* search radius (squared radius for euclidian metric) */
+                                          struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_radius_search_int(flann_index_t index_ptr, /* the index */
+                                         int* query, /* query point */
+                                         int* indices, /* array for storing the indices found (will be modified) */
+                                         float* dists, /* similar, but for storing distances */
+                                         int max_nn,  /* size of arrays indices and dists */
+                                         float radius, /* search radius (squared radius for euclidian metric) */
+                                         struct FLANNParameters* flann_params);
+
+/**
+   Deletes an index and releases the memory used by it.
+
+   Params:
+    index_id = the index (constructed previously using flann_build_index).
+    flann_params = generic flann parameters
+
+   Returns: zero or a number <0 for error
+ */
+FLANN_EXPORT int flann_free_index(flann_index_t index_id,
+                                  struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_free_index_float(flann_index_t index_id,
+                                        struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_free_index_double(flann_index_t index_id,
+                                         struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_free_index_byte(flann_index_t index_id,
+                                       struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_free_index_int(flann_index_t index_id,
+                                      struct FLANNParameters* flann_params);
+
+/**
+   Clusters the features in the dataset using a hierarchical kmeans clustering approach.
+   This is significantly faster than using a flat kmeans clustering for a large number
+   of clusters.
+
+   Params:
+    dataset = pointer to a data set stored in row major order
+    rows = number of rows (features) in the dataset
+    cols = number of columns in the dataset (feature dimensionality)
+    clusters = number of cluster to compute
+    result = memory buffer where the output cluster centers are storred
+    index_params = used to specify the kmeans tree parameters (branching factor, max number of iterations to use)
+    flann_params = generic flann parameters
+
+   Returns: number of clusters computed or a number <0 for error. This number can be different than the number of clusters requested, due to the
+    way hierarchical clusters are computed. The number of clusters returned will be the highest number of the form
+    (branch_size-1)*K+1 smaller than the number of clusters requested.
+ */
+
+FLANN_EXPORT int flann_compute_cluster_centers(float* dataset,
+                                               int rows,
+                                               int cols,
+                                               int clusters,
+                                               float* result,
+                                               struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_compute_cluster_centers_float(float* dataset,
+                                                     int rows,
+                                                     int cols,
+                                                     int clusters,
+                                                     float* result,
+                                                     struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_compute_cluster_centers_double(double* dataset,
+                                                      int rows,
+                                                      int cols,
+                                                      int clusters,
+                                                      double* result,
+                                                      struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_compute_cluster_centers_byte(unsigned char* dataset,
+                                                    int rows,
+                                                    int cols,
+                                                    int clusters,
+                                                    float* result,
+                                                    struct FLANNParameters* flann_params);
+
+FLANN_EXPORT int flann_compute_cluster_centers_int(int* dataset,
+                                                   int rows,
+                                                   int cols,
+                                                   int clusters,
+                                                   float* result,
+                                                   struct FLANNParameters* flann_params);
+
+
+#ifdef __cplusplus
+}
+
+
+#include "flann.hpp"
+
+#endif
+
+
+#endif /*FLANN_H_*/
+

corelib/src/flann/flann.hpp

@@ -0,0 +1,435 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_HPP_
+#define FLANN_HPP_
+
+
+#include <vector>
+#include <string>
+#include <cassert>
+#include <cstdio>
+
+#include "flann/general.h"
+#include "flann/util/matrix.h"
+#include "flann/util/params.h"
+#include "flann/util/saving.h"
+
+#include "flann/algorithms/all_indices.h"
+
+namespace flann
+{
+
+/**
+ * Sets the log level used for all flann functions
+ * @param level Verbosity level
+ */
+inline void log_verbosity(int level)
+{
+    if (level >= 0) {
+        Logger::setLevel(level);
+    }
+}
+
+/**
+ * (Deprecated) Index parameters for creating a saved index.
+ */
+struct SavedIndexParams : public IndexParams
+{
+    SavedIndexParams(std::string filename)
+    {
+        (*this)["algorithm"] = FLANN_INDEX_SAVED;
+        (*this)["filename"] = filename;
+    }
+};
+
+
+
+template<typename Distance>
+class Index
+{
+public:
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+    typedef NNIndex<Distance> IndexType;
+
+    Index(const IndexParams& params, Distance distance = Distance() )
+        : index_params_(params)
+    {
+        flann_algorithm_t index_type = get_param<flann_algorithm_t>(params,"algorithm");
+        loaded_ = false;
+
+        Matrix<ElementType> features;
+        if (index_type == FLANN_INDEX_SAVED) {
+            nnIndex_ = load_saved_index(features, get_param<std::string>(params,"filename"), distance);
+            loaded_ = true;
+        }
+        else {
+        	flann_algorithm_t index_type = get_param<flann_algorithm_t>(params, "algorithm");
+            nnIndex_ = create_index_by_type<Distance>(index_type, features, params, distance);
+        }
+    }
+
+
+    Index(const Matrix<ElementType>& features, const IndexParams& params, Distance distance = Distance() )
+        : index_params_(params)
+    {
+        flann_algorithm_t index_type = get_param<flann_algorithm_t>(params,"algorithm");
+        loaded_ = false;
+
+        if (index_type == FLANN_INDEX_SAVED) {
+            nnIndex_ = load_saved_index(features, get_param<std::string>(params,"filename"), distance);
+            loaded_ = true;
+        }
+        else {
+        	flann_algorithm_t index_type = get_param<flann_algorithm_t>(params, "algorithm");
+            nnIndex_ = create_index_by_type<Distance>(index_type, features, params, distance);
+        }
+    }
+
+
+    Index(const Index& other) : loaded_(other.loaded_), index_params_(other.index_params_)
+    {
+    	nnIndex_ = other.nnIndex_->clone();
+    }
+
+    Index& operator=(Index other)
+    {
+    	this->swap(other);
+    	return *this;
+    }
+
+    virtual ~Index()
+    {
+        delete nnIndex_;
+    }
+
+    /**
+     * Builds the index.
+     */
+    void buildIndex()
+    {
+        if (!loaded_) {
+            nnIndex_->buildIndex();
+        }
+    }
+
+    void buildIndex(const Matrix<ElementType>& points)
+    {
+    	nnIndex_->buildIndex(points);
+    }
+
+    void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
+    {
+        nnIndex_->addPoints(points, rebuild_threshold);
+    }
+
+    /**
+     * Remove point from the index
+     * @param index Index of point to be removed
+     */
+    void removePoint(size_t point_id)
+    {
+    	nnIndex_->removePoint(point_id);
+    }
+
+    /**
+     * Returns pointer to a data point with the specified id.
+     * @param point_id the id of point to retrieve
+     * @return
+     */
+    ElementType* getPoint(size_t point_id)
+    {
+    	return nnIndex_->getPoint(point_id);
+    }
+
+    /**
+     * Save index to file
+     * @param filename
+     */
+    void save(std::string filename)
+    {
+        FILE* fout = fopen(filename.c_str(), "wb");
+        if (fout == NULL) {
+            throw FLANNException("Cannot open file");
+        }
+        nnIndex_->saveIndex(fout);
+        fclose(fout);
+    }
+
+    /**
+     * \returns number of features in this index.
+     */
+    size_t veclen() const
+    {
+        return nnIndex_->veclen();
+    }
+
+    /**
+     * \returns The dimensionality of the features in this index.
+     */
+    size_t size() const
+    {
+        return nnIndex_->size();
+    }
+
+    /**
+     * \returns The index type (kdtree, kmeans,...)
+     */
+    flann_algorithm_t getType() const
+    {
+        return nnIndex_->getType();
+    }
+
+    /**
+     * \returns The amount of memory (in bytes) used by the index.
+     */
+    int usedMemory() const
+    {
+        return nnIndex_->usedMemory();
+    }
+
+
+    /**
+     * \returns The index parameters
+     */
+    IndexParams getParameters() const
+    {
+        return nnIndex_->getParameters();
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 Matrix<size_t>& indices,
+                                 Matrix<DistanceType>& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	return nnIndex_->knnSearch(queries, indices, dists, knn, params);
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param knn
+     * @param params
+     * @return
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 Matrix<int>& indices,
+                                 Matrix<DistanceType>& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	return nnIndex_->knnSearch(queries, indices, dists, knn, params);
+    }
+
+    /**
+     * \brief Perform k-nearest neighbor search
+     * \param[in] queries The query points for which to find the nearest neighbors
+     * \param[out] indices The indices of the nearest neighbors found
+     * \param[out] dists Distances to the nearest neighbors found
+     * \param[in] knn Number of nearest neighbors to return
+     * \param[in] params Search parameters
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 std::vector< std::vector<size_t> >& indices,
+                                 std::vector<std::vector<DistanceType> >& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	return nnIndex_->knnSearch(queries, indices, dists, knn, params);
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param knn
+     * @param params
+     * @return
+     */
+    int knnSearch(const Matrix<ElementType>& queries,
+                                 std::vector< std::vector<int> >& indices,
+                                 std::vector<std::vector<DistanceType> >& dists,
+                                 size_t knn,
+                           const SearchParams& params) const
+    {
+    	return nnIndex_->knnSearch(queries, indices, dists, knn, params);
+    }
+
+    /**
+     * \brief Perform radius search
+     * \param[in] queries The query points
+     * \param[out] indices The indices of the neighbors found within the given radius
+     * \param[out] dists The distances to the nearest neighbors found
+     * \param[in] radius The radius used for search
+     * \param[in] params Search parameters
+     * \returns Number of neighbors found
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    Matrix<size_t>& indices,
+                                    Matrix<DistanceType>& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param radius
+     * @param params
+     * @return
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    Matrix<int>& indices,
+                                    Matrix<DistanceType>& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
+    }
+
+    /**
+     * \brief Perform radius search
+     * \param[in] queries The query points
+     * \param[out] indices The indices of the neighbors found within the given radius
+     * \param[out] dists The distances to the nearest neighbors found
+     * \param[in] radius The radius used for search
+     * \param[in] params Search parameters
+     * \returns Number of neighbors found
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    std::vector< std::vector<size_t> >& indices,
+                                    std::vector<std::vector<DistanceType> >& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
+    }
+
+    /**
+     *
+     * @param queries
+     * @param indices
+     * @param dists
+     * @param radius
+     * @param params
+     * @return
+     */
+    int radiusSearch(const Matrix<ElementType>& queries,
+                                    std::vector< std::vector<int> >& indices,
+                                    std::vector<std::vector<DistanceType> >& dists,
+                                    float radius,
+                              const SearchParams& params) const
+    {
+    	return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
+    }
+
+private:
+    IndexType* load_saved_index(const Matrix<ElementType>& dataset, const std::string& filename, Distance distance)
+    {
+        FILE* fin = fopen(filename.c_str(), "rb");
+        if (fin == NULL) {
+            return NULL;
+        }
+        IndexHeader header = load_header(fin);
+        if (header.h.data_type != flann_datatype_value<ElementType>::value) {
+            throw FLANNException("Datatype of saved index is different than of the one to be loaded.");
+        }
+
+        IndexParams params;
+        params["algorithm"] = header.h.index_type;
+        IndexType* nnIndex = create_index_by_type<Distance>(header.h.index_type, dataset, params, distance);
+        rewind(fin);
+        nnIndex->loadIndex(fin);
+        fclose(fin);
+
+        return nnIndex;
+    }
+
+    void swap( Index& other)
+    {
+    	std::swap(nnIndex_, other.nnIndex_);
+    	std::swap(loaded_, other.loaded_);
+    	std::swap(index_params_, other.index_params_);
+    }
+
+private:
+    /** Pointer to actual index class */
+    IndexType* nnIndex_;
+    /** Indices if the index was loaded from a file */
+    bool loaded_;
+    /** Parameters passed to the index */
+    IndexParams index_params_;
+};
+
+
+
+
+
+/**
+ * Performs a hierarchical clustering of the points passed as argument and then takes a cut in the
+ * the clustering tree to return a flat clustering.
+ * @param[in] points Points to be clustered
+ * @param centers The computed cluster centres. Matrix should be preallocated and centers.rows is the
+ *  number of clusters requested.
+ * @param params Clustering parameters (The same as for flann::KMeansIndex)
+ * @param d Distance to be used for clustering (eg: flann::L2)
+ * @return number of clusters computed (can be different than clusters.rows and is the highest number
+ * of the form (branching-1)*K+1 smaller than clusters.rows).
+ */
+template <typename Distance>
+int hierarchicalClustering(const Matrix<typename Distance::ElementType>& points, Matrix<typename Distance::ResultType>& centers,
+                           const KMeansIndexParams& params, Distance d = Distance())
+{
+    KMeansIndex<Distance> kmeans(points, params, d);
+    kmeans.buildIndex();
+
+    int clusterNum = kmeans.getClusterCenters(centers);
+    return clusterNum;
+}
+
+}
+#endif /* FLANN_HPP_ */

corelib/src/flann/flann_cpp.cpp

@@ -0,0 +1,30 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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.
+ *************************************************************************/
+
+
+#include "flann/flann.hpp"

corelib/src/flann/general.h

@@ -0,0 +1,227 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_GENERAL_H_
+#define FLANN_GENERAL_H_
+
+#include "defines.h"
+#include <stdexcept>
+#include <cassert>
+#include <limits.h>
+
+namespace flann
+{
+
+class FLANNException : public std::runtime_error
+{
+public:
+    FLANNException(const char* message) : std::runtime_error(message) { }
+
+    FLANNException(const std::string& message) : std::runtime_error(message) { }
+};
+
+
+template <typename T>
+struct flann_datatype_value
+{
+	static const flann_datatype_t value = FLANN_NONE;
+};
+
+template<>
+struct flann_datatype_value<char>
+{
+	static const flann_datatype_t value = FLANN_INT8;
+};
+
+template<>
+struct flann_datatype_value<short>
+{
+	static const flann_datatype_t value = FLANN_INT16;
+};
+
+template<>
+struct flann_datatype_value<int>
+{
+	static const flann_datatype_t value = FLANN_INT32;
+};
+
+#ifdef LLONG_MAX
+template<>
+struct flann_datatype_value<long long>
+{
+	static const flann_datatype_t value = FLANN_INT64;
+};
+#endif
+
+template<>
+struct flann_datatype_value<unsigned char>
+{
+	static const flann_datatype_t value = FLANN_UINT8;
+};
+
+template<>
+struct flann_datatype_value<unsigned short>
+{
+	static const flann_datatype_t value = FLANN_UINT16;
+};
+
+template<>
+struct flann_datatype_value<unsigned int>
+{
+	static const flann_datatype_t value = FLANN_UINT32;
+};
+
+#ifdef ULLONG_MAX
+template<>
+struct flann_datatype_value<unsigned long long>
+{
+	static const flann_datatype_t value = FLANN_UINT64;
+};
+#endif
+
+
+template<>
+struct flann_datatype_value<float>
+{
+	static const flann_datatype_t value = FLANN_FLOAT32;
+};
+
+template<>
+struct flann_datatype_value<double>
+{
+	static const flann_datatype_t value = FLANN_FLOAT64;
+};
+
+
+
+template <flann_datatype_t datatype>
+struct flann_datatype_type
+{
+	typedef void type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_INT8>
+{
+	typedef char type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_INT16>
+{
+	typedef short type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_INT32>
+{
+	typedef int type;
+};
+
+#ifdef LLONG_MAX
+template<>
+struct flann_datatype_type<FLANN_INT64>
+{
+	typedef long long type;
+};
+#endif
+
+template<>
+struct flann_datatype_type<FLANN_UINT8>
+{
+	typedef unsigned char type;
+};
+
+
+template<>
+struct flann_datatype_type<FLANN_UINT16>
+{
+	typedef unsigned short type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_UINT32>
+{
+	typedef unsigned int type;
+};
+
+#ifdef ULLONG_MAX
+template<>
+struct flann_datatype_type<FLANN_UINT64>
+{
+	typedef unsigned long long type;
+};
+#endif
+
+template<>
+struct flann_datatype_type<FLANN_FLOAT32>
+{
+	typedef float type;
+};
+
+template<>
+struct flann_datatype_type<FLANN_FLOAT64>
+{
+	typedef double type;
+};
+
+
+inline size_t flann_datatype_size(flann_datatype_t type)
+{
+	switch (type) {
+	case FLANN_INT8:
+		return sizeof(flann_datatype_type<FLANN_INT8>::type);
+	case FLANN_INT16:
+		return sizeof(flann_datatype_type<FLANN_INT16>::type);
+	case FLANN_INT32:
+		return sizeof(flann_datatype_type<FLANN_INT32>::type);
+	case FLANN_INT64:
+		return sizeof(flann_datatype_type<FLANN_INT64>::type);
+	case FLANN_UINT8:
+		return sizeof(flann_datatype_type<FLANN_UINT8>::type);
+	case FLANN_UINT16:
+		return sizeof(flann_datatype_type<FLANN_UINT16>::type);
+	case FLANN_UINT32:
+		return sizeof(flann_datatype_type<FLANN_UINT32>::type);
+	case FLANN_UINT64:
+		return sizeof(flann_datatype_type<FLANN_UINT64>::type);
+	case FLANN_FLOAT32:
+		return sizeof(flann_datatype_type<FLANN_FLOAT32>::type);
+	case FLANN_FLOAT64:
+		return sizeof(flann_datatype_type<FLANN_FLOAT64>::type);
+	default:
+		return 0;
+	}
+}
+
+}
+
+
+#endif  /* FLANN_GENERAL_H_ */

corelib/src/flann/io/hdf5.h

@@ -0,0 +1,231 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_HDF5_H_
+#define FLANN_HDF5_H_
+
+#include <hdf5.h>
+
+#include "flann/util/matrix.h"
+
+
+namespace flann
+{
+
+namespace
+{
+
+template<typename T>
+hid_t get_hdf5_type()
+{
+    throw FLANNException("Unsupported type for IO operations");
+}
+
+template<>
+hid_t get_hdf5_type<char>() { return H5T_NATIVE_CHAR; }
+template<>
+hid_t get_hdf5_type<unsigned char>() { return H5T_NATIVE_UCHAR; }
+template<>
+hid_t get_hdf5_type<short int>() { return H5T_NATIVE_SHORT; }
+template<>
+hid_t get_hdf5_type<unsigned short int>() { return H5T_NATIVE_USHORT; }
+template<>
+hid_t get_hdf5_type<int>() { return H5T_NATIVE_INT; }
+template<>
+hid_t get_hdf5_type<unsigned int>() { return H5T_NATIVE_UINT; }
+template<>
+hid_t get_hdf5_type<long>() { return H5T_NATIVE_LONG; }
+template<>
+hid_t get_hdf5_type<unsigned long>() { return H5T_NATIVE_ULONG; }
+template<>
+hid_t get_hdf5_type<float>() { return H5T_NATIVE_FLOAT; }
+template<>
+hid_t get_hdf5_type<double>() { return H5T_NATIVE_DOUBLE; }
+template<>
+hid_t get_hdf5_type<long double>() { return H5T_NATIVE_LDOUBLE; }
+}
+
+
+#define CHECK_ERROR(x,y) if ((x)<0) throw FLANNException((y));
+
+template<typename T>
+void save_to_file(const flann::Matrix<T>& dataset, const std::string& filename, const std::string& name)
+{
+
+#if H5Eset_auto_vers == 2
+    H5Eset_auto( H5E_DEFAULT, NULL, NULL );
+#else
+    H5Eset_auto( NULL, NULL );
+#endif
+
+    herr_t status;
+    hid_t file_id;
+    file_id = H5Fopen(filename.c_str(), H5F_ACC_RDWR, H5P_DEFAULT);
+    if (file_id < 0) {
+        file_id = H5Fcreate(filename.c_str(), H5F_ACC_EXCL, H5P_DEFAULT, H5P_DEFAULT);
+    }
+    CHECK_ERROR(file_id,"Error creating hdf5 file.");
+
+    hsize_t     dimsf[2];              // dataset dimensions
+    dimsf[0] = dataset.rows;
+    dimsf[1] = dataset.cols;
+
+    hid_t space_id = H5Screate_simple(2, dimsf, NULL);
+    hid_t memspace_id = H5Screate_simple(2, dimsf, NULL);
+
+    hid_t dataset_id;
+#if H5Dcreate_vers == 2
+    dataset_id = H5Dcreate2(file_id, name.c_str(), get_hdf5_type<T>(), space_id, H5P_DEFAULT, H5P_DEFAULT, H5P_DEFAULT);
+#else
+    dataset_id = H5Dcreate(file_id, name.c_str(), get_hdf5_type<T>(), space_id, H5P_DEFAULT);
+#endif
+
+    if (dataset_id<0) {
+#if H5Dopen_vers == 2
+        dataset_id = H5Dopen2(file_id, name.c_str(), H5P_DEFAULT);
+#else
+        dataset_id = H5Dopen(file_id, name.c_str());
+#endif
+    }
+    CHECK_ERROR(dataset_id,"Error creating or opening dataset in file.");
+
+    status = H5Dwrite(dataset_id, get_hdf5_type<T>(), memspace_id, space_id, H5P_DEFAULT, dataset.ptr() );
+    CHECK_ERROR(status, "Error writing to dataset");
+
+    H5Sclose(memspace_id);
+    H5Sclose(space_id);
+    H5Dclose(dataset_id);
+    H5Fclose(file_id);
+
+}
+
+
+template<typename T>
+void load_from_file(flann::Matrix<T>& dataset, const std::string& filename, const std::string& name)
+{
+    herr_t status;
+    hid_t file_id = H5Fopen(filename.c_str(), H5F_ACC_RDWR, H5P_DEFAULT);
+    CHECK_ERROR(file_id,"Error opening hdf5 file.");
+
+    hid_t dataset_id;
+#if H5Dopen_vers == 2
+    dataset_id = H5Dopen2(file_id, name.c_str(), H5P_DEFAULT);
+#else
+    dataset_id = H5Dopen(file_id, name.c_str());
+#endif
+    CHECK_ERROR(dataset_id,"Error opening dataset in file.");
+
+    hid_t space_id = H5Dget_space(dataset_id);
+
+    hsize_t dims_out[2];
+    H5Sget_simple_extent_dims(space_id, dims_out, NULL);
+
+    dataset = flann::Matrix<T>(new T[dims_out[0]*dims_out[1]], dims_out[0], dims_out[1]);
+
+    status = H5Dread(dataset_id, get_hdf5_type<T>(), H5S_ALL, H5S_ALL, H5P_DEFAULT, dataset[0]);
+    CHECK_ERROR(status, "Error reading dataset");
+
+    H5Sclose(space_id);
+    H5Dclose(dataset_id);
+    H5Fclose(file_id);
+}
+
+
+#ifdef HAVE_MPI
+
+namespace mpi
+{
+/**
+ * Loads a the hyperslice corresponding to this processor from a hdf5 file.
+ * @param flann_dataset Dataset where the data is loaded
+ * @param filename HDF5 file name
+ * @param name Name of dataset inside file
+ */
+template<typename T>
+void load_from_file(flann::Matrix<T>& dataset, const std::string& filename, const std::string& name)
+{
+    MPI_Comm comm  = MPI_COMM_WORLD;
+    MPI_Info info  = MPI_INFO_NULL;
+
+    int mpi_size, mpi_rank;
+    MPI_Comm_size(comm, &mpi_size);
+    MPI_Comm_rank(comm, &mpi_rank);
+
+    herr_t status;
+
+    hid_t plist_id = H5Pcreate(H5P_FILE_ACCESS);
+    H5Pset_fapl_mpio(plist_id, comm, info);
+    hid_t file_id = H5Fopen(filename.c_str(), H5F_ACC_RDWR, plist_id);
+    CHECK_ERROR(file_id,"Error opening hdf5 file.");
+    H5Pclose(plist_id);
+    hid_t dataset_id;
+#if H5Dopen_vers == 2
+    dataset_id = H5Dopen2(file_id, name.c_str(), H5P_DEFAULT);
+#else
+    dataset_id = H5Dopen(file_id, name.c_str());
+#endif
+    CHECK_ERROR(dataset_id,"Error opening dataset in file.");
+
+    hid_t space_id = H5Dget_space(dataset_id);
+    hsize_t dims[2];
+    H5Sget_simple_extent_dims(space_id, dims, NULL);
+
+    hsize_t count[2];
+    hsize_t offset[2];
+
+    hsize_t item_cnt = dims[0]/mpi_size+(dims[0]%mpi_size==0 ? 0 : 1);
+    hsize_t cnt = (mpi_rank<mpi_size-1 ? item_cnt : dims[0]-item_cnt*(mpi_size-1));
+
+    count[0] = cnt;
+    count[1] = dims[1];
+    offset[0] = mpi_rank*item_cnt;
+    offset[1] = 0;
+
+    hid_t memspace_id = H5Screate_simple(2,count,NULL);
+
+    H5Sselect_hyperslab(space_id, H5S_SELECT_SET, offset, NULL, count, NULL);
+
+    dataset = flann::Matrix<T>(new T[count[0]*count[1]], count[0], count[1]);
+
+    plist_id = H5Pcreate(H5P_DATASET_XFER);
+//    H5Pset_dxpl_mpio(plist_id, H5FD_MPIO_COLLECTIVE);
+    status = H5Dread(dataset_id, get_hdf5_type<T>(), memspace_id, space_id, plist_id, dataset[0]);
+    CHECK_ERROR(status, "Error reading dataset");
+
+    H5Pclose(plist_id);
+    H5Sclose(space_id);
+    H5Sclose(memspace_id);
+    H5Dclose(dataset_id);
+    H5Fclose(file_id);
+}
+}
+#endif // HAVE_MPI
+} // namespace flann::mpi
+
+#endif /* FLANN_HDF5_H_ */

corelib/src/flann/mpi/client.h

@@ -0,0 +1,89 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * 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 MPI_CLIENT_H_
+#define MPI_CLIENT_H_
+
+#include <cstdlib>
+#include <boost/asio.hpp>
+#include <flann/util/matrix.h>
+#include <flann/util/params.h>
+#include "queries.h"
+
+namespace flann {
+namespace mpi {
+
+
+class Client
+{
+public:
+	Client(const std::string& host, const std::string& service)
+	{
+	    tcp::resolver resolver(io_service_);
+	    tcp::resolver::query query(tcp::v4(), host, service);
+	    iterator_ = resolver.resolve(query);
+	}
+
+
+	template<typename ElementType, typename DistanceType>
+	void knnSearch(const flann::Matrix<ElementType>& queries, flann::Matrix<int>& indices, flann::Matrix<DistanceType>& dists, int knn, const SearchParams& params)
+	{
+	    tcp::socket sock(io_service_);
+	    sock.connect(*iterator_);
+
+	    Request<ElementType> req;
+	    req.nn = knn;
+	    req.queries = queries;
+	    req.checks = params.checks;
+	    // send request
+	    write_object(sock,req);
+
+	    Response<DistanceType> resp;
+	    // read response
+	    read_object(sock, resp);
+
+	    for (size_t i=0;i<indices.rows;++i) {
+	    	for (size_t j=0;j<indices.cols;++j) {
+	    		indices[i][j] = resp.indices[i][j];
+	    		dists[i][j] = resp.dists[i][j];
+	    	}
+	    }
+	}
+
+
+private:
+	boost::asio::io_service io_service_;
+	tcp::resolver::iterator iterator_;
+};
+
+
+} //namespace mpi
+} // namespace flann
+
+#endif // MPI_CLIENT_H_

corelib/src/flann/mpi/flann_mpi_client.cpp

@@ -0,0 +1,85 @@
+#include <stdio.h>
+#include <time.h>
+
+#include <cstdlib>
+#include <iostream>
+#include <flann/util/params.h>
+#include <flann/io/hdf5.h>
+#include <flann/mpi/client.h>
+
+
+
+#define IF_RANK0 if (world.rank()==0)
+
+timeval start_time_;
+void start_timer(const std::string& message = "")
+{
+	if (!message.empty()) {
+		printf("%s", message.c_str());
+		fflush(stdout);
+	}
+    gettimeofday(&start_time_,NULL);
+}
+
+double stop_timer()
+{
+    timeval end_time;
+    gettimeofday(&end_time,NULL);
+
+	return double(end_time.tv_sec-start_time_.tv_sec)+ double(end_time.tv_usec-start_time_.tv_usec)/1000000;
+}
+
+float compute_precision(const flann::Matrix<int>& match, const flann::Matrix<int>& indices)
+{
+	int count = 0;
+
+	assert(match.rows == indices.rows);
+	size_t nn = std::min(match.cols, indices.cols);
+
+	for(size_t i=0; i<match.rows; ++i) {
+		for (size_t j=0;j<nn;++j) {
+			for (size_t k=0;k<nn;++k) {
+				if (match[i][j]==indices[i][k]) {
+					count ++;
+				}
+			}
+		}
+	}
+
+	return float(count)/(nn*match.rows);
+}
+
+
+int main(int argc, char* argv[])
+{
+	try {
+
+		flann::Matrix<float> query;
+		flann::Matrix<int> match;
+
+		flann::load_from_file(query, "sift100K.h5","query");
+		flann::load_from_file(match, "sift100K.h5","match");
+		//	flann::load_from_file(gt_dists, "sift100K.h5","dists");
+
+		flann::mpi::Client index("localhost","9999");
+
+		int nn = 1;
+		flann::Matrix<int> indices(new int[query.rows*nn], query.rows, nn);
+		flann::Matrix<float> dists(new float[query.rows*nn], query.rows, nn);
+
+		start_timer("Performing search...\n");
+		index.knnSearch(query, indices, dists, nn, flann::SearchParams(64));
+		printf("Search done (%g seconds)\n", stop_timer());
+
+		printf("Checking results\n");
+		float precision = compute_precision(match, indices);
+		printf("Precision is: %g\n", precision);
+
+	}
+	catch (std::exception& e) {
+		std::cerr << "Exception: " << e.what() << "\n";
+	}
+
+	return 0;
+}
+

corelib/src/flann/mpi/flann_mpi_server.cpp

@@ -0,0 +1,26 @@
+#include <boost/mpi.hpp>
+#include <flann/mpi/server.h>
+#include <stdio.h>
+#include <time.h>
+
+int main(int argc, char* argv[])
+{
+	boost::mpi::environment env(argc, argv);
+
+	try {
+		if (argc != 4) {
+			std::cout << "Usage: " << argv[0] << " <file> <dataset> <port>\n";
+			return 1;
+		}
+		flann::mpi::Server<flann::L2<float> > server(argv[1], argv[2], std::atoi(argv[3]),
+				flann::KDTreeIndexParams(4));
+
+		server.run();
+	}
+	catch (std::exception& e) {
+		std::cerr << "Exception: " << e.what() << "\n";
+	}
+
+	return 0;
+}
+

corelib/src/flann/mpi/index.h

@@ -0,0 +1,271 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2010  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2010  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * 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 FLANN_MPI_HPP_
+#define FLANN_MPI_HPP_
+
+#include <boost/mpi.hpp>
+#include <boost/serialization/array.hpp>
+#include <flann/flann.hpp>
+#include <flann/io/hdf5.h>
+
+namespace flann
+{
+namespace mpi
+{
+
+template<typename DistanceType>
+struct SearchResults
+{
+    flann::Matrix<int> indices;
+    flann::Matrix<DistanceType> dists;
+
+    template<typename Archive>
+    void serialize(Archive& ar, const unsigned int version)
+    {
+        ar& indices.rows;
+        ar& indices.cols;
+        if (Archive::is_loading::value) {
+            indices = Matrix<int>(new int[indices.rows*indices.cols], indices.rows, indices.cols);
+        }
+        ar& boost::serialization::make_array(indices.ptr(), indices.rows*indices.cols);
+        if (Archive::is_saving::value) {
+            delete[] indices.ptr();
+        }
+        ar& dists.rows;
+        ar& dists.cols;
+        if (Archive::is_loading::value) {
+            dists = Matrix<DistanceType>(new DistanceType[dists.rows*dists.cols], dists.rows, dists.cols);
+        }
+        ar& boost::serialization::make_array(dists.ptr(), dists.rows*dists.cols);
+        if (Archive::is_saving::value) {
+            delete[] dists.ptr();
+        }
+    }
+};
+
+template<typename DistanceType>
+struct ResultsMerger
+{
+    SearchResults<DistanceType> operator()(SearchResults<DistanceType> a, SearchResults<DistanceType> b)
+    {
+        SearchResults<DistanceType> results;
+        results.indices = flann::Matrix<int>(new int[a.indices.rows*a.indices.cols],a.indices.rows,a.indices.cols);
+        results.dists = flann::Matrix<DistanceType>(new DistanceType[a.dists.rows*a.dists.cols],a.dists.rows,a.dists.cols);
+
+
+        for (size_t i = 0; i < results.dists.rows; ++i) {
+            size_t idx = 0;
+            size_t a_idx = 0;
+            size_t b_idx = 0;
+            while (idx < results.dists.cols) {
+                if (a.dists[i][a_idx] <= b.dists[i][b_idx]) {
+                    results.dists[i][idx] = a.dists[i][a_idx];
+                    results.indices[i][idx] = a.indices[i][a_idx];
+                    idx++;
+                    a_idx++;
+                }
+                else {
+                    results.dists[i][idx] = b.dists[i][b_idx];
+                    results.indices[i][idx] = b.indices[i][b_idx];
+                    idx++;
+                    b_idx++;
+                }
+            }
+        }
+        delete[] a.indices.ptr();
+        delete[] a.dists.ptr();
+        delete[] b.indices.ptr();
+        delete[] b.dists.ptr();
+        return results;
+    }
+};
+
+
+
+template<typename Distance>
+class Index
+{
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    flann::Index<Distance>* flann_index;
+    flann::Matrix<ElementType> dataset;
+    int size_;
+    int offset_;
+
+public:
+    Index(const std::string& file_name,
+          const std::string& dataset_name,
+          const IndexParams& params);
+
+    ~Index();
+
+    void buildIndex()
+    {
+        flann_index->buildIndex();
+    }
+
+    void knnSearch(const flann::Matrix<ElementType>& queries,
+                   flann::Matrix<int>& indices,
+                   flann::Matrix<DistanceType>& dists,
+                   int knn, const
+                   SearchParams& params);
+
+    int radiusSearch(const flann::Matrix<ElementType>& query,
+                     flann::Matrix<int>& indices,
+                     flann::Matrix<DistanceType>& dists,
+                     float radius,
+                     const SearchParams& params);
+
+    // void save(std::string filename);
+
+    int veclen() const
+    {
+        return flann_index->veclen();
+    }
+
+    int size() const
+    {
+        return size_;
+    }
+
+    IndexParams getIndexParameters()
+    {
+        return flann_index->getParameters();
+    }
+};
+
+
+template<typename Distance>
+Index<Distance>::Index(const std::string& file_name, const std::string& dataset_name, const IndexParams& params)
+{
+    boost::mpi::communicator world;
+    flann_algorithm_t index_type = get_param<flann_algorithm_t>(params,"algorithm");
+    if (index_type == FLANN_INDEX_SAVED) {
+        throw FLANNException("Saving/loading of MPI indexes is not currently supported.");
+    }
+    flann::mpi::load_from_file(dataset, file_name, dataset_name);
+    flann_index = new flann::Index<Distance>(dataset, params);
+
+    std::vector<int> sizes;
+    // get the sizes of all MPI indices
+    all_gather(world, (int)flann_index->size(), sizes);
+    size_ = 0;
+    offset_ = 0;
+    for (size_t i = 0; i < sizes.size(); ++i) {
+        if ((int)i < world.rank()) offset_ += sizes[i];
+        size_ += sizes[i];
+    }
+}
+
+template<typename Distance>
+Index<Distance>::~Index()
+{
+    delete flann_index;
+    delete[] dataset.ptr();
+}
+
+template<typename Distance>
+void Index<Distance>::knnSearch(const flann::Matrix<ElementType>& queries, flann::Matrix<int>& indices, flann::Matrix<DistanceType>& dists, int knn, const SearchParams& params)
+{
+    boost::mpi::communicator world;
+    flann::Matrix<int> local_indices(new int[queries.rows*knn], queries.rows, knn);
+    flann::Matrix<DistanceType> local_dists(new DistanceType[queries.rows*knn], queries.rows, knn);
+
+    flann_index->knnSearch(queries, local_indices, local_dists, knn, params);
+    for (size_t i = 0; i < local_indices.rows; ++i) {
+        for (size_t j = 0; j < local_indices.cols; ++j) {
+            local_indices[i][j] += offset_;
+        }
+    }
+    SearchResults<DistanceType> local_results;
+    local_results.indices = local_indices;
+    local_results.dists = local_dists;
+    SearchResults<DistanceType> results;
+
+    // perform MPI reduce
+    reduce(world, local_results, results, ResultsMerger<DistanceType>(), 0);
+
+    if (world.rank() == 0) {
+        for (size_t i = 0; i < results.indices.rows; ++i) {
+            for (size_t j = 0; j < results.indices.cols; ++j) {
+                indices[i][j] = results.indices[i][j];
+                dists[i][j] = results.dists[i][j];
+            }
+        }
+        delete[] results.indices.ptr();
+        delete[] results.dists.ptr();
+    }
+}
+
+template<typename Distance>
+int Index<Distance>::radiusSearch(const flann::Matrix<ElementType>& query, flann::Matrix<int>& indices, flann::Matrix<DistanceType>& dists, float radius, const SearchParams& params)
+{
+    boost::mpi::communicator world;
+    flann::Matrix<int> local_indices(new int[indices.rows*indices.cols], indices.rows, indices.cols);
+    flann::Matrix<DistanceType> local_dists(new DistanceType[dists.rows*dists.cols], dists.rows, dists.cols);
+
+    flann_index->radiusSearch(query, local_indices, local_dists, radius, params);
+    for (size_t i = 0; i < local_indices.rows; ++i) {
+        for (size_t j = 0; j < local_indices.cols; ++j) {
+            local_indices[i][j] += offset_;
+        }
+    }
+    SearchResults<DistanceType> local_results;
+    local_results.indices = local_indices;
+    local_results.dists = local_dists;
+    SearchResults<DistanceType> results;
+
+    // perform MPI reduce
+    reduce(world, local_results, results, ResultsMerger<DistanceType>(), 0);
+
+    if (world.rank() == 0) {
+        for (int i = 0; i < std::min(results.indices.rows, indices.rows); ++i) {
+            for (int j = 0; j < std::min(results.indices.cols, indices.cols); ++j) {
+                indices[i][j] = results.indices[i][j];
+                dists[i][j] = results.dists[i][j];
+            }
+        }
+        delete[] results.indices.ptr();
+        delete[] results.dists.ptr();
+    }
+    return 0;
+}
+
+}
+} //namespace flann::mpi
+
+namespace boost { namespace mpi {
+template<typename DistanceType>
+struct is_commutative<flann::mpi::ResultsMerger<DistanceType>, flann::mpi::SearchResults<DistanceType> > : mpl::true_ { };
+} } // end namespace boost::mpi
+
+
+#endif /* FLANN_MPI_HPP_ */

corelib/src/flann/mpi/matrix.h

@@ -0,0 +1,54 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * 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 MPI_MATRIX_H_
+#define MPI_MATRIX_H_
+
+#include <flann/util/matrix.h>
+#include <boost/serialization/array.hpp>
+
+
+namespace boost {
+namespace serialization {
+
+template<class Archive, class T>
+void serialize(Archive & ar, flann::Matrix<T> & matrix, const unsigned int version)
+{
+	ar & matrix.rows & matrix.cols & matrix.stride;
+    if (Archive::is_loading::value) {
+        matrix = flann::Matrix<T>(new T[matrix.rows*matrix.cols], matrix.rows, matrix.cols, matrix.stride);
+    }
+    ar & boost::serialization::make_array(matrix.ptr(), matrix.rows*matrix.cols);
+}
+
+}
+}
+
+
+#endif /* MPI_MATRIX_H_ */

corelib/src/flann/mpi/queries.h

@@ -0,0 +1,103 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * 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 MPI_QUERIES_H_
+#define MPI_QUERIES_H_
+
+#include <flann/mpi/matrix.h>
+#include <boost/archive/binary_iarchive.hpp>
+#include <boost/archive/binary_oarchive.hpp>
+#include <boost/asio.hpp>
+
+namespace flann
+{
+
+template<typename T>
+struct Request
+{
+	flann::Matrix<T> queries;
+	int nn;
+	int checks;
+
+	template<typename Archive>
+	void serialize(Archive& ar, const unsigned int version)
+	{
+		ar & queries & nn & checks;
+	}
+};
+
+template<typename T>
+struct Response
+{
+	flann::Matrix<int> indices;
+	flann::Matrix<T> dists;
+
+	template<typename Archive>
+	void serialize(Archive& ar, const unsigned int version)
+	{
+		ar & indices & dists;
+	}
+};
+
+
+using boost::asio::ip::tcp;
+
+template <typename T>
+void read_object(tcp::socket& sock, T& val)
+{
+	uint32_t size;
+	boost::asio::read(sock, boost::asio::buffer(&size, sizeof(size)));
+	size = ntohl(size);
+
+	boost::asio::streambuf archive_stream;
+	boost::asio::read(sock, archive_stream, boost::asio::transfer_at_least(size));
+
+	boost::archive::binary_iarchive archive(archive_stream);
+	archive >> val;
+}
+
+template <typename T>
+void write_object(tcp::socket& sock, const T& val)
+{
+	boost::asio::streambuf archive_stream;
+	boost::archive::binary_oarchive archive(archive_stream);
+	archive << val;
+
+	uint32_t size = archive_stream.size();
+	size = htonl(size);
+	boost::asio::write(sock, boost::asio::buffer(&size, sizeof(size)));
+	boost::asio::write(sock, archive_stream);
+
+}
+
+}
+
+
+
+#endif /* MPI_QUERIES_H_ */

corelib/src/flann/mpi/server.h

@@ -0,0 +1,153 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * 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 MPI_SERVER_H_
+#define MPI_SERVER_H_
+
+#include <flann/mpi/index.h>
+#include <stdio.h>
+#include <time.h>
+
+#include <cstdlib>
+#include <iostream>
+#include <boost/bind.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/asio.hpp>
+#include <boost/thread/thread.hpp>
+
+#include "queries.h"
+
+namespace flann {
+
+namespace mpi {
+
+template<typename Distance>
+class Server
+{
+
+	typedef typename Distance::ElementType ElementType;
+	typedef typename Distance::ResultType DistanceType;
+	typedef boost::shared_ptr<tcp::socket> socket_ptr;
+	typedef flann::mpi::Index<Distance> FlannIndex;
+
+	void session(socket_ptr sock)
+	{
+		boost::mpi::communicator world;
+		try {
+			Request<ElementType> req;
+			if (world.rank()==0) {
+				read_object(*sock,req);
+				std::cout << "Received query\n";
+			}
+			// broadcast request to all MPI processes
+			boost::mpi::broadcast(world, req, 0);
+
+			Response<DistanceType> resp;
+			if (world.rank()==0) {
+				int rows = req.queries.rows;
+				int cols = req.nn;
+				resp.indices = flann::Matrix<int>(new int[rows*cols], rows, cols);
+				resp.dists = flann::Matrix<DistanceType>(new DistanceType[rows*cols], rows, cols);
+			}
+
+			std::cout << "Searching in process " << world.rank() << "\n";
+			index_->knnSearch(req.queries, resp.indices, resp.dists, req.nn, flann::SearchParams(req.checks));
+
+			if (world.rank()==0) {
+				std::cout << "Sending result\n";
+				write_object(*sock,resp);
+			}
+
+			delete[] req.queries.ptr();
+			if (world.rank()==0) {
+				delete[] resp.indices.ptr();
+				delete[] resp.dists.ptr();
+			}
+
+		}
+		catch (std::exception& e) {
+			std::cerr << "Exception in thread: " << e.what() << "\n";
+		}
+	}
+
+
+
+public:
+	Server(const std::string& filename, const std::string& dataset, short port, const IndexParams& params) :
+		port_(port)
+	{
+		boost::mpi::communicator world;
+		if (world.rank()==0) {
+			std::cout << "Reading dataset and building index...";
+			std::flush(std::cout);
+		}
+		index_ = new FlannIndex(filename, dataset, params);
+		index_->buildIndex();
+		world.barrier(); // wait for data to be loaded and indexes to be created
+		if (world.rank()==0) {
+			std::cout << "done.\n";
+		}
+	}
+
+
+	void run()
+	{
+		boost::mpi::communicator world;
+		boost::shared_ptr<boost::asio::io_service> io_service;
+		boost::shared_ptr<tcp::acceptor> acceptor;
+
+		if (world.rank()==0) {
+			io_service.reset(new boost::asio::io_service());
+			acceptor.reset(new tcp::acceptor(*io_service, tcp::endpoint(tcp::v4(), port_)));
+			std::cout << "Start listening for queries...\n";
+		}
+		for (;;) {
+			socket_ptr sock;
+			if (world.rank()==0) {
+				sock.reset(new tcp::socket(*io_service));
+				acceptor->accept(*sock);
+				std::cout << "Accepted connection\n";
+			}
+			world.barrier(); // everybody waits here for a connection
+			boost::thread t(boost::bind(&Server::session, this, sock));
+			t.join();
+		}
+
+	}
+
+private:
+	FlannIndex* index_;
+	short port_;
+};
+
+
+} // namespace mpi
+} // namespace flann
+
+#endif // MPI_SERVER_H_

corelib/src/flann/nn/ground_truth.h

@@ -0,0 +1,98 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_GROUND_TRUTH_H_
+#define FLANN_GROUND_TRUTH_H_
+
+#include "flann/algorithms/dist.h"
+#include "flann/util/matrix.h"
+
+
+namespace flann
+{
+
+template <typename Distance>
+void find_nearest(const Matrix<typename Distance::ElementType>& dataset, typename Distance::ElementType* query, size_t* matches, size_t nn,
+                  size_t skip = 0, Distance distance = Distance())
+{
+    //typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+    int n = nn + skip;
+
+    int* match = new int[n];
+    DistanceType* dists = new DistanceType[n];
+
+    dists[0] = distance(dataset[0], query, dataset.cols);
+    match[0] = 0;
+    int dcnt = 1;
+
+    for (size_t i=1; i<dataset.rows; ++i) {
+        DistanceType tmp = distance(dataset[i], query, dataset.cols);
+
+        if (dcnt<n) {
+            match[dcnt] = i;
+            dists[dcnt++] = tmp;
+        }
+        else if (tmp < dists[dcnt-1]) {
+            dists[dcnt-1] = tmp;
+            match[dcnt-1] = i;
+        }
+
+        int j = dcnt-1;
+        // bubble up
+        while (j>=1 && dists[j]<dists[j-1]) {
+            std::swap(dists[j],dists[j-1]);
+            std::swap(match[j],match[j-1]);
+            j--;
+        }
+    }
+
+    for (size_t i=0; i<nn; ++i) {
+        matches[i] = match[i+skip];
+    }
+
+    delete[] match;
+    delete[] dists;
+}
+
+
+template <typename Distance>
+void compute_ground_truth(const Matrix<typename Distance::ElementType>& dataset, const Matrix<typename Distance::ElementType>& testset, Matrix<size_t>& matches,
+                          int skip=0, Distance d = Distance())
+{
+    for (size_t i=0; i<testset.rows; ++i) {
+        find_nearest<Distance>(dataset, testset[i], matches[i], matches.cols, skip, d);
+    }
+}
+
+
+}
+
+#endif //FLANN_GROUND_TRUTH_H_

corelib/src/flann/nn/index_testing.h

@@ -0,0 +1,323 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_INDEX_TESTING_H_
+#define FLANN_INDEX_TESTING_H_
+
+#include <cstring>
+#include <cassert>
+#include <cmath>
+
+#include "flann/util/matrix.h"
+#include "flann/algorithms/nn_index.h"
+#include "flann/util/result_set.h"
+#include "flann/util/logger.h"
+#include "flann/util/timer.h"
+
+
+namespace flann
+{
+
+inline int countCorrectMatches(size_t* neighbors, size_t* groundTruth, int n)
+{
+    int count = 0;
+    for (int i=0; i<n; ++i) {
+        for (int k=0; k<n; ++k) {
+            if (neighbors[i]==groundTruth[k]) {
+                count++;
+                break;
+            }
+        }
+    }
+    return count;
+}
+
+
+template <typename Distance>
+typename Distance::ResultType computeDistanceRaport(const Matrix<typename Distance::ElementType>& inputData, typename Distance::ElementType* target,
+		size_t* neighbors, size_t* groundTruth, int veclen, int n, const Distance& distance)
+{
+    typedef typename Distance::ResultType DistanceType;
+
+    DistanceType ret = 0;
+    for (int i=0; i<n; ++i) {
+        DistanceType den = distance(inputData[groundTruth[i]], target, veclen);
+        DistanceType num = distance(inputData[neighbors[i]], target, veclen);
+
+        if ((den==0)&&(num==0)) {
+            ret += 1;
+        }
+        else {
+            ret += num/den;
+        }
+    }
+
+    return ret;
+}
+
+template <typename Index, typename Distance>
+float search_with_ground_truth(Index& index, const Matrix<typename Distance::ElementType>& inputData,
+                               const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches, int nn, int checks,
+                               float& time, typename Distance::ResultType& dist, const Distance& distance, int skipMatches)
+{
+    typedef typename Distance::ElementType ElementType;
+    typedef typename Distance::ResultType DistanceType;
+
+    if (matches.cols<size_t(nn)) {
+        Logger::info("matches.cols=%d, nn=%d\n",matches.cols,nn);
+        throw FLANNException("Ground truth is not computed for as many neighbors as requested");
+    }
+
+    SearchParams searchParams(checks);
+
+    size_t* indices = new size_t[nn+skipMatches];
+    DistanceType* dists = new DistanceType[nn+skipMatches];
+    
+    Matrix<size_t> indices_mat(indices, 1, nn+skipMatches);
+    Matrix<DistanceType> dists_mat(dists, 1, nn+skipMatches);
+        
+    size_t* neighbors = indices + skipMatches;
+
+    int correct = 0;
+    DistanceType distR = 0;
+    StartStopTimer t;
+    int repeats = 0;
+    while (t.value<0.2) {
+        repeats++;
+        t.start();
+        correct = 0;
+        distR = 0;        
+        for (size_t i = 0; i < testData.rows; i++) {
+            index.knnSearch(Matrix<ElementType>(testData[i], 1, testData.cols), indices_mat, dists_mat, nn+skipMatches, searchParams);
+
+            correct += countCorrectMatches(neighbors,matches[i], nn);
+            distR += computeDistanceRaport<Distance>(inputData, testData[i], neighbors, matches[i], testData.cols, nn, distance);
+        }
+        t.stop();
+    }
+    time = float(t.value/repeats);
+
+    delete[] indices;
+    delete[] dists;
+
+    float precicion = (float)correct/(nn*testData.rows);
+
+    dist = distR/(testData.rows*nn);
+
+    Logger::info("%8d %10.4g %10.5g %10.5g %10.5g\n",
+                 checks, precicion, time, 1000.0 * time / testData.rows, dist);
+
+    return precicion;
+}
+
+
+template <typename Index, typename Distance>
+float test_index_checks(Index& index, const Matrix<typename Distance::ElementType>& inputData,
+                        const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches,
+                        int checks, float& precision, const Distance& distance, int nn = 1, int skipMatches = 0)
+{
+    typedef typename Distance::ResultType DistanceType;
+
+    Logger::info("  Nodes  Precision(%)   Time(s)   Time/vec(ms)  Mean dist\n");
+    Logger::info("---------------------------------------------------------\n");
+
+    float time = 0;
+    DistanceType dist = 0;
+    precision = search_with_ground_truth(index, inputData, testData, matches, nn, checks, time, dist, distance, skipMatches);
+
+    return time;
+}
+
+template <typename Index, typename Distance>
+float test_index_precision(Index& index, const Matrix<typename Distance::ElementType>& inputData,
+                           const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches,
+                           float precision, int& checks, const Distance& distance, int nn = 1, int skipMatches = 0)
+{
+    typedef typename Distance::ResultType DistanceType;
+    const float SEARCH_EPS = 0.001f;
+
+    Logger::info("  Nodes  Precision(%)   Time(s)   Time/vec(ms)  Mean dist\n");
+    Logger::info("---------------------------------------------------------\n");
+
+    int c2 = 1;
+    float p2;
+    int c1 = 1;
+//     float p1;
+    float time;
+    DistanceType dist;
+
+    p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
+
+    if (p2>precision) {
+        Logger::info("Got as close as I can\n");
+        checks = c2;
+        return time;
+    }
+
+    while (p2<precision) {
+        c1 = c2;
+//         p1 = p2;
+        c2 *=2;
+        p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
+    }
+
+    int cx;
+    float realPrecision;
+    if (fabs(p2-precision)>SEARCH_EPS) {
+        Logger::info("Start linear estimation\n");
+        // after we got to values in the vecinity of the desired precision
+        // use linear approximation get a better estimation
+
+        cx = (c1+c2)/2;
+        realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
+        while (fabs(realPrecision-precision)>SEARCH_EPS) {
+
+            if (realPrecision<precision) {
+                c1 = cx;
+            }
+            else {
+                c2 = cx;
+            }
+            cx = (c1+c2)/2;
+            if (cx==c1) {
+                Logger::info("Got as close as I can\n");
+                break;
+            }
+            realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
+        }
+
+        c2 = cx;
+        p2 = realPrecision;
+
+    }
+    else {
+        Logger::info("No need for linear estimation\n");
+        cx = c2;
+        realPrecision = p2;
+    }
+
+    checks = cx;
+    return time;
+}
+
+
+template <typename Index, typename Distance>
+void test_index_precisions(Index& index, const Matrix<typename Distance::ElementType>& inputData,
+                           const Matrix<typename Distance::ElementType>& testData, const Matrix<int>& matches,
+                           float* precisions, int precisions_length, const Distance& distance, int nn = 1, int skipMatches = 0, float maxTime = 0)
+{
+    typedef typename Distance::ResultType DistanceType;
+
+    const float SEARCH_EPS = 0.001;
+
+    // make sure precisions array is sorted
+    std::sort(precisions, precisions+precisions_length);
+
+    int pindex = 0;
+    float precision = precisions[pindex];
+
+    Logger::info("  Nodes  Precision(%)   Time(s)   Time/vec(ms)  Mean dist\n");
+    Logger::info("---------------------------------------------------------\n");
+
+    int c2 = 1;
+    float p2;
+
+    int c1 = 1;
+    float p1;
+
+    float time;
+    DistanceType dist;
+
+    p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
+
+    // if precision for 1 run down the tree is already
+    // better then some of the requested precisions, then
+    // skip those
+    while (precisions[pindex]<p2 && pindex<precisions_length) {
+        pindex++;
+    }
+
+    if (pindex==precisions_length) {
+        Logger::info("Got as close as I can\n");
+        return;
+    }
+
+    for (int i=pindex; i<precisions_length; ++i) {
+
+        precision = precisions[i];
+        while (p2<precision) {
+            c1 = c2;
+            p1 = p2;
+            c2 *=2;
+            p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
+            if ((maxTime> 0)&&(time > maxTime)&&(p2<precision)) return;
+        }
+
+        int cx;
+        float realPrecision;
+        if (fabs(p2-precision)>SEARCH_EPS) {
+            Logger::info("Start linear estimation\n");
+            // after we got to values in the vecinity of the desired precision
+            // use linear approximation get a better estimation
+
+            cx = (c1+c2)/2;
+            realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
+            while (fabs(realPrecision-precision)>SEARCH_EPS) {
+
+                if (realPrecision<precision) {
+                    c1 = cx;
+                }
+                else {
+                    c2 = cx;
+                }
+                cx = (c1+c2)/2;
+                if (cx==c1) {
+                    Logger::info("Got as close as I can\n");
+                    break;
+                }
+                realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
+            }
+
+            c2 = cx;
+            p2 = realPrecision;
+
+        }
+        else {
+            Logger::info("No need for linear estimation\n");
+            cx = c2;
+            realPrecision = p2;
+        }
+
+    }
+}
+
+}
+
+#endif //FLANN_INDEX_TESTING_H_

corelib/src/flann/nn/simplex_downhill.h

@@ -0,0 +1,186 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_SIMPLEX_DOWNHILL_H_
+#define FLANN_SIMPLEX_DOWNHILL_H_
+
+namespace flann
+{
+
+/**
+    Adds val to array vals (and point to array points) and keeping the arrays sorted by vals.
+ */
+template <typename T>
+void addValue(int pos, float val, float* vals, T* point, T* points, int n)
+{
+    vals[pos] = val;
+    for (int i=0; i<n; ++i) {
+        points[pos*n+i] = point[i];
+    }
+
+    // bubble down
+    int j=pos;
+    while (j>0 && vals[j]<vals[j-1]) {
+        swap(vals[j],vals[j-1]);
+        for (int i=0; i<n; ++i) {
+            swap(points[j*n+i],points[(j-1)*n+i]);
+        }
+        --j;
+    }
+}
+
+
+/**
+    Simplex downhill optimization function.
+    Preconditions: points is a 2D mattrix of size (n+1) x n
+                    func is the cost function taking n an array of n params and returning float
+                    vals is the cost function in the n+1 simplex points, if NULL it will be computed
+
+    Postcondition: returns optimum value and points[0..n] are the optimum parameters
+ */
+template <typename T, typename F>
+float optimizeSimplexDownhill(T* points, int n, F func, float* vals = NULL )
+{
+    const int MAX_ITERATIONS = 10;
+
+    assert(n>0);
+
+    T* p_o = new T[n];
+    T* p_r = new T[n];
+    T* p_e = new T[n];
+
+    int alpha = 1;
+
+    int iterations = 0;
+
+    bool ownVals = false;
+    if (vals == NULL) {
+        ownVals = true;
+        vals = new float[n+1];
+        for (int i=0; i<n+1; ++i) {
+            float val = func(points+i*n);
+            addValue(i, val, vals, points+i*n, points, n);
+        }
+    }
+    int nn = n*n;
+
+    while (true) {
+
+        if (iterations++ > MAX_ITERATIONS) break;
+
+        // compute average of simplex points (except the highest point)
+        for (int j=0; j<n; ++j) {
+            p_o[j] = 0;
+            for (int i=0; i<n; ++i) {
+                p_o[i] += points[j*n+i];
+            }
+        }
+        for (int i=0; i<n; ++i) {
+            p_o[i] /= n;
+        }
+
+        bool converged = true;
+        for (int i=0; i<n; ++i) {
+            if (p_o[i] != points[nn+i]) {
+                converged = false;
+            }
+        }
+        if (converged) break;
+
+        // trying a reflection
+        for (int i=0; i<n; ++i) {
+            p_r[i] = p_o[i] + alpha*(p_o[i]-points[nn+i]);
+        }
+        float val_r = func(p_r);
+
+        if ((val_r>=vals[0])&&(val_r<vals[n])) {
+            // reflection between second highest and lowest
+            // add it to the simplex
+            Logger::info("Choosing reflection\n");
+            addValue(n, val_r,vals, p_r, points, n);
+            continue;
+        }
+
+        if (val_r<vals[0]) {
+            // value is smaller than smalest in simplex
+
+            // expand some more to see if it drops further
+            for (int i=0; i<n; ++i) {
+                p_e[i] = 2*p_r[i]-p_o[i];
+            }
+            float val_e = func(p_e);
+
+            if (val_e<val_r) {
+                Logger::info("Choosing reflection and expansion\n");
+                addValue(n, val_e,vals,p_e,points,n);
+            }
+            else {
+                Logger::info("Choosing reflection\n");
+                addValue(n, val_r,vals,p_r,points,n);
+            }
+            continue;
+        }
+        if (val_r>=vals[n]) {
+            for (int i=0; i<n; ++i) {
+                p_e[i] = (p_o[i]+points[nn+i])/2;
+            }
+            float val_e = func(p_e);
+
+            if (val_e<vals[n]) {
+                Logger::info("Choosing contraction\n");
+                addValue(n,val_e,vals,p_e,points,n);
+                continue;
+            }
+        }
+        {
+            Logger::info("Full contraction\n");
+            for (int j=1; j<=n; ++j) {
+                for (int i=0; i<n; ++i) {
+                    points[j*n+i] = (points[j*n+i]+points[i])/2;
+                }
+                float val = func(points+j*n);
+                addValue(j,val,vals,points+j*n,points,n);
+            }
+        }
+    }
+
+    float bestVal = vals[0];
+
+    delete[] p_r;
+    delete[] p_o;
+    delete[] p_e;
+    if (ownVals) delete[] vals;
+
+    return bestVal;
+}
+
+}
+
+#endif //FLANN_SIMPLEX_DOWNHILL_H_

corelib/src/flann/readme.txt

@@ -0,0 +1,8 @@
+
+flann is included in rtabmap for convenience, to have this commit needed for incremental flann index:
+https://github.com/mariusmuja/flann/commit/23051820b2314f07cf40ba633a4067782a982ff3
+
+Info: http://www.cs.ubc.ca/research/flann/
+Source: https://github.com/mariusmuja/flann
+Commit: 9a01507d2231b29eb688246a6e442054defa7460
+License: BSD 

corelib/src/flann/util/allocator.h

@@ -0,0 +1,202 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_ALLOCATOR_H_
+#define FLANN_ALLOCATOR_H_
+
+#include <stdlib.h>
+#include <stdio.h>
+
+
+namespace flann
+{
+
+/**
+ * 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>
+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. */
+
+
+    int     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. */
+    int     blocksize;
+
+
+public:
+    int     usedMemory;
+    int     wastedMemory;
+
+    /**
+        Default constructor. Initializes a new pool.
+     */
+    PooledAllocator(int blocksize = BLOCKSIZE)
+    {
+        this->blocksize = blocksize;
+        remaining = 0;
+        base = NULL;
+
+        usedMemory = 0;
+        wastedMemory = 0;
+    }
+
+    /**
+     * Destructor. Frees all the memory allocated in this pool.
+     */
+    ~PooledAllocator()
+    {
+        free();
+    }
+    
+    void free()
+    {
+        void* prev;
+        while (base != NULL) {
+            prev = *((void**) base); /* Get pointer to prev block. */
+            ::free(base);
+            base = prev;
+        }
+        base = NULL;
+        remaining = 0;
+        usedMemory = 0;
+        wastedMemory = 0;
+    }
+
+    /**
+     * Returns a pointer to a piece of new memory of the given size in bytes
+     * allocated from the pool.
+     */
+    void* allocateMemory(int size)
+    {
+        int blocksize;
+
+        /* 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.
+         */
+        size = (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. */
+            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;
+
+            int shift = 0;
+            //int shift = (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(size_t count = 1)
+    {
+        T* mem = (T*) this->allocateMemory((int)(sizeof(T)*count));
+        return mem;
+    }
+
+};
+
+}
+
+inline void* operator new (std::size_t size, flann::PooledAllocator& allocator)
+{
+    return allocator.allocateMemory(size) ;
+}
+
+#endif //FLANN_ALLOCATOR_H_

corelib/src/flann/util/any.h

@@ -0,0 +1,294 @@
+#ifndef FLANN_ANY_H_
+#define FLANN_ANY_H_
+/*
+ * (C) Copyright Christopher Diggins 2005-2011
+ * (C) Copyright Pablo Aguilar 2005
+ * (C) Copyright Kevlin Henney 2001
+ *
+ * Distributed under the Boost Software License, Version 1.0. (See
+ * accompanying file LICENSE_1_0.txt or copy at
+ * http://www.boost.org/LICENSE_1_0.txt
+ *
+ * Adapted for FLANN by Marius Muja
+ */
+
+#include <stdexcept>
+#include <ostream>
+#include <typeinfo>
+
+namespace flann
+{
+
+namespace anyimpl
+{
+
+struct bad_any_cast : public std::runtime_error
+{
+	bad_any_cast() : std::runtime_error("Cannot convert 'any' value") { }
+};
+
+struct empty_any
+{
+};
+
+inline std::ostream& operator <<(std::ostream& out, const empty_any&)
+{
+    out << "[empty_any]";
+    return out;
+}
+
+struct base_any_policy
+{
+    virtual void static_delete(void** x) = 0;
+    virtual void copy_from_value(void const* src, void** dest) = 0;
+    virtual void clone(void* const* src, void** dest) = 0;
+    virtual void move(void* const* src, void** dest) = 0;
+    virtual void* get_value(void** src) = 0;
+    virtual const void* get_value(void* const * src) = 0;
+    virtual ::size_t get_size() = 0;
+    virtual const std::type_info& type() = 0;
+    virtual void print(std::ostream& out, void* const* src) = 0;
+};
+
+template<typename T>
+struct typed_base_any_policy : base_any_policy
+{
+    virtual ::size_t get_size() { return sizeof(T); }
+    virtual const std::type_info& type() { return typeid(T); }
+
+};
+
+template<typename T>
+struct small_any_policy : typed_base_any_policy<T>
+{
+    virtual void static_delete(void**) { }
+    virtual void copy_from_value(void const* src, void** dest)
+    {
+        new (dest) T(* reinterpret_cast<T const*>(src));
+    }
+    virtual void clone(void* const* src, void** dest) { *dest = *src; }
+    virtual void move(void* const* src, void** dest) { *dest = *src; }
+    virtual void* get_value(void** src) { return reinterpret_cast<void*>(src); }
+    virtual const void* get_value(void* const * src) { return reinterpret_cast<const void*>(src); }
+    virtual void print(std::ostream& out, void* const* src) { out << *reinterpret_cast<T const*>(src); }
+};
+
+template<typename T>
+struct big_any_policy : typed_base_any_policy<T>
+{
+    virtual void static_delete(void** x)
+    {
+        if (* x) delete (* reinterpret_cast<T**>(x)); *x = NULL;
+    }
+    virtual void copy_from_value(void const* src, void** dest)
+    {
+        *dest = new T(*reinterpret_cast<T const*>(src));
+    }
+    virtual void clone(void* const* src, void** dest)
+    {
+        *dest = new T(**reinterpret_cast<T* const*>(src));
+    }
+    virtual void move(void* const* src, void** dest)
+    {
+        (*reinterpret_cast<T**>(dest))->~T();
+        **reinterpret_cast<T**>(dest) = **reinterpret_cast<T* const*>(src);
+    }
+    virtual void* get_value(void** src) { return *src; }
+    virtual const void* get_value(void* const * src) { return *src; }
+    virtual void print(std::ostream& out, void* const* src) { out << *reinterpret_cast<T const*>(*src); }
+};
+
+template<typename T>
+struct choose_policy
+{
+    typedef big_any_policy<T> type;
+};
+
+template<typename T>
+struct choose_policy<T*>
+{
+    typedef small_any_policy<T*> type;
+};
+
+struct any;
+
+/// Choosing the policy for an any type is illegal, but should never happen.
+/// This is designed to throw a compiler error.
+template<>
+struct choose_policy<any>
+{
+    typedef void type;
+};
+
+/// Specializations for small types.
+#define SMALL_POLICY(TYPE) \
+    template<> \
+    struct choose_policy<TYPE> { typedef small_any_policy<TYPE> type; \
+    }
+
+SMALL_POLICY(signed char);
+SMALL_POLICY(unsigned char);
+SMALL_POLICY(signed short);
+SMALL_POLICY(unsigned short);
+SMALL_POLICY(signed int);
+SMALL_POLICY(unsigned int);
+SMALL_POLICY(signed long);
+SMALL_POLICY(unsigned long);
+SMALL_POLICY(float);
+SMALL_POLICY(bool);
+
+//#undef SMALL_POLICY
+
+/// This function will return a different policy for each type.
+template<typename T>
+base_any_policy* get_policy()
+{
+    static typename choose_policy<T>::type policy;
+    return &policy;
+}
+} // namespace anyimpl
+
+class any
+{
+	typedef any any_t; // workaround for the NVCC compiler under windows
+private:
+    // fields
+    anyimpl::base_any_policy* policy;
+    void* object;
+
+public:
+    /// Initializing constructor.
+    template <typename T>
+    any(const T& x)
+        : policy(anyimpl::get_policy<anyimpl::empty_any>()), object(NULL)
+    {
+        assign(x);
+    }
+
+    /// Empty constructor.
+    any()
+        : policy(anyimpl::get_policy<anyimpl::empty_any>()), object(NULL)
+    { }
+
+    /// Special initializing constructor for string literals.
+    any(const char* x)
+        : policy(anyimpl::get_policy<anyimpl::empty_any>()), object(NULL)
+    {
+        assign(x);
+    }
+
+    /// Copy constructor.
+    any(const any& x)
+        : policy(anyimpl::get_policy<anyimpl::empty_any>()), object(NULL)
+    {
+        assign(x);
+    }
+
+    /// Destructor.
+    ~any()
+    {
+        policy->static_delete(&object);
+    }
+
+    /// Assignment function from another any.
+    any& assign(const any& x)
+    {
+        reset();
+        policy = x.policy;
+        policy->clone(&x.object, &object);
+        return *this;
+    }
+
+    /// Assignment function.
+    template <typename T>
+    any_t& assign(const T& x)
+    {
+        reset();
+        policy = anyimpl::get_policy<T>();
+        policy->copy_from_value(&x, &object);
+        return *this;
+    }
+
+    /// Assignment operator.
+    template<typename T>
+    any_t& operator=(const T& x)
+    {
+        return assign(x);
+    }
+
+    /// Assignment operator, specialed for literal strings.
+    /// They have types like const char [6] which don't work as expected.
+    any& operator=(const char* x)
+    {
+        return assign(x);
+    }
+
+    /// Utility functions
+    any& swap(any& x)
+    {
+        std::swap(policy, x.policy);
+        std::swap(object, x.object);
+        return *this;
+    }
+
+    /// Cast operator. You can only cast to the original type.
+    template<typename T>
+    T& cast()
+    {
+        if (policy->type() != typeid(T)) throw anyimpl::bad_any_cast();
+        T* r = reinterpret_cast<T*>(policy->get_value(&object));
+        return *r;
+    }
+
+    /// Cast operator. You can only cast to the original type.
+    template<typename T>
+    const T& cast() const
+    {
+        if (policy->type() != typeid(T)) throw anyimpl::bad_any_cast();
+        const T* r = reinterpret_cast<const T*>(policy->get_value(&object));
+        return *r;
+    }
+
+    /// Returns true if the any contains no value.
+    bool empty() const
+    {
+        return policy->type() == typeid(anyimpl::empty_any);
+    }
+
+    /// Frees any allocated memory, and sets the value to NULL.
+    void reset()
+    {
+        policy->static_delete(&object);
+        policy = anyimpl::get_policy<anyimpl::empty_any>();
+    }
+
+    /// Returns true if the two types are the same.
+    bool compatible(const any& x) const
+    {
+        return policy->type() == x.policy->type();
+    }
+
+    /// Returns if the type is compatible with the policy
+    template<typename T>
+    bool has_type()
+    {
+        return policy->type() == typeid(T);
+    }
+
+    const std::type_info& type() const
+    {
+        return policy->type();
+    }
+
+    friend std::ostream& operator <<(std::ostream& out, const any& any_val);
+};
+
+inline std::ostream& operator <<(std::ostream& out, const any& any_val)
+{
+    any_val.policy->print(out,&any_val.object);
+    return out;
+}
+
+}
+
+#endif // FLANN_ANY_H_

corelib/src/flann/util/cuda/heap.h

@@ -0,0 +1,139 @@
+#ifndef FLANN_UTIL_CUDA_HEAP_H
+#define FLANN_UTIL_CUDA_HEAP_H
+
+/*
+    Copyright (c) 2011, Andreas Mützel <andreas.muetzel@gmx.net>
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+ * Redistributions of source code must retain the above copyright
+        notice, this list of conditions and the following disclaimer.
+ * 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.
+ * Neither the name of the <organization> nor the
+        names of its contributors may be used to endorse or promote products
+        derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY Andreas Mützel <andreas.muetzel@gmx.net> ''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 Andreas Mützel <andreas.muetzel@gmx.net> 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.
+ */
+
+namespace flann
+{
+namespace cuda
+{
+template <class T>
+__device__ __host__ void swap( T& x, T& y )
+{
+    T t=x;
+    x=y;
+    y=t;
+}
+
+namespace heap
+{
+
+//! moves an element down the heap until all children are smaller than the element
+//! if c is a less-than comparator, it do this until all children are larger
+template <class GreaterThan, class RandomAccessIterator>
+__host__ __device__ void
+sift_down( RandomAccessIterator array, size_t begin, size_t length, GreaterThan c = GreaterThan() )
+{
+
+    while( 2*begin+1 < length ) {
+        size_t left = 2*begin+1;
+        size_t right = 2*begin+2;
+        size_t largest=begin;
+        if((left < length)&& c(array[left], array[largest]) ) largest=left;
+
+        if((right < length)&& c(array[right], array[largest]) ) largest=right;
+
+        if( largest != begin ) {
+            cuda::swap( array[begin], array[largest] );
+            begin=largest;
+        }
+        else return;
+    }
+}
+
+//! creates a max-heap in the array beginning at begin of length "length"
+//! if c is a less-than comparator, it will create a min-heap
+template <class GreaterThan, class RandomAccessIterator>
+__host__ __device__ void
+make_heap( RandomAccessIterator begin, size_t length, GreaterThan c = GreaterThan() )
+{
+    int i=length/2-1;
+    while( i>=0 ) {
+        sift_down( begin, i, length, c );
+        i--;
+    }
+}
+
+
+//! verifies if the array is a max-heap
+//! if c is a less-than comparator, it will verify if it is a min-heap
+template <class GreaterThan, class RandomAccessIterator>
+__host__ __device__ bool
+is_heap( RandomAccessIterator begin, size_t length, GreaterThan c = GreaterThan() )
+{
+    for( unsigned i=0; i<length; i++ ) {
+        if((2*i+1 < length)&& c(begin[2*i+1],begin[i]) ) return false;
+        if((2*i+2 < length)&& c(begin[2*i+2],begin[i]) ) return false;
+    }
+    return true;
+}
+
+
+//! moves an element down the heap until all children are smaller than the element
+//! if c is a less-than comparator, it do this until all children are larger
+template <class GreaterThan, class RandomAccessIterator, class RandomAccessIterator2>
+__host__ __device__ void
+sift_down( RandomAccessIterator key, RandomAccessIterator2 value, size_t begin, size_t length, GreaterThan c = GreaterThan() )
+{
+
+    while( 2*begin+1 < length ) {
+        size_t left = 2*begin+1;
+        size_t right = 2*begin+2;
+        size_t largest=begin;
+        if((left < length)&& c(key[left], key[largest]) ) largest=left;
+
+        if((right < length)&& c(key[right], key[largest]) ) largest=right;
+
+        if( largest != begin ) {
+            cuda::swap( key[begin], key[largest] );
+            cuda::swap( value[begin], value[largest] );
+            begin=largest;
+        }
+        else return;
+    }
+}
+
+//! creates a max-heap in the array beginning at begin of length "length"
+//! if c is a less-than comparator, it will create a min-heap
+template <class GreaterThan, class RandomAccessIterator, class RandomAccessIterator2>
+__host__ __device__ void
+make_heap( RandomAccessIterator key,  RandomAccessIterator2 value, size_t length, GreaterThan c = GreaterThan() )
+{
+    int i=length/2-1;
+    while( i>=0 ) {
+        sift_down( key, value, i, length, c );
+        i--;
+    }
+}
+
+}
+
+}
+}
+
+#endif

corelib/src/flann/util/cuda/result_set.h

@@ -0,0 +1,536 @@
+/**********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2011  Andreas Muetzel (amuetzel@uni-koblenz.de). 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 FLANN_UTIL_CUDA_RESULTSET_H
+#define FLANN_UTIL_CUDA_RESULTSET_H
+
+#include <flann/util/cuda/heap.h>
+#include <limits>
+
+__device__ __forceinline__
+float infinity()
+{
+       return __int_as_float(0x7f800000);
+}
+
+#ifndef INFINITY
+#define INFINITY infinity()
+#endif
+
+namespace flann
+{
+namespace cuda
+{
+//! result set for the 1nn search. Doesn't do any global memory accesses on its own,
+template< typename DistanceType >
+struct SingleResultSet
+{
+    int bestIndex;
+    DistanceType bestDist;
+    const DistanceType epsError;
+
+    __device__ __host__
+    SingleResultSet( DistanceType eps ) : bestIndex(-1),bestDist(INFINITY), epsError(eps){ }
+
+    __device__
+    inline float
+    worstDist()
+    {
+        return bestDist;
+    }
+
+    __device__
+    inline void
+    insert(int index, DistanceType dist)
+    {
+        if( dist <= bestDist ) {
+            bestIndex=index;
+            bestDist=dist;
+        }
+    }
+
+    DistanceType* resultDist;
+    int* resultIndex;
+
+    __device__
+    inline void
+    setResultLocation( DistanceType* dists, int* index, int thread, int stride )
+    {
+        resultDist=dists+thread*stride;
+        resultIndex=index+thread*stride;
+        if( stride != 1 ) {
+            for( int i=1; i<stride; i++ ) {
+                resultDist[i]=INFINITY;
+                resultIndex[i]=-1;
+            }
+        }
+    }
+
+    __device__
+    inline void
+    finish()
+    {
+        resultDist[0]=bestDist;
+        resultIndex[0]=bestIndex;
+    }
+};
+
+template< typename DistanceType >
+struct GreaterThan
+{
+    __device__
+    bool operator()(DistanceType a, DistanceType b)
+    {
+        return a>b;
+    }
+};
+
+
+// using this and the template uses 2 or 3 registers more than the direct implementation in the kNearestKernel, but
+// there is no speed difference.
+// Setting useHeap as a template parameter leads to a whole lot of things being
+// optimized away by nvcc.
+// Register counts are the same as when removing not-needed variables in explicit specializations
+// and the "if( useHeap )" branches are eliminated at compile time.
+// The downside of this: a bit more complex kernel launch code.
+template< typename DistanceType, bool useHeap >
+struct KnnResultSet
+{
+    int foundNeighbors;
+    DistanceType largestHeapDist;
+    int maxDistIndex;
+    const int k;
+    const bool sorted;
+    const DistanceType epsError;
+
+
+    __device__ __host__
+    KnnResultSet(int knn, bool sortResults, DistanceType eps) : foundNeighbors(0),largestHeapDist(INFINITY),k(knn), sorted(sortResults), epsError(eps){ }
+
+    //          __host__ __device__
+    //          KnnResultSet(const KnnResultSet& o):foundNeighbors(o.foundNeighbors),largestHeapDist(o.largestHeapDist),k(o.k){ }
+
+    __device__
+    inline DistanceType
+    worstDist()
+    {
+        return largestHeapDist;
+    }
+
+    __device__
+    inline void
+    insert(int index, DistanceType dist)
+    {
+        if( foundNeighbors<k ) {
+            resultDist[foundNeighbors]=dist;
+            resultIndex[foundNeighbors]=index;
+            if( foundNeighbors==k-1) {
+                if( useHeap ) {
+                    flann::cuda::heap::make_heap(resultDist,resultIndex,k,GreaterThan<DistanceType>());
+                    largestHeapDist=resultDist[0];
+                }
+                else {
+                    findLargestDistIndex();
+                }
+
+            }
+            foundNeighbors++;
+        }
+        else if( dist < largestHeapDist ) {
+            if( useHeap ) {
+                resultDist[0]=dist;
+                resultIndex[0]=index;
+                flann::cuda::heap::sift_down(resultDist,resultIndex,0,k,GreaterThan<DistanceType>());
+                largestHeapDist=resultDist[0];
+            }
+            else {
+                resultDist[maxDistIndex]=dist;
+                resultIndex[maxDistIndex]=index;
+                findLargestDistIndex();
+            }
+
+        }
+    }
+
+    __device__
+    void
+    findLargestDistIndex( )
+    {
+        largestHeapDist=resultDist[0];
+        maxDistIndex=0;
+        for( int i=1; i<k; i++ )
+            if( resultDist[i] > largestHeapDist ) {
+                maxDistIndex=i;
+                largestHeapDist=resultDist[i];
+            }
+    }
+
+    float* resultDist;
+    int* resultIndex;
+
+    __device__
+    inline void
+    setResultLocation( DistanceType* dists, int* index, int thread, int stride )
+    {
+        resultDist=dists+stride*thread;
+        resultIndex=index+stride*thread;
+        for( int i=0; i<stride; i++ ) {
+            resultDist[i]=INFINITY;
+            resultIndex[i]=-1;
+            //                  resultIndex[tid+i*blockDim.x]=-1;
+            //                  resultDist[tid+i*blockDim.x]=INFINITY;
+        }
+    }
+
+    __host__ __device__
+    inline void
+    finish()
+    {
+        if( sorted ) {
+            if( !useHeap ) flann::cuda::heap::make_heap(resultDist,resultIndex,k,GreaterThan<DistanceType>());
+            for( int i=k-1; i>0; i-- ) {
+                flann::cuda::swap( resultDist[0], resultDist[i] );
+                flann::cuda::swap( resultIndex[0], resultIndex[i] );
+                flann::cuda::heap::sift_down( resultDist,resultIndex, 0, i, GreaterThan<DistanceType>() );
+            }
+        }
+    }
+};
+
+template <typename DistanceType>
+struct CountingRadiusResultSet
+{
+    int count_;
+    DistanceType radius_sq_;
+    int max_neighbors_;
+
+    __device__ __host__
+    CountingRadiusResultSet(DistanceType radius, int max_neighbors) : count_(0),radius_sq_(radius), max_neighbors_(max_neighbors){ }
+
+    __device__
+    inline DistanceType
+    worstDist()
+    {
+        return radius_sq_;
+    }
+
+    __device__
+    inline void
+    insert(int index, float dist)
+    {
+        if( dist < radius_sq_ ) {
+            count_++;
+        }
+    }
+
+    int* resultIndex;
+
+    __device__
+    inline void
+    setResultLocation( DistanceType* /*dists*/, int* count, int thread, int stride )
+    {
+        resultIndex=count+thread*stride;
+    }
+
+    __device__
+    inline void
+    finish()
+    {
+        if(( max_neighbors_<=0) ||( count_<=max_neighbors_) ) resultIndex[0]=count_;
+        else resultIndex[0]=max_neighbors_;
+    }
+};
+
+template<typename DistanceType, bool useHeap>
+struct RadiusKnnResultSet
+{
+    int foundNeighbors;
+    DistanceType largestHeapDist;
+    int maxDistElem;
+    const int k;
+    const bool sorted;
+    const DistanceType radius_sq_;
+    int* segment_starts_;
+    //          int count_;
+
+
+    __device__ __host__
+    RadiusKnnResultSet(DistanceType radius, int knn, int* segment_starts, bool sortResults) : foundNeighbors(0),largestHeapDist(radius),k(knn), sorted(sortResults), radius_sq_(radius),segment_starts_(segment_starts) { }
+
+    //          __host__ __device__
+    //          KnnResultSet(const KnnResultSet& o):foundNeighbors(o.foundNeighbors),largestHeapDist(o.largestHeapDist),k(o.k){ }
+
+    __device__
+    inline DistanceType
+    worstDist()
+    {
+        return largestHeapDist;
+    }
+
+    __device__
+    inline void
+    insert(int index, DistanceType dist)
+    {
+        if( dist < radius_sq_ ) {
+            if( foundNeighbors<k ) {
+                resultDist[foundNeighbors]=dist;
+                resultIndex[foundNeighbors]=index;
+                if(( foundNeighbors==k-1) && useHeap) {
+                    if( useHeap ) {
+                        flann::cuda::heap::make_heap(resultDist,resultIndex,k,GreaterThan<DistanceType>());
+                        largestHeapDist=resultDist[0];
+                    }
+                    else {
+                        findLargestDistIndex();
+                    }
+                }
+                foundNeighbors++;
+
+            }
+            else if( dist < largestHeapDist ) {
+                if( useHeap ) {
+                    resultDist[0]=dist;
+                    resultIndex[0]=index;
+                    flann::cuda::heap::sift_down(resultDist,resultIndex,0,k,GreaterThan<DistanceType>());
+                    largestHeapDist=resultDist[0];
+                }
+                else {
+                    resultDist[maxDistElem]=dist;
+                    resultIndex[maxDistElem]=index;
+                    findLargestDistIndex();
+                }
+            }
+        }
+    }
+
+    __device__
+    void
+    findLargestDistIndex( )
+    {
+        largestHeapDist=resultDist[0];
+        maxDistElem=0;
+        for( int i=1; i<k; i++ )
+            if( resultDist[i] > largestHeapDist ) {
+                maxDistElem=i;
+                largestHeapDist=resultDist[i];
+            }
+    }
+
+
+    DistanceType* resultDist;
+    int* resultIndex;
+
+    __device__
+    inline void
+    setResultLocation( DistanceType* dists, int* index, int thread, int /*stride*/ )
+    {
+        resultDist=dists+segment_starts_[thread];
+        resultIndex=index+segment_starts_[thread];
+    }
+
+    __device__
+    inline void
+    finish()
+    {
+        if( sorted ) {
+            if( !useHeap ) flann::cuda::heap::make_heap(resultDist,resultIndex,k,GreaterThan<DistanceType>());
+            for( int i=foundNeighbors-1; i>0; i-- ) {
+                flann::cuda::swap( resultDist[0], resultDist[i] );
+                flann::cuda::swap( resultIndex[0], resultIndex[i] );
+                flann::cuda::heap::sift_down( resultDist,resultIndex, 0, i, GreaterThan<DistanceType>() );
+            }
+        }
+    }
+};
+
+// Difference to RadiusKnnResultSet: Works like KnnResultSet, doesn't pack the results densely (as the RadiusResultSet does)
+template <typename DistanceType, bool useHeap>
+struct KnnRadiusResultSet
+{
+    int foundNeighbors;
+    DistanceType largestHeapDist;
+    int maxDistIndex;
+    const int k;
+    const bool sorted;
+    const DistanceType epsError;
+    const DistanceType radius_sq;
+
+
+    __device__ __host__
+    KnnRadiusResultSet(int knn, bool sortResults, DistanceType eps, DistanceType radius) : foundNeighbors(0),largestHeapDist(radius),k(knn), sorted(sortResults), epsError(eps),radius_sq(radius){ }
+
+    //          __host__ __device__
+    //          KnnResultSet(const KnnResultSet& o):foundNeighbors(o.foundNeighbors),largestHeapDist(o.largestHeapDist),k(o.k){ }
+
+    __device__
+    inline DistanceType
+    worstDist()
+    {
+        return largestHeapDist;
+    }
+
+    __device__
+    inline void
+    insert(int index, DistanceType dist)
+    {
+        if( dist < largestHeapDist ) {
+            if( foundNeighbors<k ) {
+                resultDist[foundNeighbors]=dist;
+                resultIndex[foundNeighbors]=index;
+                if( foundNeighbors==k-1 ) {
+                    if( useHeap ) {
+                        flann::cuda::heap::make_heap(resultDist,resultIndex,k,GreaterThan<DistanceType>());
+                        largestHeapDist=resultDist[0];
+                    }
+                    else {
+                        findLargestDistIndex();
+                    }
+                }
+                foundNeighbors++;
+            }
+            else { //if( dist < largestHeapDist )
+                if( useHeap ) {
+                    resultDist[0]=dist;
+                    resultIndex[0]=index;
+                    flann::cuda::heap::sift_down(resultDist,resultIndex,0,k,GreaterThan<DistanceType>());
+                    largestHeapDist=resultDist[0];
+                }
+                else {
+                    resultDist[maxDistIndex]=dist;
+                    resultIndex[maxDistIndex]=index;
+                    findLargestDistIndex();
+                }
+            }
+        }
+    }
+    __device__
+    void
+    findLargestDistIndex( )
+    {
+        largestHeapDist=resultDist[0];
+        maxDistIndex=0;
+        for( int i=1; i<k; i++ )
+            if( resultDist[i] > largestHeapDist ) {
+                maxDistIndex=i;
+                largestHeapDist=resultDist[i];
+            }
+    }
+
+    DistanceType* resultDist;
+    int* resultIndex;
+
+    __device__
+    inline void
+    setResultLocation( DistanceType* dists, int* index, int thread, int stride )
+    {
+        resultDist=dists+stride*thread;
+        resultIndex=index+stride*thread;
+        for( int i=0; i<stride; i++ ) {
+            resultDist[i]=INFINITY;
+            resultIndex[i]=-1;
+            //                  resultIndex[tid+i*blockDim.x]=-1;
+            //                  resultDist[tid+i*blockDim.x]=INFINITY;
+        }
+    }
+
+    __device__
+    inline void
+    finish()
+    {
+        if( sorted ) {
+            if( !useHeap ) flann::cuda::heap::make_heap(resultDist,resultIndex,k,GreaterThan<DistanceType>());
+            for( int i=k-1; i>0; i-- ) {
+                flann::cuda::swap( resultDist[0], resultDist[i] );
+                flann::cuda::swap( resultIndex[0], resultIndex[i] );
+                flann::cuda::heap::sift_down( resultDist,resultIndex, 0, i, GreaterThan<DistanceType>() );
+            }
+        }
+    }
+};
+
+//! fills the radius output buffer.
+//! IMPORTANT ASSERTION: ASSUMES THAT THERE IS ENOUGH SPACE FOR EVERY NEIGHBOR! IF THIS ISN'T
+//! TRUE, USE KnnRadiusResultSet! (Otherwise, the neighbors of one element might overflow into the next element, or past the buffer.)
+template< typename DistanceType >
+struct RadiusResultSet
+{
+    DistanceType radius_sq_;
+    int* segment_starts_;
+    int count_;
+    bool sorted_;
+
+    __device__ __host__
+    RadiusResultSet(DistanceType radius, int* segment_starts, bool sorted) : radius_sq_(radius), segment_starts_(segment_starts), count_(0), sorted_(sorted){ }
+
+    __device__
+    inline DistanceType
+    worstDist()
+    {
+        return radius_sq_;
+    }
+
+    __device__
+    inline void
+    insert(int index, DistanceType dist)
+    {
+        if( dist < radius_sq_ ) {
+            resultIndex[count_]=index;
+            resultDist[count_]=dist;
+            count_++;
+        }
+    }
+
+    int* resultIndex;
+    DistanceType* resultDist;
+
+    __device__
+    inline void
+    setResultLocation( DistanceType* dists, int* index, int thread, int /*stride*/ )
+    {
+        resultIndex=index+segment_starts_[thread];
+        resultDist=dists+segment_starts_[thread];
+    }
+
+    __device__
+    inline void
+    finish()
+    {
+        if( sorted_ ) {
+            flann::cuda::heap::make_heap( resultDist,resultIndex, count_, GreaterThan<DistanceType>() );
+            for( int i=count_-1; i>0; i-- ) {
+                flann::cuda::swap( resultDist[0], resultDist[i] );
+                flann::cuda::swap( resultIndex[0], resultIndex[i] );
+                flann::cuda::heap::sift_down( resultDist,resultIndex, 0, i, GreaterThan<DistanceType>() );
+            }
+        }
+    }
+};
+}
+}
+
+#endif

corelib/src/flann/util/dynamic_bitset.h

@@ -0,0 +1,163 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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.
+ *************************************************************************/
+
+/***********************************************************************
+ * Author: Vincent Rabaud
+ *************************************************************************/
+
+#ifndef FLANN_DYNAMIC_BITSET_H_
+#define FLANN_DYNAMIC_BITSET_H_
+
+//#define FLANN_USE_BOOST 1
+#if FLANN_USE_BOOST
+#include <boost/dynamic_bitset.hpp>
+typedef boost::dynamic_bitset<> DynamicBitset;
+#else
+
+#include <limits.h>
+
+namespace flann {
+
+/** Class re-implementing the boost version of it
+ * This helps not depending on boost, it also does not do the bound checks
+ * and has a way to reset a block for speed
+ */
+class DynamicBitset
+{
+public:
+    /** @param default constructor
+     */
+    DynamicBitset() : size_(0)
+    {
+    }
+
+    /** @param only constructor we use in our code
+     * @param the size of the bitset (in bits)
+     */
+    DynamicBitset(size_t size)
+    {
+        resize(size);
+        reset();
+    }
+
+    /** Sets all the bits to 0
+     */
+    void clear()
+    {
+        std::fill(bitset_.begin(), bitset_.end(), 0);
+    }
+
+    /** @brief checks if the bitset is empty
+     * @return true if the bitset is empty
+     */
+    bool empty() const
+    {
+        return bitset_.empty();
+    }
+
+    /** @param set all the bits to 0
+     */
+    void reset()
+    {
+        std::fill(bitset_.begin(), bitset_.end(), 0);
+    }
+
+    /** @brief set one bit to 0
+     * @param
+     */
+    void reset(size_t index)
+    {
+        bitset_[index / cell_bit_size_] &= ~(size_t(1) << (index % cell_bit_size_));
+    }
+
+    /** @brief sets a specific bit to 0, and more bits too
+     * This function is useful when resetting a given set of bits so that the
+     * whole bitset ends up being 0: if that's the case, we don't care about setting
+     * other bits to 0
+     * @param
+     */
+    void reset_block(size_t index)
+    {
+        bitset_[index / cell_bit_size_] = 0;
+    }
+
+    /** @param resize the bitset so that it contains at least size bits
+     * @param size
+     */
+    void resize(size_t size)
+    {
+        size_ = size;
+        bitset_.resize(size / cell_bit_size_ + 1);
+    }
+
+    /** @param set a bit to true
+     * @param index the index of the bit to set to 1
+     */
+    void set(size_t index)
+    {
+        bitset_[index / cell_bit_size_] |= size_t(1) << (index % cell_bit_size_);
+    }
+
+    /** @param gives the number of contained bits
+     */
+    size_t size() const
+    {
+        return size_;
+    }
+
+    /** @param check if a bit is set
+     * @param index the index of the bit to check
+     * @return true if the bit is set
+     */
+    bool test(size_t index) const
+    {
+        return (bitset_[index / cell_bit_size_] & (size_t(1) << (index % cell_bit_size_))) != 0;
+    }
+
+private:
+    template <typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar & size_;
+    	ar & bitset_;
+    }
+    friend struct serialization::access;
+
+private:
+    std::vector<size_t> bitset_;
+    size_t size_;
+    static const unsigned int cell_bit_size_ = CHAR_BIT * sizeof(size_t);
+};
+
+} // namespace flann
+
+#endif
+
+#endif // FLANN_DYNAMIC_BITSET_H_

corelib/src/flann/util/heap.h

@@ -0,0 +1,456 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_HEAP_H_
+#define FLANN_HEAP_H_
+
+#include <algorithm>
+#include <vector>
+
+namespace flann
+{
+
+/**
+ * Priority Queue Implementation
+ *
+ * The priority queue is implemented with a heap.  A heap is a complete
+ * (full) binary tree in which each parent is less than both of its
+ * children, but the order of the children is unspecified.
+ */
+template <typename T>
+class Heap
+{
+
+    /**
+     * Storage array for the heap.
+     * Type T must be comparable.
+     */
+    std::vector<T> heap;
+    int length;
+
+    /**
+     * Number of element in the heap
+     */
+    int count;
+
+
+
+public:
+    /**
+     * Constructor.
+     *
+     * Params:
+     *     size = heap size
+     */
+
+    Heap(int size)
+    {
+        length = size;
+        heap.reserve(length);
+        count = 0;
+    }
+
+    /**
+     *
+     * Returns: heap size
+     */
+    int size()
+    {
+        return count;
+    }
+
+    /**
+     * Tests if the heap is empty
+     *
+     * Returns: true is heap empty, false otherwise
+     */
+    bool empty()
+    {
+        return size()==0;
+    }
+
+    /**
+     * Clears the heap.
+     */
+    void clear()
+    {
+        heap.clear();
+        count = 0;
+    }
+
+    struct CompareT : public std::binary_function<T,T,bool>
+    {
+        bool operator()(const T& t_1, const T& t_2) const
+        {
+            return t_2 < t_1;
+        }
+    };
+
+    /**
+     * Insert a new element in the heap.
+     *
+     * We select the next empty leaf node, and then keep moving any larger
+     * parents down until the right location is found to store this element.
+     *
+     * Params:
+     *     value = the new element to be inserted in the heap
+     */
+    void insert(const T& value)
+    {
+        /* If heap is full, then return without adding this element. */
+        if (count == length) {
+            return;
+        }
+
+        heap.push_back(value);
+        static CompareT compareT;
+        std::push_heap(heap.begin(), heap.end(), compareT);
+        ++count;
+    }
+
+
+
+    /**
+     * Returns the node of minimum value from the heap (top of the heap).
+     *
+     * Params:
+     *     value = out parameter used to return the min element
+     * Returns: false if heap empty
+     */
+    bool popMin(T& value)
+    {
+        if (count == 0) {
+            return false;
+        }
+
+        value = heap[0];
+        static CompareT compareT;
+        std::pop_heap(heap.begin(), heap.end(), compareT);
+        heap.pop_back();
+        --count;
+
+        return true;  /* Return old last node. */
+    }
+};
+
+
+template <typename T>
+class IntervalHeap
+{
+	struct Interval
+	{
+		T left;
+		T right;
+	};
+
+    /**
+     * Storage array for the heap.
+     * Type T must be comparable.
+     */
+    std::vector<Interval> heap;
+    size_t capacity_;
+    size_t size_;
+
+public:
+    /**
+     * Constructor.
+     *
+     * Params:
+     *     size = heap size
+     */
+
+    IntervalHeap(int capacity) : capacity_(capacity), size_(0)
+    {
+        heap.resize(capacity/2 + capacity%2 + 1); // 1-based indexing
+    }
+
+    /**
+     * @return Heap size
+     */
+    size_t size()
+    {
+        return size_;
+    }
+
+    /**
+     * Tests if the heap is empty
+     * @return true is heap empty, false otherwise
+     */
+    bool empty()
+    {
+        return size_==0;
+    }
+
+    /**
+     * Clears the heap.
+     */
+    void clear()
+    {
+        size_ = 0;
+    }
+
+    void insert(const T& value)
+    {
+        /* If heap is full, then return without adding this element. */
+        if (size_ == capacity_) {
+            return;
+        }
+
+        // insert into the root
+        if (size_<2) {
+        	if (size_==0) {
+        		heap[1].left = value;
+        		heap[1].right = value;
+        	}
+        	else {
+        		if (value<heap[1].left) {
+        			heap[1].left = value;
+        		}
+        		else {
+        			heap[1].right = value;
+        		}
+        	}
+        	++size_;
+        	return;
+        }
+
+        size_t last_pos = size_/2 + size_%2;
+        bool min_heap;
+
+        if (size_%2) { // odd number of elements
+        	min_heap = (value<heap[last_pos].left)? true : false;
+        }
+        else {
+        	++last_pos;
+        	min_heap = (value<heap[last_pos/2].left)? true : false;
+        }
+
+        if (min_heap) {
+        	size_t pos = last_pos;
+        	size_t par = pos/2;
+        	while (pos>1 && value < heap[par].left) {
+        		heap[pos].left = heap[par].left;
+        		pos = par;
+        		par = pos/2;
+        	}
+        	heap[pos].left = value;
+        	++size_;
+
+        	if (size_%2) { // duplicate element in last position if size is odd
+        		heap[last_pos].right = heap[last_pos].left;
+        	}
+        }
+        else {
+        	size_t pos = last_pos;
+        	size_t par = pos/2;
+        	while (pos>1 && heap[par].right < value) {
+        		heap[pos].right = heap[par].right;
+        		pos = par;
+        		par = pos/2;
+        	}
+        	heap[pos].right = value;
+        	++size_;
+
+        	if (size_%2) { // duplicate element in last position if size is odd
+        		heap[last_pos].left = heap[last_pos].right;
+        	}
+        }
+    }
+
+
+    /**
+     * Returns the node of minimum value from the heap
+     * @param value out parameter used to return the min element
+     * @return false if heap empty
+     */
+    bool popMin(T& value)
+    {
+        if (size_ == 0) {
+            return false;
+        }
+
+        value = heap[1].left;
+        size_t last_pos = size_/2 + size_%2;
+        T elem = heap[last_pos].left;
+
+        if (size_ % 2) { // odd number of elements
+        	--last_pos;
+        }
+        else {
+        	heap[last_pos].left = heap[last_pos].right;
+        }
+        --size_;
+        if (size_<2) return true;
+
+        size_t crt=1; // root node
+        size_t child = crt*2;
+
+        while (child <= last_pos) {
+        	if (child < last_pos && heap[child+1].left < heap[child].left) ++child; // pick the child with min
+
+        	if (!(heap[child].left<elem)) break;
+
+        	heap[crt].left = heap[child].left;
+        	if (heap[child].right<elem) {
+        		std::swap(elem, heap[child].right);
+        	}
+
+        	crt = child;
+    		child *= 2;
+        }
+        heap[crt].left = elem;
+        return true;
+    }
+
+
+    /**
+     * Returns the element of maximum value from the heap
+     * @param value
+     * @return false if heap empty
+     */
+    bool popMax(T& value)
+    {
+        if (size_ == 0) {
+            return false;
+        }
+
+        value = heap[1].right;
+        size_t last_pos = size_/2 + size_%2;
+        T elem = heap[last_pos].right;
+
+        if (size_%2) { // odd number of elements
+        	--last_pos;
+        }
+        else {
+        	heap[last_pos].right = heap[last_pos].left;
+        }
+        --size_;
+        if (size_<2) return true;
+
+        size_t crt=1; // root node
+        size_t child = crt*2;
+
+        while (child <= last_pos) {
+        	if (child < last_pos && heap[child].right < heap[child+1].right) ++child; // pick the child with max
+
+        	if (!(elem < heap[child].right)) break;
+
+        	heap[crt].right = heap[child].right;
+        	if (elem<heap[child].left) {
+        		std::swap(elem, heap[child].left);
+        	}
+
+        	crt = child;
+    		child *= 2;
+        }
+        heap[crt].right = elem;
+        return true;
+    }
+
+
+    bool getMin(T& value)
+    {
+    	if (size_==0) {
+    		return false;
+    	}
+    	value = heap[1].left;
+    	return true;
+    }
+
+
+    bool getMax(T& value)
+    {
+    	if (size_==0) {
+    		return false;
+    	}
+    	value = heap[1].right;
+    	return true;
+    }
+};
+
+
+template <typename T>
+class BoundedHeap
+{
+	IntervalHeap<T> interval_heap_;
+	size_t capacity_;
+public:
+	BoundedHeap(size_t capacity) : interval_heap_(capacity), capacity_(capacity)
+	{
+
+	}
+
+    /**
+     * Returns: heap size
+     */
+    int size()
+    {
+        return interval_heap_.size();
+    }
+
+    /**
+     * Tests if the heap is empty
+     * Returns: true is heap empty, false otherwise
+     */
+    bool empty()
+    {
+        return interval_heap_.empty();
+    }
+
+    /**
+     * Clears the heap.
+     */
+    void clear()
+    {
+    	interval_heap_.clear();
+    }
+
+    void insert(const T& value)
+    {
+    	if (interval_heap_.size()==capacity_) {
+    		T max;
+    		interval_heap_.getMax(max);
+    		if (max<value) return;
+   			interval_heap_.popMax(max);
+    	}
+    	interval_heap_.insert(value);
+    }
+
+    bool popMin(T& value)
+    {
+    	return interval_heap_.popMin(value);
+    }
+};
+
+
+
+}
+
+#endif //FLANN_HEAP_H_

corelib/src/flann/util/logger.h

@@ -0,0 +1,137 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_LOGGER_H
+#define FLANN_LOGGER_H
+
+#include <stdio.h>
+#include <stdarg.h>
+
+#include "flann/defines.h"
+
+
+namespace flann
+{
+
+class Logger
+{
+    Logger() : stream(stdout), logLevel(FLANN_LOG_WARN) {}
+
+    ~Logger()
+    {
+        if ((stream!=NULL)&&(stream!=stdout)) {
+            fclose(stream);
+        }
+    }
+
+    static Logger& instance()
+    {
+        static Logger logger;
+        return logger;
+    }
+
+    void _setDestination(const char* name)
+    {
+        if (name==NULL) {
+            stream = stdout;
+        }
+        else {
+            stream = fopen(name,"w");
+            if (stream == NULL) {
+                stream = stdout;
+            }
+        }
+    }
+
+    int _log(int level, const char* fmt, va_list arglist)
+    {
+        if (level > logLevel ) return -1;
+        int ret = vfprintf(stream, fmt, arglist);
+        return ret;
+    }
+
+public:
+    /**
+     * Sets the logging level. All messages with lower priority will be ignored.
+     * @param level Logging level
+     */
+    static void setLevel(int level) { instance().logLevel = level; }
+
+    /**
+     * Returns the currently set logging level.
+     * @return current logging level
+     */
+    static int getLevel() { return instance().logLevel; }
+
+    /**
+     * Sets the logging destination
+     * @param name Filename or NULL for console
+     */
+    static void setDestination(const char* name) { instance()._setDestination(name); }
+
+    /**
+     * Print log message
+     * @param level Log level
+     * @param fmt Message format
+     * @return
+     */
+    static int log(int level, const char* fmt, ...)
+    {
+        va_list arglist;
+        va_start(arglist, fmt);
+        int ret = instance()._log(level,fmt,arglist);
+        va_end(arglist);
+        return ret;
+    }
+
+#define LOG_METHOD(NAME,LEVEL) \
+    static int NAME(const char* fmt, ...) \
+    { \
+        va_list ap; \
+        va_start(ap, fmt); \
+        int ret = instance()._log(LEVEL, fmt, ap); \
+        va_end(ap); \
+        return ret; \
+    }
+
+    LOG_METHOD(fatal, FLANN_LOG_FATAL)
+    LOG_METHOD(error, FLANN_LOG_ERROR)
+    LOG_METHOD(warn, FLANN_LOG_WARN)
+    LOG_METHOD(info, FLANN_LOG_INFO)
+    LOG_METHOD(debug, FLANN_LOG_DEBUG)
+
+private:
+    FILE* stream;
+    int logLevel;
+};
+
+}
+
+#endif //FLANN_LOGGER_H

corelib/src/flann/util/lsh_table.h

@@ -0,0 +1,506 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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.
+ *************************************************************************/
+
+/***********************************************************************
+ * Author: Vincent Rabaud
+ *************************************************************************/
+
+#ifndef FLANN_LSH_TABLE_H_
+#define FLANN_LSH_TABLE_H_
+
+#include <algorithm>
+#include <iostream>
+#include <iomanip>
+#include <limits.h>
+// TODO as soon as we use C++0x, use the code in USE_UNORDERED_MAP
+#if USE_UNORDERED_MAP
+#include <unordered_map>
+#else
+#include <map>
+#endif
+#include <math.h>
+#include <stddef.h>
+
+#include "flann/util/dynamic_bitset.h"
+#include "flann/util/matrix.h"
+
+namespace flann
+{
+
+namespace lsh
+{
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** What is stored in an LSH bucket
+ */
+typedef uint32_t FeatureIndex;
+/** The id from which we can get a bucket back in an LSH table
+ */
+typedef unsigned int BucketKey;
+
+/** A bucket in an LSH table
+ */
+typedef std::vector<FeatureIndex> Bucket;
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** POD for stats about an LSH table
+ */
+struct LshStats
+{
+    std::vector<unsigned int> bucket_sizes_;
+    size_t n_buckets_;
+    size_t bucket_size_mean_;
+    size_t bucket_size_median_;
+    size_t bucket_size_min_;
+    size_t bucket_size_max_;
+    size_t bucket_size_std_dev;
+    /** Each contained vector contains three value: beginning/end for interval, number of elements in the bin
+     */
+    std::vector<std::vector<unsigned int> > size_histogram_;
+};
+
+/** Overload the << operator for LshStats
+ * @param out the streams
+ * @param stats the stats to display
+ * @return the streams
+ */
+inline std::ostream& operator <<(std::ostream& out, const LshStats& stats)
+{
+    size_t w = 20;
+    out << "Lsh Table Stats:\n" << std::setw(w) << std::setiosflags(std::ios::right) << "N buckets : "
+    << stats.n_buckets_ << "\n" << std::setw(w) << std::setiosflags(std::ios::right) << "mean size : "
+    << std::setiosflags(std::ios::left) << stats.bucket_size_mean_ << "\n" << std::setw(w)
+    << std::setiosflags(std::ios::right) << "median size : " << stats.bucket_size_median_ << "\n" << std::setw(w)
+    << std::setiosflags(std::ios::right) << "min size : " << std::setiosflags(std::ios::left)
+    << stats.bucket_size_min_ << "\n" << std::setw(w) << std::setiosflags(std::ios::right) << "max size : "
+    << std::setiosflags(std::ios::left) << stats.bucket_size_max_;
+
+    // Display the histogram
+    out << std::endl << std::setw(w) << std::setiosflags(std::ios::right) << "histogram : "
+    << std::setiosflags(std::ios::left);
+    for (std::vector<std::vector<unsigned int> >::const_iterator iterator = stats.size_histogram_.begin(), end =
+             stats.size_histogram_.end(); iterator != end; ++iterator) out << (*iterator)[0] << "-" << (*iterator)[1] << ": " << (*iterator)[2] << ",  ";
+
+    return out;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Lsh hash table. As its key is a sub-feature, and as usually
+ * the size of it is pretty small, we keep it as a continuous memory array.
+ * The value is an index in the corpus of features (we keep it as an unsigned
+ * int for pure memory reasons, it could be a size_t)
+ */
+template<typename ElementType>
+class LshTable
+{
+public:
+    /** A container of all the feature indices. Optimized for space
+     */
+#if USE_UNORDERED_MAP
+    typedef std::unordered_map<BucketKey, Bucket> BucketsSpace;
+#else
+    typedef std::map<BucketKey, Bucket> BucketsSpace;
+#endif
+
+    /** A container of all the feature indices. Optimized for speed
+     */
+    typedef std::vector<Bucket> BucketsSpeed;
+
+    /** Default constructor
+     */
+    LshTable()
+    {
+    }
+
+    /** Default constructor
+     * Create the mask and allocate the memory
+     * @param feature_size is the size of the feature (considered as a ElementType[])
+     * @param key_size is the number of bits that are turned on in the feature
+     */
+    LshTable(unsigned int /*feature_size*/, unsigned int /*key_size*/)
+    {
+        std::cerr << "LSH is not implemented for that type" << std::endl;
+        throw;
+    }
+
+    /** Add a feature to the table
+     * @param value the value to store for that feature
+     * @param feature the feature itself
+     */
+    void add(unsigned int value, const ElementType* feature)
+    {
+        // Add the value to the corresponding bucket
+        BucketKey key = getKey(feature);
+
+        switch (speed_level_) {
+        case kArray:
+            // That means we get the buckets from an array
+            buckets_speed_[key].push_back(value);
+            break;
+        case kBitsetHash:
+            // That means we can check the bitset for the presence of a key
+            key_bitset_.set(key);
+            buckets_space_[key].push_back(value);
+            break;
+        case kHash:
+        {
+            // That means we have to check for the hash table for the presence of a key
+            buckets_space_[key].push_back(value);
+            break;
+        }
+        }
+    }
+
+    /** Add a set of features to the table
+     * @param dataset the values to store
+     */
+    void add(const std::vector< std::pair<size_t, ElementType*> >& features)
+    {
+#if USE_UNORDERED_MAP
+        buckets_space_.rehash((buckets_space_.size() + features.size()) * 1.2);
+#endif
+        // Add the features to the table
+        for (size_t i = 0; i < features.size(); ++i) {
+        	add(features[i].first, features[i].second);
+        }
+        // Now that the table is full, optimize it for speed/space
+        optimize();
+    }
+
+    /** Get a bucket given the key
+     * @param key
+     * @return
+     */
+    inline const Bucket* getBucketFromKey(BucketKey key) const
+    {
+        // Generate other buckets
+        switch (speed_level_) {
+        case kArray:
+            // That means we get the buckets from an array
+            return &buckets_speed_[key];
+            break;
+        case kBitsetHash:
+            // That means we can check the bitset for the presence of a key
+            if (key_bitset_.test(key)) return &buckets_space_.find(key)->second;
+            else return 0;
+            break;
+        case kHash:
+        {
+            // That means we have to check for the hash table for the presence of a key
+            BucketsSpace::const_iterator bucket_it, bucket_end = buckets_space_.end();
+            bucket_it = buckets_space_.find(key);
+            // Stop here if that bucket does not exist
+            if (bucket_it == bucket_end) return 0;
+            else return &bucket_it->second;
+            break;
+        }
+        }
+        return 0;
+    }
+
+    /** Compute the sub-signature of a feature
+     */
+    size_t getKey(const ElementType* /*feature*/) const
+    {
+        std::cerr << "LSH is not implemented for that type" << std::endl;
+        throw;
+        return 1;
+    }
+
+    /** Get statistics about the table
+     * @return
+     */
+    LshStats getStats() const;
+
+private:
+    /** defines the speed fo the implementation
+     * kArray uses a vector for storing data
+     * kBitsetHash uses a hash map but checks for the validity of a key with a bitset
+     * kHash uses a hash map only
+     */
+    enum SpeedLevel
+    {
+        kArray, kBitsetHash, kHash
+    };
+
+    /** Initialize some variables
+     */
+    void initialize(size_t key_size)
+    {
+        speed_level_ = kHash;
+        key_size_ = key_size;
+    }
+
+    /** Optimize the table for speed/space
+     */
+    void optimize()
+    {
+        // If we are already using the fast storage, no need to do anything
+        if (speed_level_ == kArray) return;
+
+        // Use an array if it will be more than half full
+        if (buckets_space_.size() > ((size_t(1) << key_size_) / 2)) {
+            speed_level_ = kArray;
+            // Fill the array version of it
+            buckets_speed_.resize(size_t(1) << key_size_);
+            for (BucketsSpace::const_iterator key_bucket = buckets_space_.begin(); key_bucket != buckets_space_.end(); ++key_bucket) buckets_speed_[key_bucket->first] = key_bucket->second;
+
+            // Empty the hash table
+            buckets_space_.clear();
+            return;
+        }
+
+        // If the bitset is going to use less than 10% of the RAM of the hash map (at least 1 size_t for the key and two
+        // for the vector) or less than 512MB (key_size_ <= 30)
+        if (((std::max(buckets_space_.size(), buckets_speed_.size()) * CHAR_BIT * 3 * sizeof(BucketKey)) / 10
+             >= size_t(size_t(1) << key_size_)) || (key_size_ <= 32)) {
+            speed_level_ = kBitsetHash;
+            key_bitset_.resize(size_t(1) << key_size_);
+            key_bitset_.reset();
+            // Try with the BucketsSpace
+            for (BucketsSpace::const_iterator key_bucket = buckets_space_.begin(); key_bucket != buckets_space_.end(); ++key_bucket) key_bitset_.set(key_bucket->first);
+        }
+        else {
+            speed_level_ = kHash;
+            key_bitset_.clear();
+        }
+    }
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	int val;
+    	if (Archive::is_saving::value) {
+    		val = (int)speed_level_;
+    	}
+    	ar & val;
+    	if (Archive::is_loading::value) {
+    		speed_level_ = (SpeedLevel) val;
+    	}
+
+    	ar & key_size_;
+    	ar & mask_;
+
+    	if (speed_level_==kArray) {
+    		ar & buckets_speed_;
+    	}
+    	if (speed_level_==kBitsetHash || speed_level_==kHash) {
+    		ar & buckets_space_;
+    	}
+		if (speed_level_==kBitsetHash) {
+			ar & key_bitset_;
+		}
+    }
+    friend struct serialization::access;
+
+    /** The vector of all the buckets if they are held for speed
+     */
+    BucketsSpeed buckets_speed_;
+
+    /** The hash table of all the buckets in case we cannot use the speed version
+     */
+    BucketsSpace buckets_space_;
+
+    /** What is used to store the data */
+    SpeedLevel speed_level_;
+
+    /** If the subkey is small enough, it will keep track of which subkeys are set through that bitset
+     * That is just a speedup so that we don't look in the hash table (which can be mush slower that checking a bitset)
+     */
+    DynamicBitset key_bitset_;
+
+    /** The size of the sub-signature in bits
+     */
+    unsigned int key_size_;
+
+    // Members only used for the unsigned char specialization
+    /** The mask to apply to a feature to get the hash key
+     * Only used in the unsigned char case
+     */
+    std::vector<size_t> mask_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Specialization for unsigned char
+
+template<>
+inline LshTable<unsigned char>::LshTable(unsigned int feature_size, unsigned int subsignature_size)
+{
+    initialize(subsignature_size);
+    // Allocate the mask
+    mask_ = std::vector<size_t>((size_t)ceil((float)(feature_size * sizeof(char)) / (float)sizeof(size_t)), 0);
+
+    // A bit brutal but fast to code
+    std::vector<size_t> indices(feature_size * CHAR_BIT);
+    for (size_t i = 0; i < feature_size * CHAR_BIT; ++i) indices[i] = i;
+    std::random_shuffle(indices.begin(), indices.end());
+
+    // Generate a random set of order of subsignature_size_ bits
+    for (unsigned int i = 0; i < key_size_; ++i) {
+        size_t index = indices[i];
+
+        // Set that bit in the mask
+        size_t divisor = CHAR_BIT * sizeof(size_t);
+        size_t idx = index / divisor; //pick the right size_t index
+        mask_[idx] |= size_t(1) << (index % divisor); //use modulo to find the bit offset
+    }
+
+    // Set to 1 if you want to display the mask for debug
+#if 0
+    {
+        size_t bcount = 0;
+        BOOST_FOREACH(size_t mask_block, mask_){
+            out << std::setw(sizeof(size_t) * CHAR_BIT / 4) << std::setfill('0') << std::hex << mask_block
+                << std::endl;
+            bcount += __builtin_popcountll(mask_block);
+        }
+        out << "bit count : " << std::dec << bcount << std::endl;
+        out << "mask size : " << mask_.size() << std::endl;
+        return out;
+    }
+#endif
+}
+
+/** Return the Subsignature of a feature
+ * @param feature the feature to analyze
+ */
+template<>
+inline size_t LshTable<unsigned char>::getKey(const unsigned char* feature) const
+{
+    // no need to check if T is dividable by sizeof(size_t) like in the Hamming
+    // distance computation as we have a mask
+    const size_t* feature_block_ptr = reinterpret_cast<const size_t*> (feature);
+
+    // Figure out the subsignature of the feature
+    // Given the feature ABCDEF, and the mask 001011, the output will be
+    // 000CEF
+    size_t subsignature = 0;
+    size_t bit_index = 1;
+
+    for (std::vector<size_t>::const_iterator pmask_block = mask_.begin(); pmask_block != mask_.end(); ++pmask_block) {
+        // get the mask and signature blocks
+        size_t feature_block = *feature_block_ptr;
+        size_t mask_block = *pmask_block;
+        while (mask_block) {
+            // Get the lowest set bit in the mask block
+            size_t lowest_bit = mask_block & (-(ptrdiff_t)mask_block);
+            // Add it to the current subsignature if necessary
+            subsignature += (feature_block & lowest_bit) ? bit_index : 0;
+            // Reset the bit in the mask block
+            mask_block ^= lowest_bit;
+            // increment the bit index for the subsignature
+            bit_index <<= 1;
+        }
+        // Check the next feature block
+        ++feature_block_ptr;
+    }
+    return subsignature;
+}
+
+template<>
+inline LshStats LshTable<unsigned char>::getStats() const
+{
+    LshStats stats;
+    stats.bucket_size_mean_ = 0;
+    if ((buckets_speed_.empty()) && (buckets_space_.empty())) {
+        stats.n_buckets_ = 0;
+        stats.bucket_size_median_ = 0;
+        stats.bucket_size_min_ = 0;
+        stats.bucket_size_max_ = 0;
+        return stats;
+    }
+
+    if (!buckets_speed_.empty()) {
+        for (BucketsSpeed::const_iterator pbucket = buckets_speed_.begin(); pbucket != buckets_speed_.end(); ++pbucket) {
+            stats.bucket_sizes_.push_back(pbucket->size());
+            stats.bucket_size_mean_ += pbucket->size();
+        }
+        stats.bucket_size_mean_ /= buckets_speed_.size();
+        stats.n_buckets_ = buckets_speed_.size();
+    }
+    else {
+        for (BucketsSpace::const_iterator x = buckets_space_.begin(); x != buckets_space_.end(); ++x) {
+            stats.bucket_sizes_.push_back(x->second.size());
+            stats.bucket_size_mean_ += x->second.size();
+        }
+        stats.bucket_size_mean_ /= buckets_space_.size();
+        stats.n_buckets_ = buckets_space_.size();
+    }
+
+    std::sort(stats.bucket_sizes_.begin(), stats.bucket_sizes_.end());
+
+    //  BOOST_FOREACH(int size, stats.bucket_sizes_)
+    //          std::cout << size << " ";
+    //  std::cout << std::endl;
+    stats.bucket_size_median_ = stats.bucket_sizes_[stats.bucket_sizes_.size() / 2];
+    stats.bucket_size_min_ = stats.bucket_sizes_.front();
+    stats.bucket_size_max_ = stats.bucket_sizes_.back();
+
+    // TODO compute mean and std
+    /*float mean, stddev;
+       stats.bucket_size_mean_ = mean;
+       stats.bucket_size_std_dev = stddev;*/
+
+    // Include a histogram of the buckets
+    unsigned int bin_start = 0;
+    unsigned int bin_end = 20;
+    bool is_new_bin = true;
+    for (std::vector<unsigned int>::iterator iterator = stats.bucket_sizes_.begin(), end = stats.bucket_sizes_.end(); iterator
+         != end; )
+        if (*iterator < bin_end) {
+            if (is_new_bin) {
+                stats.size_histogram_.push_back(std::vector<unsigned int>(3, 0));
+                stats.size_histogram_.back()[0] = bin_start;
+                stats.size_histogram_.back()[1] = bin_end - 1;
+                is_new_bin = false;
+            }
+            ++stats.size_histogram_.back()[2];
+            ++iterator;
+        }
+        else {
+            bin_start += 20;
+            bin_end += 20;
+            is_new_bin = true;
+        }
+
+    return stats;
+}
+
+// End the two namespaces
+}
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#endif /* FLANN_LSH_TABLE_H_ */

corelib/src/flann/util/matrix.h

@@ -0,0 +1,135 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_DATASET_H_
+#define FLANN_DATASET_H_
+
+#include "flann/general.h"
+#include "flann/util/serialization.h"
+#include <stdio.h>
+
+namespace flann
+{
+
+typedef unsigned char uchar;
+
+class Matrix_
+{
+public:
+
+	Matrix_() : rows(0), cols(0), stride(0), type(FLANN_NONE), data(NULL)
+	{
+	};
+
+    Matrix_(void* data_, size_t rows_, size_t cols_, flann_datatype_t type_, size_t stride_ = 0) :
+        rows(rows_), cols(cols_), stride(stride_), type(type_)
+    {
+    	data = static_cast<uchar*>(data_);
+
+    	if (stride==0) stride = flann_datatype_size(type)*cols;
+    }
+
+    /**
+     * Operator that returns a (pointer to a) row of the data.
+     */
+    inline void* operator[](size_t index) const
+    {
+        return data+index*stride;
+    }
+
+    void* ptr() const
+    {
+        return data;
+    }
+
+    size_t rows;
+    size_t cols;
+    size_t stride;
+    flann_datatype_t type;
+protected:
+    uchar* data;
+
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+    	ar & rows;
+    	ar & cols;
+    	ar & stride;
+    	ar & type;
+    	if (Archive::is_loading::value) {
+    		data = new uchar[rows*stride];
+    	}
+    	ar & serialization::make_binary_object(data, rows*stride);
+    }
+    friend struct serialization::access;
+};
+
+
+/**
+ * Class that implements a simple rectangular matrix stored in a memory buffer and
+ * provides convenient matrix-like access using the [] operators.
+ *
+ * This class has the same memory structure as the un-templated class flann::Matrix_ and
+ * it's directly convertible from it.
+ */
+template <typename T>
+class Matrix : public Matrix_
+{
+public:
+    typedef T type;
+
+    Matrix() : Matrix_()
+    {
+    }
+
+    Matrix(T* data_, size_t rows_, size_t cols_, size_t stride_ = 0) :
+    	Matrix_(data_, rows_, cols_, flann_datatype_value<T>::value, stride_)
+    {
+    	if (stride==0) stride = sizeof(T)*cols;
+    }
+
+    /**
+     * Operator that returns a (pointer to a) row of the data.
+     */
+    inline T* operator[](size_t index) const
+    {
+    	return reinterpret_cast<T*>(data+index*stride);
+    }
+
+
+    T* ptr() const
+    {
+    	return reinterpret_cast<T*>(data);
+    }
+};
+
+}
+
+#endif //FLANN_DATASET_H_

corelib/src/flann/util/object_factory.h

@@ -0,0 +1,91 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_OBJECT_FACTORY_H_
+#define FLANN_OBJECT_FACTORY_H_
+
+#include <map>
+
+namespace flann
+{
+
+class CreatorNotFound
+{
+};
+
+template<typename BaseClass,
+         typename UniqueIdType,
+         typename ObjectCreator = BaseClass* (*)()>
+class ObjectFactory
+{
+    typedef ObjectFactory<BaseClass,UniqueIdType,ObjectCreator> ThisClass;
+    typedef std::map<UniqueIdType, ObjectCreator> ObjectRegistry;
+
+    // singleton class, private constructor
+    ObjectFactory() {}
+
+public:
+
+    bool subscribe(UniqueIdType id, ObjectCreator creator)
+    {
+        if (object_registry.find(id) != object_registry.end()) return false;
+
+        object_registry[id] = creator;
+        return true;
+    }
+
+    bool unregister(UniqueIdType id)
+    {
+        return object_registry.erase(id) == 1;
+    }
+
+    ObjectCreator create(UniqueIdType id)
+    {
+        typename ObjectRegistry::const_iterator iter = object_registry.find(id);
+
+        if (iter == object_registry.end()) {
+            throw CreatorNotFound();
+        }
+
+        return iter->second;
+    }
+
+    static ThisClass& instance()
+    {
+        static ThisClass the_factory;
+        return the_factory;
+    }
+private:
+    ObjectRegistry object_registry;
+};
+
+}
+
+#endif /* FLANN_OBJECT_FACTORY_H_ */

corelib/src/flann/util/params.h

@@ -0,0 +1,139 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2011  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2011  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ *
+ * 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 FLANN_PARAMS_H_
+#define FLANN_PARAMS_H_
+
+#include "any.h"
+#include "flann/general.h"
+#include <iostream>
+#include <map>
+
+
+namespace flann
+{
+
+namespace anyimpl
+{
+SMALL_POLICY(flann_algorithm_t);
+SMALL_POLICY(flann_centers_init_t);
+SMALL_POLICY(flann_log_level_t);
+SMALL_POLICY(flann_datatype_t);
+}
+
+
+typedef std::map<std::string, any> IndexParams;
+
+
+typedef enum {
+	FLANN_False = 0,
+	FLANN_True = 1,
+	FLANN_Undefined
+} tri_type;
+
+
+struct SearchParams
+{
+    SearchParams(int checks_ = 32, float eps_ = 0.0, bool sorted_ = true ) :
+    	checks(checks_), eps(eps_), sorted(sorted_)
+    {
+    	max_neighbors = -1;
+    	use_heap = FLANN_Undefined;
+    	cores = 1;
+    	matrices_in_gpu_ram = false;
+    }
+
+    // how many leafs to visit when searching for neighbours (-1 for unlimited)
+    int checks;
+    // search for eps-approximate neighbours (default: 0)
+    float eps;
+    // only for radius search, require neighbours sorted by distance (default: true)
+    bool sorted;
+    // maximum number of neighbors radius search should return (-1 for unlimited)
+    int max_neighbors;
+    // use a heap to manage the result set (default: FLANN_Undefined)
+    tri_type use_heap;
+    // how many cores to assign to the search (used only if compiled with OpenMP capable compiler) (0 for auto)
+    int cores;
+    // for GPU search indicates if matrices are already in GPU ram
+    bool matrices_in_gpu_ram;
+};
+
+
+inline bool has_param(const IndexParams& params, std::string name)
+{
+	return params.find(name)!=params.end();
+}
+
+template<typename T>
+T get_param(const IndexParams& params, std::string name, const T& default_value)
+{
+    IndexParams::const_iterator it = params.find(name);
+    if (it != params.end()) {
+        return it->second.cast<T>();
+    }
+    else {
+        return default_value;
+    }
+}
+
+template<typename T>
+T get_param(const IndexParams& params, std::string name)
+{
+    IndexParams::const_iterator it = params.find(name);
+    if (it != params.end()) {
+        return it->second.cast<T>();
+    }
+    else {
+        throw FLANNException(std::string("Missing parameter '")+name+std::string("' in the parameters given"));
+    }
+}
+
+inline void print_params(const IndexParams& params)
+{
+    IndexParams::const_iterator it;
+
+    for(it=params.begin(); it!=params.end(); ++it) {
+        std::cout << it->first << " : " << it->second << std::endl;
+    }
+}
+
+inline void print_params(const SearchParams& params)
+{
+	std::cout << "checks : " << params.checks << std::endl;
+	std::cout << "eps : " << params.eps << std::endl;
+	std::cout << "sorted : " << params.sorted << std::endl;
+	std::cout << "max_neighbors : " << params.max_neighbors << std::endl;
+}
+
+
+}
+
+
+#endif /* FLANN_PARAMS_H_ */

corelib/src/flann/util/random.h

@@ -0,0 +1,145 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_RANDOM_H
+#define FLANN_RANDOM_H
+
+#include <algorithm>
+#include <cstdlib>
+#include <cstddef>
+#include <vector>
+
+#include "flann/general.h"
+
+namespace flann
+{
+
+/**
+ * Seeds the random number generator
+ *  @param seed Random seed
+ */
+inline void seed_random(unsigned int seed)
+{
+    srand(seed);
+}
+
+/*
+ * Generates a random double value.
+ */
+/**
+ * Generates a random double value.
+ * @param high Upper limit
+ * @param low Lower limit
+ * @return Random double value
+ */
+inline double rand_double(double high = 1.0, double low = 0)
+{
+    return low + ((high-low) * (std::rand() / (RAND_MAX + 1.0)));
+}
+
+/**
+ * Generates a random integer value.
+ * @param high Upper limit
+ * @param low Lower limit
+ * @return Random integer value
+ */
+inline int rand_int(int high = RAND_MAX, int low = 0)
+{
+    return low + (int) ( double(high-low) * (std::rand() / (RAND_MAX + 1.0)));
+}
+
+
+class RandomGenerator
+{
+public:
+    ptrdiff_t operator() (ptrdiff_t i) { return rand_int(i); }
+};
+
+
+/**
+ * Random number generator that returns a distinct number from
+ * the [0,n) interval each time.
+ */
+class UniqueRandom
+{
+    std::vector<int> vals_;
+    int size_;
+    int counter_;
+
+public:
+    /**
+     * Constructor.
+     * @param n Size of the interval from which to generate
+     * @return
+     */
+    UniqueRandom(int n)
+    {
+        init(n);
+    }
+
+    /**
+     * Initializes the number generator.
+     * @param n the size of the interval from which to generate random numbers.
+     */
+    void init(int n)
+    {
+        static RandomGenerator generator;
+        // create and initialize an array of size n
+        vals_.resize(n);
+        size_ = n;
+        for (int i = 0; i < size_; ++i) vals_[i] = i;
+
+        // shuffle the elements in the array
+        std::random_shuffle(vals_.begin(), vals_.end(), generator);
+
+        counter_ = 0;
+    }
+
+    /**
+     * Return a distinct random integer in greater or equal to 0 and less
+     * than 'n' on each call. It should be called maximum 'n' times.
+     * Returns: a random integer
+     */
+    int next()
+    {
+        if (counter_ == size_) {
+            return -1;
+        }
+        else {
+            return vals_[counter_++];
+        }
+    }
+};
+
+}
+
+#endif //FLANN_RANDOM_H
+
+

corelib/src/flann/util/result_set.h

@@ -0,0 +1,934 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_RESULTSET_H
+#define FLANN_RESULTSET_H
+
+#include <algorithm>
+#include <cstring>
+#include <iostream>
+#include <limits>
+#include <set>
+#include <vector>
+
+namespace flann
+{
+
+/* 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) {}
+
+    bool operator<(const BranchStruct<T, DistanceType>& rhs) const
+    {
+        return mindist<rhs.mindist;
+    }
+};
+
+
+template <typename DistanceType>
+struct DistanceIndex
+{
+    DistanceIndex(DistanceType dist, size_t index) :
+        dist_(dist), index_(index)
+    {
+    }
+    bool operator<(const DistanceIndex& dist_index) const
+    {
+        return (dist_ < dist_index.dist_) || ((dist_ == dist_index.dist_) && index_ < dist_index.index_);
+    }
+    DistanceType dist_;
+    size_t index_;
+};
+
+
+template <typename DistanceType>
+class ResultSet
+{
+public:
+    virtual ~ResultSet() {}
+
+    virtual bool full() const = 0;
+
+    virtual void addPoint(DistanceType dist, size_t index) = 0;
+
+    virtual DistanceType worstDist() const = 0;
+
+};
+
+/**
+ * KNNSimpleResultSet does not ensure that the element it holds are unique.
+ * Is used in those cases where the nearest neighbour algorithm used does not
+ * attempt to insert the same element multiple times.
+ */
+template <typename DistanceType>
+class KNNSimpleResultSet : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+	KNNSimpleResultSet(size_t capacity_) :
+        capacity_(capacity_)
+    {
+		// reserving capacity to prevent memory re-allocations
+		dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),-1));
+    	clear();
+    }
+
+    ~KNNSimpleResultSet()
+    {
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        worst_distance_ = std::numeric_limits<DistanceType>::max();
+        dist_index_[capacity_-1].dist_ = worst_distance_;
+        count_ = 0;
+    }
+
+    /**
+     *
+     * @return Number of elements in the result set
+     */
+    size_t size() const
+    {
+        return count_;
+    }
+
+    /**
+     * Radius search result set always reports full
+     * @return
+     */
+    bool full() const
+    {
+        return count_==capacity_;
+    }
+
+    /**
+     * Add a point to result set
+     * @param dist distance to point
+     * @param index index of point
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+    	if (dist>=worst_distance_) return;
+
+        if (count_ < capacity_) ++count_;
+        size_t i;
+        for (i=count_-1; i>0; --i) {
+#ifdef FLANN_FIRST_MATCH
+            if ( (dist_index_[i-1].dist_>dist) || ((dist==dist_index_[i-1].dist_)&&(dist_index_[i-1].index_>index)) )
+#else
+            if (dist_index_[i-1].dist_>dist)
+#endif
+            {
+            	dist_index_[i] = dist_index_[i-1];
+            }
+            else break;
+        }
+        dist_index_[i].dist_ = dist;
+        dist_index_[i].index_ = index;
+        worst_distance_ = dist_index_[capacity_-1].dist_;
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	size_t n = std::min(count_, num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+    	return worst_distance_;
+    }
+
+private:
+    size_t capacity_;
+    size_t count_;
+    DistanceType worst_distance_;
+    std::vector<DistIndex> dist_index_;
+};
+
+/**
+ * K-Nearest neighbour result set. Ensures that the elements inserted are unique
+ */
+template <typename DistanceType>
+class KNNResultSet : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+    KNNResultSet(int capacity) : capacity_(capacity)
+    {
+		// reserving capacity to prevent memory re-allocations
+		dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),-1));
+    	clear();
+    }
+
+    ~KNNResultSet()
+    {
+
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        worst_distance_ = std::numeric_limits<DistanceType>::max();
+        dist_index_[capacity_-1].dist_ = worst_distance_;
+        count_ = 0;
+    }
+
+    size_t size() const
+    {
+        return count_;
+    }
+
+    bool full() const
+    {
+        return count_ == capacity_;
+    }
+
+
+    void addPoint(DistanceType dist, size_t index)
+    {
+        if (dist >= worst_distance_) return;
+        size_t i;
+        for (i = count_; i > 0; --i) {
+#ifdef FLANN_FIRST_MATCH
+            if ( (dist_index_[i-1].dist_<=dist) && ((dist!=dist_index_[i-1].dist_)||(dist_index_[i-1].index_<=index)) )
+#else
+            if (dist_index_[i-1].dist_<=dist)
+#endif
+            {
+                // Check for duplicate indices
+                for (size_t j = i - 1; dist_index_[j].dist_ == dist && j--;) {
+                    if (dist_index_[j].index_ == index) {
+                        return;
+                    }
+                }
+                break;
+            }
+        }
+        if (count_ < capacity_) ++count_;
+        for (size_t j = count_-1; j > i; --j) {
+            dist_index_[j] = dist_index_[j-1];
+        }
+        dist_index_[i].dist_ = dist;
+        dist_index_[i].index_ = index;
+        worst_distance_ = dist_index_[capacity_-1].dist_;
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	size_t n = std::min(count_, num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+        return worst_distance_;
+    }
+
+private:
+    size_t capacity_;
+    size_t count_;
+    DistanceType worst_distance_;
+    std::vector<DistIndex> dist_index_;
+
+};
+
+
+
+template <typename DistanceType>
+class KNNResultSet2 : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+	KNNResultSet2(size_t capacity_) :
+        capacity_(capacity_)
+    {
+		// reserving capacity to prevent memory re-allocations
+		dist_index_.reserve(capacity_);
+    	clear();
+    }
+
+    ~KNNResultSet2()
+    {
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        dist_index_.clear();
+        worst_dist_ = std::numeric_limits<DistanceType>::max();
+        is_full_ = false;
+    }
+
+    /**
+     *
+     * @return Number of elements in the result set
+     */
+    size_t size() const
+    {
+        return dist_index_.size();
+    }
+
+    /**
+     * Radius search result set always reports full
+     * @return
+     */
+    bool full() const
+    {
+        return is_full_;
+    }
+
+    /**
+     * Add another point to result set
+     * @param dist distance to point
+     * @param index index of point
+     * Pre-conditions: capacity_>0
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+    	if (dist>=worst_dist_) return;
+
+    	if (dist_index_.size()==capacity_) {
+    		// if result set if filled to capacity, remove farthest element
+    		std::pop_heap(dist_index_.begin(), dist_index_.end());
+        	dist_index_.pop_back();
+    	}
+
+    	// add new element
+    	dist_index_.push_back(DistIndex(dist,index));
+    	if (is_full_) { // when is_full_==true, we have a heap
+    		std::push_heap(dist_index_.begin(), dist_index_.end());
+    	}
+
+    	if (dist_index_.size()==capacity_) {
+    		if (!is_full_) {
+    			std::make_heap(dist_index_.begin(), dist_index_.end());
+            	is_full_ = true;
+    		}
+    		// we replaced the farthest element, update worst distance
+        	worst_dist_ = dist_index_[0].dist_;
+        }
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	if (sorted) {
+    		// std::sort_heap(dist_index_.begin(), dist_index_.end());
+    		// sort seems faster here, even though dist_index_ is a heap
+    		std::sort(dist_index_.begin(), dist_index_.end());
+    	}
+    	else {
+    		if (num_elements<size()) {
+    			std::nth_element(dist_index_.begin(), dist_index_.begin()+num_elements, dist_index_.end());
+    		}
+    	}
+
+    	size_t n = std::min(dist_index_.size(), num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+    	return worst_dist_;
+    }
+
+private:
+    size_t capacity_;
+    DistanceType worst_dist_;
+    std::vector<DistIndex> dist_index_;
+    bool is_full_;
+};
+
+
+/**
+ * Unbounded radius result set. It will hold as many elements as
+ * are added to it.
+ */
+template <typename DistanceType>
+class RadiusResultSet : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+	RadiusResultSet(DistanceType radius_) :
+        radius_(radius_)
+    {
+		// reserving some memory to limit number of re-allocations
+		dist_index_.reserve(1024);
+    	clear();
+    }
+
+    ~RadiusResultSet()
+    {
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        dist_index_.clear();
+    }
+
+    /**
+     *
+     * @return Number of elements in the result set
+     */
+    size_t size() const
+    {
+        return dist_index_.size();
+    }
+
+    /**
+     * Radius search result set always reports full
+     * @return
+     */
+    bool full() const
+    {
+        return true;
+    }
+
+    /**
+     * Add another point to result set
+     * @param dist distance to point
+     * @param index index of point
+     * Pre-conditions: capacity_>0
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+    	if (dist<radius_) {
+    		// add new element
+    		dist_index_.push_back(DistIndex(dist,index));
+    	}
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	if (sorted) {
+    		// std::sort_heap(dist_index_.begin(), dist_index_.end());
+    		// sort seems faster here, even though dist_index_ is a heap
+    		std::sort(dist_index_.begin(), dist_index_.end());
+    	}
+    	else {
+    		if (num_elements<size()) {
+    			std::nth_element(dist_index_.begin(), dist_index_.begin()+num_elements, dist_index_.end());
+    		}
+    	}
+
+    	size_t n = std::min(dist_index_.size(), num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+    	return radius_;
+    }
+
+private:
+    DistanceType radius_;
+    std::vector<DistIndex> dist_index_;
+};
+
+
+
+/**
+ * Bounded radius result set. It limits the number of elements
+ * it can hold to a preset capacity.
+ */
+template <typename DistanceType>
+class KNNRadiusResultSet : public ResultSet<DistanceType>
+{
+public:
+	typedef DistanceIndex<DistanceType> DistIndex;
+
+	KNNRadiusResultSet(DistanceType radius_, size_t capacity_) :
+        radius_(radius_), capacity_(capacity_)
+    {
+		// reserving capacity to prevent memory re-allocations
+		dist_index_.reserve(capacity_);
+    	clear();
+    }
+
+    ~KNNRadiusResultSet()
+    {
+    }
+
+    /**
+     * Clears the result set
+     */
+    void clear()
+    {
+        dist_index_.clear();
+        worst_dist_ = radius_;
+        is_heap_ = false;
+    }
+
+    /**
+     *
+     * @return Number of elements in the result set
+     */
+    size_t size() const
+    {
+        return dist_index_.size();
+    }
+
+    /**
+     * Radius search result set always reports full
+     * @return
+     */
+    bool full() const
+    {
+        return true;
+    }
+
+    /**
+     * Add another point to result set
+     * @param dist distance to point
+     * @param index index of point
+     * Pre-conditions: capacity_>0
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+    	if (dist>=worst_dist_) return;
+
+    	if (dist_index_.size()==capacity_) {
+    		// if result set is filled to capacity, remove farthest element
+    		std::pop_heap(dist_index_.begin(), dist_index_.end());
+        	dist_index_.pop_back();
+    	}
+
+    	// add new element
+    	dist_index_.push_back(DistIndex(dist,index));
+    	if (is_heap_) {
+    		std::push_heap(dist_index_.begin(), dist_index_.end());
+    	}
+
+    	if (dist_index_.size()==capacity_) {
+    		// when got to full capacity, make it a heap
+    		if (!is_heap_) {
+    			std::make_heap(dist_index_.begin(), dist_index_.end());
+    			is_heap_ = true;
+    		}
+    		// we replaced the farthest element, update worst distance
+        	worst_dist_ = dist_index_[0].dist_;
+        }
+    }
+
+    /**
+     * Copy indices and distances to output buffers
+     * @param indices
+     * @param dists
+     * @param num_elements Number of elements to copy
+     * @param sorted Indicates if results should be sorted
+     */
+    void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
+    {
+    	if (sorted) {
+    		// std::sort_heap(dist_index_.begin(), dist_index_.end());
+    		// sort seems faster here, even though dist_index_ is a heap
+    		std::sort(dist_index_.begin(), dist_index_.end());
+    	}
+    	else {
+    		if (num_elements<size()) {
+    			std::nth_element(dist_index_.begin(), dist_index_.begin()+num_elements, dist_index_.end());
+    		}
+    	}
+
+    	size_t n = std::min(dist_index_.size(), num_elements);
+    	for (size_t i=0; i<n; ++i) {
+    		*indices++ = dist_index_[i].index_;
+    		*dists++ = dist_index_[i].dist_;
+    	}
+    }
+
+    DistanceType worstDist() const
+    {
+    	return worst_dist_;
+    }
+
+private:
+    bool is_heap_;
+    DistanceType radius_;
+    size_t capacity_;
+    DistanceType worst_dist_;
+    std::vector<DistIndex> dist_index_;
+};
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ * This is a result set that only counts the neighbors within a radius.
+ */
+
+template <typename DistanceType>
+class CountRadiusResultSet : public ResultSet<DistanceType>
+{
+    DistanceType radius;
+    size_t count;
+
+public:
+    CountRadiusResultSet(DistanceType radius_ ) :
+        radius(radius_)
+    {
+        clear();
+    }
+
+    ~CountRadiusResultSet()
+    {
+    }
+
+    void clear()
+    {
+        count = 0;
+    }
+
+    size_t size() const
+    {
+        return count;
+    }
+
+    bool full() const
+    {
+        return true;
+    }
+
+    void addPoint(DistanceType dist, size_t index)
+    {
+        if (dist<radius) {
+            count++;
+        }
+    }
+
+    DistanceType worstDist() const
+    {
+        return radius;
+    }
+
+};
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Class that holds the k NN neighbors
+ */
+template<typename DistanceType>
+class UniqueResultSet : public ResultSet<DistanceType>
+{
+public:
+    struct DistIndex
+    {
+        DistIndex(DistanceType dist, unsigned int index) :
+            dist_(dist), index_(index)
+        {
+        }
+        bool operator<(const DistIndex dist_index) const
+        {
+            return (dist_ < dist_index.dist_) || ((dist_ == dist_index.dist_) && index_ < dist_index.index_);
+        }
+        DistanceType dist_;
+        unsigned int index_;
+    };
+
+    /** Default cosntructor */
+    UniqueResultSet() :
+        worst_distance_(std::numeric_limits<DistanceType>::max())
+    {
+    }
+
+    /** Check the status of the set
+     * @return true if we have k NN
+     */
+    inline bool full() const
+    {
+        return is_full_;
+    }
+
+    /** Copy the set to two C arrays
+     * @param indices pointer to a C array of indices
+     * @param dist pointer to a C array of distances
+     * @param n_neighbors the number of neighbors to copy
+     */
+    void copy(size_t* indices, DistanceType* dist, int n_neighbors, bool sorted = true)
+    {
+    	if (n_neighbors<0) n_neighbors = dist_indices_.size();
+    	int i = 0;
+    	typedef typename std::set<DistIndex>::const_iterator Iterator;
+    	for (Iterator dist_index = dist_indices_.begin(), dist_index_end =
+    			dist_indices_.end(); (dist_index != dist_index_end) && (i < n_neighbors); ++dist_index, ++indices, ++dist, ++i) {
+    		*indices = dist_index->index_;
+    		*dist = dist_index->dist_;
+    	}
+    }
+
+    /** The number of neighbors in the set
+     * @return
+     */
+    size_t size() const
+    {
+        return dist_indices_.size();
+    }
+
+    /** The distance of the furthest neighbor
+     * If we don't have enough neighbors, it returns the max possible value
+     * @return
+     */
+    inline DistanceType worstDist() const
+    {
+        return worst_distance_;
+    }
+protected:
+    /** Flag to say if the set is full */
+    bool is_full_;
+
+    /** The worst distance found so far */
+    DistanceType worst_distance_;
+
+    /** The best candidates so far */
+    std::set<DistIndex> dist_indices_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Class that holds the k NN neighbors
+ * Faster than KNNResultSet as it uses a binary heap and does not maintain two arrays
+ */
+template<typename DistanceType>
+class KNNUniqueResultSet : public UniqueResultSet<DistanceType>
+{
+public:
+    /** Constructor
+     * @param capacity the number of neighbors to store at max
+     */
+    KNNUniqueResultSet(unsigned int capacity) : capacity_(capacity)
+    {
+        this->is_full_ = false;
+        this->clear();
+    }
+
+    /** Add a possible candidate to the best neighbors
+     * @param dist distance for that neighbor
+     * @param index index of that neighbor
+     */
+    inline void addPoint(DistanceType dist, size_t index)
+    {
+        // Don't do anything if we are worse than the worst
+        if (dist >= worst_distance_) return;
+        dist_indices_.insert(DistIndex(dist, index));
+
+        if (is_full_) {
+            if (dist_indices_.size() > capacity_) {
+                dist_indices_.erase(*dist_indices_.rbegin());
+                worst_distance_ = dist_indices_.rbegin()->dist_;
+            }
+        }
+        else if (dist_indices_.size() == capacity_) {
+            is_full_ = true;
+            worst_distance_ = dist_indices_.rbegin()->dist_;
+        }
+    }
+
+    /** Remove all elements in the set
+     */
+    void clear()
+    {
+        dist_indices_.clear();
+        worst_distance_ = std::numeric_limits<DistanceType>::max();
+        is_full_ = false;
+    }
+
+protected:
+    typedef typename UniqueResultSet<DistanceType>::DistIndex DistIndex;
+    using UniqueResultSet<DistanceType>::is_full_;
+    using UniqueResultSet<DistanceType>::worst_distance_;
+    using UniqueResultSet<DistanceType>::dist_indices_;
+
+    /** The number of neighbors to keep */
+    unsigned int capacity_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Class that holds the radius nearest neighbors
+ * It is more accurate than RadiusResult as it is not limited in the number of neighbors
+ */
+template<typename DistanceType>
+class RadiusUniqueResultSet : public UniqueResultSet<DistanceType>
+{
+public:
+    /** Constructor
+     * @param capacity the number of neighbors to store at max
+     */
+    RadiusUniqueResultSet(DistanceType radius) :
+        radius_(radius)
+    {
+        is_full_ = true;
+    }
+
+    /** Add a possible candidate to the best neighbors
+     * @param dist distance for that neighbor
+     * @param index index of that neighbor
+     */
+    void addPoint(DistanceType dist, size_t index)
+    {
+        if (dist < radius_) dist_indices_.insert(DistIndex(dist, index));
+    }
+
+    /** Remove all elements in the set
+     */
+    inline void clear()
+    {
+        dist_indices_.clear();
+    }
+
+
+    /** Check the status of the set
+     * @return alwys false
+     */
+    inline bool full() const
+    {
+        return true;
+    }
+
+    /** The distance of the furthest neighbor
+     * If we don't have enough neighbors, it returns the max possible value
+     * @return
+     */
+    inline DistanceType worstDist() const
+    {
+        return radius_;
+    }
+private:
+    typedef typename UniqueResultSet<DistanceType>::DistIndex DistIndex;
+    using UniqueResultSet<DistanceType>::dist_indices_;
+    using UniqueResultSet<DistanceType>::is_full_;
+
+    /** The furthest distance a neighbor can be */
+    DistanceType radius_;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/** Class that holds the k NN neighbors within a radius distance
+ */
+template<typename DistanceType>
+class KNNRadiusUniqueResultSet : public KNNUniqueResultSet<DistanceType>
+{
+public:
+    /** Constructor
+     * @param capacity the number of neighbors to store at max
+     */
+    KNNRadiusUniqueResultSet(DistanceType radius, size_t capacity) : KNNUniqueResultSet<DistanceType>(capacity)
+    {
+        this->radius_ = radius;
+        this->clear();
+    }
+
+    /** Remove all elements in the set
+     */
+    void clear()
+    {
+        dist_indices_.clear();
+        worst_distance_ = radius_;
+        is_full_ = true;
+    }
+private:
+    using KNNUniqueResultSet<DistanceType>::dist_indices_;
+    using KNNUniqueResultSet<DistanceType>::is_full_;
+    using KNNUniqueResultSet<DistanceType>::worst_distance_;
+
+    /** The maximum distance of a neighbor */
+    DistanceType radius_;
+};
+}
+
+#endif //FLANN_RESULTSET_H
+

corelib/src/flann/util/sampling.h

@@ -0,0 +1,72 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_SAMPLING_H_
+#define FLANN_SAMPLING_H_
+
+#include "flann/util/matrix.h"
+#include "flann/util/random.h"
+
+namespace flann
+{
+
+template<typename T>
+Matrix<T> random_sample(Matrix<T>& srcMatrix, size_t size, bool remove = false)
+{
+	UniqueRandom rand_unique(srcMatrix.rows);
+    Matrix<T> newSet(new T[size * srcMatrix.cols], size,srcMatrix.cols);
+
+    T* src,* dest;
+    for (size_t i=0; i<size; ++i) {
+    	size_t r;
+    	if (remove) {
+            r = static_cast<size_t>(rand_int(srcMatrix.rows-i));
+    	}
+    	else {
+    		r = static_cast<size_t>(rand_unique.next());
+    	}
+        dest = newSet[i];
+        src = srcMatrix[r];
+        std::copy(src, src+srcMatrix.cols, dest);
+        if (remove) {
+            src = srcMatrix[srcMatrix.rows-i-1];
+            dest = srcMatrix[r];
+            std::copy(src, src+srcMatrix.cols, dest);
+        }
+    }
+    if (remove) {
+        srcMatrix.rows -= size;
+    }
+    return newSet;
+}
+
+} // namespace
+
+
+#endif /* FLANN_SAMPLING_H_ */

corelib/src/flann/util/saving.h

@@ -0,0 +1,135 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 NNIndexGOODS 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 FLANN_SAVING_H_
+#define FLANN_SAVING_H_
+
+#include <cstring>
+#include <vector>
+#include <stdio.h>
+
+#include "flann/general.h"
+#include "flann/util/serialization.h"
+
+
+#ifdef FLANN_SIGNATURE_
+#undef FLANN_SIGNATURE_
+#endif
+#define FLANN_SIGNATURE_ "FLANN_INDEX_v1.1"
+
+namespace flann
+{
+
+/**
+ * Structure representing the index header.
+ */
+struct IndexHeader
+{
+    IndexHeaderStruct h;
+
+    IndexHeader()
+	{
+        memset(h.signature, 0, sizeof(h.signature));
+        strcpy(h.signature, FLANN_SIGNATURE_);
+        memset(h.version, 0, sizeof(h.version));
+        strcpy(h.version, FLANN_VERSION_);
+
+        h.compression = 0;
+        h.first_block_size = 0;
+	}
+
+private:
+    template<typename Archive>
+    void serialize(Archive& ar)
+    {
+        ar & h.signature;
+        ar & h.version;
+        ar & h.data_type;
+        ar & h.index_type;
+        ar & h.rows;
+        ar & h.cols;
+        ar & h.compression;
+        ar & h.first_block_size;
+    }
+    friend struct serialization::access;
+};
+
+/**
+ * Saves index header to stream
+ *
+ * @param stream - Stream to save to
+ * @param index - The index to save
+ */
+template<typename Index>
+void save_header(FILE* stream, const Index& index)
+{
+    IndexHeader header;
+    header.h.data_type = flann_datatype_value<typename Index::ElementType>::value;
+    header.h.index_type = index.getType();
+    header.h.rows = index.size();
+    header.h.cols = index.veclen();
+
+    fwrite(&header, sizeof(header),1,stream);
+}
+
+
+/**
+ *
+ * @param stream - Stream to load from
+ * @return Index header
+ */
+inline IndexHeader load_header(FILE* stream)
+{
+    IndexHeader header;
+    int read_size = fread(&header,sizeof(header),1,stream);
+
+    if (read_size != 1) {
+        throw FLANNException("Invalid index file, cannot read");
+    }
+
+    if (strncmp(header.h.signature,
+                FLANN_SIGNATURE_,
+                strlen(FLANN_SIGNATURE_) - strlen("v0.0")) != 0) {
+        throw FLANNException("Invalid index file, wrong signature");
+    }
+
+    return header;
+}
+
+
+namespace serialization
+{
+ENUM_SERIALIZER(flann_algorithm_t);
+ENUM_SERIALIZER(flann_centers_init_t);
+ENUM_SERIALIZER(flann_log_level_t);
+ENUM_SERIALIZER(flann_datatype_t);
+}
+
+}
+
+#endif /* FLANN_SAVING_H_ */

corelib/src/flann/util/serialization.h

@@ -0,0 +1,813 @@
+#ifndef SERIALIZATION_H_
+#define SERIALIZATION_H_
+
+#include <vector>
+#include <map>
+#include <cstdlib>
+#include <cstring>
+#include <stdio.h>
+#include "flann/ext/lz4.h"
+#include "flann/ext/lz4hc.h"
+
+
+namespace flann
+{
+    struct IndexHeaderStruct {
+        char signature[24];
+        char version[16];
+        flann_datatype_t data_type;
+        flann_algorithm_t index_type;
+        size_t rows;
+        size_t cols;
+        size_t compression;
+        size_t first_block_size;
+    };
+
+namespace serialization
+{
+
+struct access 
+{
+    template<typename Archive, typename T>
+    static inline void serialize(Archive& ar, T& type)
+    {
+        type.serialize(ar);
+    }
+};
+
+
+template<typename Archive, typename T>
+inline void serialize(Archive& ar, T& type)
+{
+    access::serialize(ar,type);
+}
+
+template<typename T>
+struct Serializer
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, T& val)
+    {
+        serialization::serialize(ar,val);
+    }
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar, const T& val)
+    {
+        serialization::serialize(ar,const_cast<T&>(val));
+    }
+};
+
+#define BASIC_TYPE_SERIALIZER(type)\
+template<> \
+struct Serializer<type> \
+{\
+    template<typename InputArchive>\
+    static inline void load(InputArchive& ar, type& val)\
+    {\
+        ar.load(val);\
+    }\
+    template<typename OutputArchive>\
+    static inline void save(OutputArchive& ar, const type& val)\
+    {\
+        ar.save(val);\
+    }\
+}
+
+#define ENUM_SERIALIZER(type)\
+template<>\
+struct Serializer<type>\
+{\
+    template<typename InputArchive>\
+    static inline void load(InputArchive& ar, type& val)\
+    {\
+        int int_val;\
+        ar & int_val;\
+        val = (type) int_val;\
+    }\
+    template<typename OutputArchive>\
+    static inline void save(OutputArchive& ar, const type& val)\
+    {\
+        int int_val = (int)val;\
+        ar & int_val;\
+    }\
+}
+
+
+// declare serializers for simple types
+BASIC_TYPE_SERIALIZER(char);
+BASIC_TYPE_SERIALIZER(unsigned char);
+BASIC_TYPE_SERIALIZER(short);
+BASIC_TYPE_SERIALIZER(unsigned short);
+BASIC_TYPE_SERIALIZER(int);
+BASIC_TYPE_SERIALIZER(unsigned int);
+BASIC_TYPE_SERIALIZER(long);
+BASIC_TYPE_SERIALIZER(unsigned long);
+BASIC_TYPE_SERIALIZER(unsigned long long);
+BASIC_TYPE_SERIALIZER(float);
+BASIC_TYPE_SERIALIZER(double);
+BASIC_TYPE_SERIALIZER(bool);
+#ifdef _MSC_VER
+// unsigned __int64 ~= unsigned long long
+// Will throw error on VS2013
+#if _MSC_VER != 1800
+BASIC_TYPE_SERIALIZER(unsigned __int64);
+#endif
+#endif
+
+
+// serializer for std::vector
+template<typename T>
+struct Serializer<std::vector<T> >
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, std::vector<T>& val)
+    {
+        size_t size;
+        ar & size;
+        val.resize(size);
+        for (size_t i=0;i<size;++i) {
+            ar & val[i];
+        }
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar, const std::vector<T>& val)
+    {
+        ar & val.size();
+        for (size_t i=0;i<val.size();++i) {
+            ar & val[i];
+        }
+    }
+};
+
+// serializer for std::vector
+template<typename K, typename V>
+struct Serializer<std::map<K,V> >
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, std::map<K,V>& map_val)
+    {
+        size_t size;
+        ar & size;
+        for (size_t i = 0; i < size; ++i)
+        {
+            K key;
+            ar & key;
+            V value;
+            ar & value;
+            map_val[key] = value;
+        }
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar, const std::map<K,V>& map_val)
+    {
+        ar & map_val.size();
+        for (typename std::map<K,V>::const_iterator i=map_val.begin(); i!=map_val.end(); ++i) {
+            ar & i->first;
+            ar & i->second;
+        }
+    }
+};
+
+template<typename T>
+struct Serializer<T*>
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, T*& val)
+    {
+        ar.load(val);
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar,  T* const& val)
+    {
+        ar.save(val);
+    }
+};
+
+template<typename T, int N>
+struct Serializer<T[N]>
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, T (&val)[N])
+    {
+        ar.load(val);
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar,  T const (&val)[N])
+    {
+        ar.save(val);
+    }
+};
+
+
+
+
+struct binary_object
+{
+    void const * ptr_;
+    size_t size_;
+
+    binary_object( void * const ptr, size_t size) :
+        ptr_(ptr),
+        size_(size)
+    {}
+    binary_object(const binary_object & rhs) :
+        ptr_(rhs.ptr_),
+        size_(rhs.size_)
+    {}
+
+    binary_object & operator=(const binary_object & rhs) {
+        ptr_ = rhs.ptr_;
+        size_ = rhs.size_;
+        return *this;
+    }
+};
+
+inline const binary_object make_binary_object(/* const */ void * t, size_t size){
+    return binary_object(t, size);
+}
+
+template<>
+struct Serializer<const binary_object>
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, const binary_object& b)
+    {
+        ar.load_binary(const_cast<void *>(b.ptr_), b.size_);
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar,  const binary_object& b)
+    {
+        ar.save_binary(b.ptr_, b.size_);
+    }
+};
+
+template<>
+struct Serializer<binary_object>
+{
+    template<typename InputArchive>
+    static inline void load(InputArchive& ar, binary_object& b)
+    {
+        ar.load_binary(const_cast<void *>(b.ptr_), b.size_);
+    }
+
+    template<typename OutputArchive>
+    static inline void save(OutputArchive& ar,  const binary_object& b)
+    {
+        ar.save_binary(b.ptr_, b.size_);
+    }
+};
+
+
+
+template <bool C_> 
+struct bool_ {
+    static const bool value = C_;
+    typedef bool value_type;
+};
+
+
+class ArchiveBase
+{
+public:
+	void* getObject() { return object_; }
+
+	void setObject(void* object) { object_ = object; }
+
+private:
+	void* object_;
+};
+
+
+template<typename Archive>
+class InputArchive : public ArchiveBase
+{
+protected:
+    InputArchive() {};
+public:
+    typedef bool_<true> is_loading;
+    typedef bool_<false> is_saving;
+
+    template<typename T>
+    Archive& operator& (T& val)
+    {
+        Serializer<T>::load(*static_cast<Archive*>(this),val);
+        return *static_cast<Archive*>(this);
+    }
+};
+
+
+template<typename Archive>
+class OutputArchive : public ArchiveBase
+{
+protected:
+    OutputArchive() {};
+public:
+    typedef bool_<false> is_loading;
+    typedef bool_<true> is_saving;
+    
+    template<typename T>
+    Archive& operator& (const T& val)
+    {
+        Serializer<T>::save(*static_cast<Archive*>(this),val);
+        return *static_cast<Archive*>(this);
+    }
+};
+
+
+
+class SizeArchive : public OutputArchive<SizeArchive>
+{
+    size_t size_;
+public:
+
+    SizeArchive() : size_(0)
+    {
+    }
+
+    template<typename T>
+    void save(const T& val)
+    {
+        size_ += sizeof(val);
+    }
+
+    template<typename T>
+    void save_binary(T* ptr, size_t size)
+    {
+    	size_ += size;
+    }
+
+
+    void reset()
+    {
+        size_ = 0;
+    }
+
+    size_t size()
+    {
+        return size_;
+    }
+};
+
+
+//
+//class PrintArchive : public OutputArchive<PrintArchive>
+//{
+//public:
+//    template<typename T>
+//    void save(const T& val)
+//    {
+//        std::cout << val << std::endl;
+//    }
+//
+//    template<typename T>
+//    void save_binary(T* ptr, size_t size)
+//    {
+//        std::cout << "<binary object>" << std::endl;
+//    }
+//};
+    
+#define BLOCK_BYTES (1024 * 64)
+
+class SaveArchive : public OutputArchive<SaveArchive>
+{
+    /**
+     * Based on blockStreaming_doubleBuffer code at:
+     * https://github.com/Cyan4973/lz4/blob/master/examples/blockStreaming_doubleBuffer.c
+     */
+    
+    FILE* stream_;
+    bool own_stream_;
+    char *buffer_;
+    size_t offset_;
+
+    int first_block_;
+    char *buffer_blocks_;
+    char *compressed_buffer_;
+    LZ4_streamHC_t lz4Stream_body;
+    LZ4_streamHC_t* lz4Stream;
+
+    void initBlock()
+    {
+        // Alloc the space for both buffer blocks (each compressed block
+        // references the previous)
+        buffer_ = buffer_blocks_ = (char *)malloc(BLOCK_BYTES*2);
+        compressed_buffer_ = (char *)malloc(LZ4_COMPRESSBOUND(BLOCK_BYTES) + sizeof(size_t));
+        if (buffer_ == NULL || compressed_buffer_ == NULL) {
+            throw FLANNException("Error allocating compression buffer");
+        }
+        
+        // Init the LZ4 stream
+        lz4Stream = &lz4Stream_body;
+        LZ4_resetStreamHC(lz4Stream, 9);
+        first_block_ = true;
+        
+        offset_ = 0;
+    }
+    
+    void flushBlock()
+    {
+        size_t compSz = 0;
+        // Handle header
+        if (first_block_) {
+            // Copy & set the header
+            IndexHeaderStruct *head = (IndexHeaderStruct *)buffer_;
+            size_t headSz = sizeof(IndexHeaderStruct);
+            
+            assert(head->compression == 0);
+            head->compression = 1; // Bool now, enum later
+        
+            // Do the compression for the block
+            compSz = LZ4_compress_HC_continue(
+                lz4Stream, buffer_+headSz, compressed_buffer_+headSz, offset_-headSz,
+                LZ4_COMPRESSBOUND(BLOCK_BYTES));
+            
+            if(compSz <= 0) {
+                throw FLANNException("Error compressing (first block)");
+            }
+            
+            // Handle header
+            head->first_block_size = compSz;
+            memcpy(compressed_buffer_, buffer_, headSz);
+            
+            compSz += headSz;
+            first_block_ = false;
+        } else {
+            size_t headSz = sizeof(compSz);
+            
+            // Do the compression for the block
+            compSz = LZ4_compress_HC_continue(
+                lz4Stream, buffer_, compressed_buffer_+headSz, offset_,
+                LZ4_COMPRESSBOUND(BLOCK_BYTES));
+            
+            if(compSz <= 0) {
+                throw FLANNException("Error compressing");
+            }
+            
+            // Save the size of the compressed block as the header
+            memcpy(compressed_buffer_, &compSz, headSz);
+            compSz += headSz;
+        }
+        
+        // Write the compressed buffer
+        fwrite(compressed_buffer_, compSz, 1, stream_);
+        
+        // Switch the buffer to the *other* block
+        if (buffer_ == buffer_blocks_)
+            buffer_ = &buffer_blocks_[BLOCK_BYTES];
+        else
+            buffer_ = buffer_blocks_;
+        offset_ = 0;
+    }
+    
+    void endBlock()
+    {
+        // Cleanup memory
+        free(buffer_blocks_);
+        buffer_blocks_ = NULL;
+        buffer_ = NULL;
+        free(compressed_buffer_);
+        compressed_buffer_ = NULL;
+        
+        // Write a '0' size for next block
+        size_t z = 0;
+        fwrite(&z, sizeof(z), 1, stream_);
+    }
+
+public:
+    SaveArchive(const char* filename)
+    {
+        stream_ = fopen(filename, "wb");
+        own_stream_ = true;
+        initBlock();
+    }
+
+    SaveArchive(FILE* stream) : stream_(stream), own_stream_(false)
+    {
+        initBlock();
+    }
+
+    ~SaveArchive()
+    {
+        flushBlock();
+        endBlock();
+        if (buffer_) {
+            free(buffer_);
+            buffer_ = NULL;
+        }
+    	if (own_stream_) {
+    		fclose(stream_);
+    	}
+    }
+
+    template<typename T>
+    void save(const T& val)
+    {
+        assert(sizeof(val) < BLOCK_BYTES);
+        if (offset_+sizeof(val) > BLOCK_BYTES)
+            flushBlock();
+        memcpy(buffer_+offset_, &val, sizeof(val));
+        offset_ += sizeof(val);
+    }
+
+    template<typename T>
+    void save(T* const& val)
+    {
+    	// don't save pointers
+        //fwrite(&val, sizeof(val), 1, handle_);
+    }
+    
+    template<typename T>
+    void save_binary(T* ptr, size_t size)
+    {
+        while (size > BLOCK_BYTES) {
+            // Flush existing block
+            flushBlock();
+            
+            // Save large chunk
+            memcpy(buffer_, ptr, BLOCK_BYTES);
+            offset_ += BLOCK_BYTES;
+            ptr = ((char *)ptr) + BLOCK_BYTES;
+            size -= BLOCK_BYTES;
+        }
+        
+        // Save existing block if new data will make it too big
+        if (offset_+size > BLOCK_BYTES)
+            flushBlock();
+        
+        // Copy out requested data
+        memcpy(buffer_+offset_, ptr, size);
+        offset_ += size;
+    }
+
+};
+
+
+class LoadArchive : public InputArchive<LoadArchive>
+{
+    /**
+     * Based on blockStreaming_doubleBuffer code at:
+     * https://github.com/Cyan4973/lz4/blob/master/examples/blockStreaming_doubleBuffer.c
+     */
+    
+    FILE* stream_;
+    bool own_stream_;
+    char *buffer_;
+    char *ptr_;
+    
+    char *buffer_blocks_;
+    char *compressed_buffer_;
+    LZ4_streamDecode_t lz4StreamDecode_body;
+    LZ4_streamDecode_t* lz4StreamDecode;
+    size_t block_sz_;
+
+    void decompressAndLoadV10(FILE* stream)
+    {
+        buffer_ = NULL;
+        
+        // Find file size
+        size_t pos = ftell(stream);
+        fseek(stream, 0, SEEK_END);
+        size_t fileSize = ftell(stream)-pos;
+        fseek(stream, pos, SEEK_SET);
+        size_t headSz = sizeof(IndexHeaderStruct);
+
+        // Read the (compressed) file to a buffer
+        char *compBuffer = (char *)malloc(fileSize);
+        if (compBuffer == NULL) {
+            throw FLANNException("Error allocating file buffer space");
+        }
+        if (fread(compBuffer, fileSize, 1, stream) != 1) {
+            free(compBuffer);
+            throw FLANNException("Invalid index file, cannot read from disk (compressed)");
+        }
+
+        // Extract header
+        IndexHeaderStruct *head = (IndexHeaderStruct *)(compBuffer);
+        
+        // Backward compatability
+        size_t compressedSz = fileSize-headSz;
+        size_t uncompressedSz = head->first_block_size-headSz;
+        
+        // Check for compression type
+        if (head->compression != 1) {
+            free(compBuffer);
+            throw FLANNException("Compression type not supported");
+        }
+        
+        // Allocate a decompressed buffer
+        ptr_ = buffer_ = (char *)malloc(uncompressedSz+headSz);
+        if (buffer_ == NULL) {
+            free(compBuffer);
+            throw FLANNException("Error (re)allocating decompression buffer");
+        }
+        
+        // Extract body
+        size_t usedSz = LZ4_decompress_safe(compBuffer+headSz,
+                                            buffer_+headSz,
+                                            compressedSz,
+                                            uncompressedSz);
+        
+        // Check if the decompression was the expected size.
+        if (usedSz != uncompressedSz) {
+            free(compBuffer);
+            throw FLANNException("Unexpected decompression size");
+        }
+        
+        // Copy header data
+        memcpy(buffer_, compBuffer, headSz);
+        free(compBuffer);
+        
+        // Put the file pointer at the end of the data we've read
+        if (compressedSz+headSz+pos != fileSize)
+            fseek(stream, compressedSz+headSz+pos, SEEK_SET);
+        block_sz_ = uncompressedSz+headSz;
+    }
+    
+    void initBlock(FILE *stream)
+    {
+        size_t pos = ftell(stream);
+        buffer_ = NULL;
+        buffer_blocks_ = NULL;
+        compressed_buffer_ = NULL;
+        size_t headSz = sizeof(IndexHeaderStruct);
+        
+        // Read the file header to a buffer
+        IndexHeaderStruct *head = (IndexHeaderStruct *)malloc(headSz);
+        if (head == NULL) {
+            throw FLANNException("Error allocating header buffer space");
+        }
+        if (fread(head, headSz, 1, stream) != 1) {
+            free(head);
+            throw FLANNException("Invalid index file, cannot read from disk (header)");
+        }
+        
+        // Backward compatability
+        if (head->signature[13] == '1' && head->signature[15] == '0') {
+            free(head);
+            fseek(stream, pos, SEEK_SET);
+            return decompressAndLoadV10(stream);
+        }
+        
+        // Alloc the space for both buffer blocks (each block
+        // references the previous)
+        buffer_ = buffer_blocks_ = (char *)malloc(BLOCK_BYTES*2);
+        compressed_buffer_ = (char *)malloc(LZ4_COMPRESSBOUND(BLOCK_BYTES));
+        if (buffer_ == NULL || compressed_buffer_ == NULL) {
+            free(head);
+            throw FLANNException("Error allocating compression buffer");
+        }
+        
+        // Init the LZ4 stream
+        lz4StreamDecode = &lz4StreamDecode_body;
+        LZ4_setStreamDecode(lz4StreamDecode, NULL, 0);
+        
+        // Read first block
+        memcpy(buffer_, head, headSz);
+        loadBlock(buffer_+headSz, head->first_block_size, stream);
+        block_sz_ += headSz;
+        ptr_ = buffer_;
+        free(head);
+    }
+
+    void loadBlock(char* buffer_, size_t compSz, FILE* stream)
+    {
+        if(compSz >= LZ4_COMPRESSBOUND(BLOCK_BYTES)) {
+            throw FLANNException("Requested block size too large");
+        }
+        
+        // Read the block into the compressed buffer
+        if (fread(compressed_buffer_, compSz, 1, stream) != 1) {
+            throw FLANNException("Invalid index file, cannot read from disk (block)");
+        }
+        
+        // Decompress into the regular buffer
+        const int decBytes = LZ4_decompress_safe_continue(
+            lz4StreamDecode, compressed_buffer_, buffer_, compSz, BLOCK_BYTES);
+        if(decBytes <= 0) {
+            throw FLANNException("Invalid index file, cannot decompress block");
+        }
+        block_sz_ = decBytes;
+    }
+    
+    void preparePtr(size_t size)
+    {
+        // Return if the new size is less than (or eq) the size of a block
+        if (ptr_+size <= buffer_+block_sz_)
+            return;
+        
+        // Switch the buffer to the *other* block
+        if (buffer_ == buffer_blocks_)
+            buffer_ = &buffer_blocks_[BLOCK_BYTES];
+        else
+            buffer_ = buffer_blocks_;
+        
+        // Find the size of the next block
+        size_t cmpSz = 0;
+        size_t readCnt = fread(&cmpSz, sizeof(cmpSz), 1, stream_);
+        if(cmpSz <= 0 || readCnt != 1) {
+            throw FLANNException("Requested to read next block past end of file");
+        }
+        
+        // Load block & init ptr
+        loadBlock(buffer_, cmpSz, stream_);
+        ptr_ = buffer_;
+    }
+    
+    void endBlock()
+    {
+        // If not v1.0 format hack...
+        if (buffer_blocks_ != NULL) {
+            // Read the last '0' in the file
+            size_t zero = -1;
+            if (fread(&zero, sizeof(zero), 1, stream_) != 1) {
+                throw FLANNException("Invalid index file, cannot read from disk (end)");
+            }
+            if (zero != 0) {
+                throw FLANNException("Invalid index file, last block not zero length");
+            }
+        }
+        
+        // Free resources
+        if (buffer_blocks_ != NULL) {
+            free(buffer_blocks_);
+            buffer_blocks_ = NULL;
+        }
+        if (compressed_buffer_ != NULL) {
+            free(compressed_buffer_);
+            compressed_buffer_ = NULL;
+        }
+        ptr_ = NULL;
+    }
+    
+public:
+    LoadArchive(const char* filename)
+    {
+        // Open the file
+        stream_ = fopen(filename, "rb");
+        own_stream_ = true;
+
+        initBlock(stream_);
+    }
+
+    LoadArchive(FILE* stream)
+    {
+        stream_ = stream;
+        own_stream_ = false;
+
+        initBlock(stream);
+    }
+
+    ~LoadArchive()
+    {
+        endBlock();
+    	if (own_stream_) {
+    		fclose(stream_);
+    	}
+    }
+
+    template<typename T>
+    void load(T& val)
+    {
+        preparePtr(sizeof(val));
+        memcpy(&val, ptr_, sizeof(val));
+        ptr_ += sizeof(val);
+    }
+
+    template<typename T>
+    void load(T*& val)
+    {
+    	// don't load pointers
+        //fread(&val, sizeof(val), 1, handle_);
+    }
+
+    template<typename T>
+    void load_binary(T* ptr, size_t size)
+    {
+        while (size > BLOCK_BYTES) {
+            // Load next block
+            preparePtr(BLOCK_BYTES);
+            
+            // Load large chunk
+            memcpy(ptr, ptr_, BLOCK_BYTES);
+            ptr_ += BLOCK_BYTES;
+            ptr = ((char *)ptr) + BLOCK_BYTES;
+            size -= BLOCK_BYTES;
+        }
+        
+        // Load next block if needed
+        preparePtr(size);
+        
+        // Load the data
+        memcpy(ptr, ptr_, size);
+        ptr_ += size;
+    }
+};
+
+} // namespace serialization
+} // namespace flann
+#endif // SERIALIZATION_H_

corelib/src/flann/util/timer.h

@@ -0,0 +1,95 @@
+/***********************************************************************
+ * 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.
+ *
+ * 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 FLANN_TIMER_H
+#define FLANN_TIMER_H
+
+#include <time.h>
+
+
+namespace flann
+{
+
+/**
+ * A start-stop timer class.
+ *
+ * Can be used to time portions of code.
+ */
+class StartStopTimer
+{
+    clock_t startTime;
+
+public:
+    /**
+     * Value of the timer.
+     */
+    double value;
+
+
+    /**
+     * Constructor.
+     */
+    StartStopTimer()
+    {
+        reset();
+    }
+
+    /**
+     * Starts the timer.
+     */
+    void start()
+    {
+        startTime = clock();
+    }
+
+    /**
+     * Stops the timer and updates timer value.
+     */
+    double stop()
+    {
+        clock_t stopTime = clock();
+        value += ( (double)stopTime - startTime) / CLOCKS_PER_SEC;
+        
+        return value;
+    }
+
+    /**
+     * Resets the timer value to 0.
+     */
+    void reset()
+    {
+        value = 0;
+    }
+
+};
+
+}
+
+#endif // FLANN_TIMER_H

guilib/src/AboutDialog.cpp

@@ -34,8 +34,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/version.hpp>
 #include <pcl/pcl_config.h>
 
-#include <flann/config.h>
-
 namespace rtabmap {
 
 AboutDialog::AboutDialog(QWidget * parent) :
@@ -55,7 +53,6 @@ AboutDialog::AboutDialog(QWidget * parent) :
 #endif
 	_ui->label_version->setText(version);
 	_ui->label_opencv_version->setText(cv_version);
-	_ui->label_flann_version->setText(FLANN_VERSION_);
 	_ui->label_pcl_version->setText(PCL_VERSION_PRETTY);
 	_ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No");
 	_ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No");

guilib/src/CMakeLists.txt

@@ -121,10 +121,6 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 add_definitions(${PCL_DEFINITIONS})
 
-IF(FLANN18_FOUND)
-	ADD_DEFINITIONS("-DWITH_FLANN18")
-ENDIF(FLANN18_FOUND)
-
 # create a library from the source files
 ADD_LIBRARY(rtabmap_gui ${SRC_FILES})
 # Linking with Qt libraries

guilib/src/ui/aboutDialog.ui

@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>831</width>
-    <height>615</height>
+    <height>681</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -82,6 +82,13 @@ p, li { white-space: pre-wrap; }
         </item>
         <item>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+          <item row="9" column="0">
+           <widget class="QLabel" name="label_11">
+            <property name="text">
+             <string>PCL version :</string>
+            </property>
+           </widget>
+          </item>
           <item row="9" column="1">
            <widget class="QLabel" name="label_pcl_version">
             <property name="text">
@@ -92,21 +99,21 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="11" column="0">
+          <item row="10" column="0">
            <widget class="QLabel" name="label_12">
             <property name="text">
              <string>With Freenect :</string>
             </property>
            </widget>
           </item>
-          <item row="12" column="0">
+          <item row="11" column="0">
            <widget class="QLabel" name="label_13">
             <property name="text">
              <string>With OpenNI2 :</string>
             </property>
            </widget>
           </item>
-          <item row="14" column="0">
+          <item row="13" column="0">
            <widget class="QLabel" name="label_16">
             <property name="text">
              <string>With stereo dc1394 :</string>
@@ -205,21 +212,14 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="9" column="0">
+          <item row="15" column="0">
-           <widget class="QLabel" name="label_11">
-            <property name="text">
-             <string>PCL version :</string>
-            </property>
-           </widget>
-          </item>
-          <item row="16" column="0">
            <widget class="QLabel" name="label_14">
             <property name="text">
              <string>With g2o :</string>
             </property>
            </widget>
           </item>
-          <item row="11" column="1">
+          <item row="10" column="1">
            <widget class="QLabel" name="label_freenect">
             <property name="text">
              <string/>
@@ -229,7 +229,7 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="12" column="1">
+          <item row="11" column="1">
            <widget class="QLabel" name="label_openni2">
             <property name="text">
              <string/>
@@ -239,7 +239,7 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="16" column="1">
+          <item row="15" column="1">
            <widget class="QLabel" name="label_g2o">
             <property name="text">
              <string/>
@@ -249,21 +249,21 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="13" column="0">
+          <item row="12" column="0">
            <widget class="QLabel" name="label_15">
             <property name="text">
              <string>With Freenect2 :</string>
             </property>
            </widget>
           </item>
-          <item row="15" column="0">
+          <item row="14" column="0">
            <widget class="QLabel" name="label_17">
             <property name="text">
              <string>With stereo FlyCapture2 :</string>
             </property>
            </widget>
           </item>
-          <item row="13" column="1">
+          <item row="12" column="1">
            <widget class="QLabel" name="label_freenect2">
             <property name="text">
              <string/>
@@ -273,7 +273,7 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="14" column="1">
+          <item row="13" column="1">
            <widget class="QLabel" name="label_dc1394">
             <property name="text">
              <string/>
@@ -283,7 +283,7 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="15" column="1">
+          <item row="14" column="1">
            <widget class="QLabel" name="label_flycapture2">
             <property name="text">
              <string/>
@@ -293,14 +293,14 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="18" column="0">
+          <item row="17" column="0">
            <widget class="QLabel" name="label_18">
             <property name="text">
              <string>With cvsba :</string>
             </property>
            </widget>
           </item>
-          <item row="18" column="1">
+          <item row="17" column="1">
            <widget class="QLabel" name="label_cvsba">
             <property name="text">
              <string/>
@@ -310,14 +310,14 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="17" column="0">
+          <item row="16" column="0">
            <widget class="QLabel" name="label_19">
             <property name="text">
              <string>With GTSAM :</string>
             </property>
            </widget>
           </item>
-          <item row="17" column="1">
+          <item row="16" column="1">
            <widget class="QLabel" name="label_gtsam">
             <property name="text">
              <string/>
@@ -327,23 +327,6 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="10" column="0">
-           <widget class="QLabel" name="label_20">
-            <property name="text">
-             <string>FLANN version :</string>
-            </property>
-           </widget>
-          </item>
-          <item row="10" column="1">
-           <widget class="QLabel" name="label_flann_version">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-            </property>
-           </widget>
-          </item>
          </layout>
         </item>
         <item>