Renamed all flann headers to avoid conflicts if flann is already installed on the computer

corelib/src/rtflann/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 RTABMAP_FLANN_ALLOCATOR_H_
+#define RTABMAP_FLANN_ALLOCATOR_H_
+
+#include <stdlib.h>
+#include <stdio.h>
+
+
+namespace rtflann
+{
+
+/**
+ * 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, rtflann::PooledAllocator& allocator)
+{
+    return allocator.allocateMemory(size) ;
+}
+
+#endif //FLANN_ALLOCATOR_H_

corelib/src/rtflann/util/any.h

@@ -0,0 +1,294 @@
+#ifndef RTABMAP_FLANN_ANY_H_
+#define RTABMAP_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 rtflann
+{
+
+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/rtflann/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 RTABMAP_FLANN_DYNAMIC_BITSET_H_
+#define RTABMAP_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 rtflann {
+
+/** 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/rtflann/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 RTABMAP_FLANN_HEAP_H_
+#define RTABMAP_FLANN_HEAP_H_
+
+#include <algorithm>
+#include <vector>
+
+namespace rtflann
+{
+
+/**
+ * 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/rtflann/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 RTABMAP_FLANN_LOGGER_H
+#define RTABMAP_FLANN_LOGGER_H
+
+#include <stdio.h>
+#include <stdarg.h>
+
+#include "rtflann/defines.h"
+
+
+namespace rtflann
+{
+
+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 //RTABMAP_FLANN_LOGGER_H

corelib/src/rtflann/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 RTABMAP_FLANN_LSH_TABLE_H_
+#define RTABMAP_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 "rtflann/util/dynamic_bitset.h"
+#include "rtflann/util/matrix.h"
+
+namespace rtflann
+{
+
+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/rtflann/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 RTABMAP_FLANN_DATASET_H_
+#define RTABMAP_FLANN_DATASET_H_
+
+#include "rtflann/general.h"
+#include "rtflann/util/serialization.h"
+#include <stdio.h>
+
+namespace rtflann
+{
+
+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/rtflann/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 RTABMAP_FLANN_OBJECT_FACTORY_H_
+#define RTABMAP_FLANN_OBJECT_FACTORY_H_
+
+#include <map>
+
+namespace rtflann
+{
+
+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/rtflann/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 RTABMAP_FLANN_PARAMS_H_
+#define RTABMAP_FLANN_PARAMS_H_
+
+#include "rtflann/util/any.h"
+#include "rtflann/general.h"
+#include <iostream>
+#include <map>
+
+
+namespace rtflann
+{
+
+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/rtflann/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 RTABMAP_FLANN_RANDOM_H
+#define RTABMAP_FLANN_RANDOM_H
+
+#include <algorithm>
+#include <cstdlib>
+#include <cstddef>
+#include <vector>
+
+#include "rtflann/general.h"
+
+namespace rtflann
+{
+
+/**
+ * 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/rtflann/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 RTABMAP_FLANN_RESULTSET_H
+#define RTABMAP_FLANN_RESULTSET_H
+
+#include <algorithm>
+#include <cstring>
+#include <iostream>
+#include <limits>
+#include <set>
+#include <vector>
+
+namespace rtflann
+{
+
+/* 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/rtflann/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 RTABMAP_FLANN_SAMPLING_H_
+#define RTABMAP_FLANN_SAMPLING_H_
+
+#include "rtflann/util/matrix.h"
+#include "rtflann/util/random.h"
+
+namespace rtflann
+{
+
+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/rtflann/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 RTABMAP_FLANN_SAVING_H_
+#define RTABMAP_FLANN_SAVING_H_
+
+#include <cstring>
+#include <vector>
+#include <stdio.h>
+
+#include "rtflann/general.h"
+#include "rtflann/util/serialization.h"
+
+
+#ifdef FLANN_SIGNATURE_
+#undef FLANN_SIGNATURE_
+#endif
+#define FLANN_SIGNATURE_ "FLANN_INDEX_v1.1"
+
+namespace rtflann
+{
+
+/**
+ * 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/rtflann/util/serialization.h

@@ -0,0 +1,813 @@
+#ifndef RTABMAP_SERIALIZATION_H_
+#define RTABMAP_SERIALIZATION_H_
+
+#include <vector>
+#include <map>
+#include <cstdlib>
+#include <cstring>
+#include <stdio.h>
+#include "rtflann/ext/lz4.h"
+#include "rtflann/ext/lz4hc.h"
+
+
+namespace rtflann
+{
+    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/rtflann/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 RTABMAP_FLANN_TIMER_H
+#define RTABMAP_FLANN_TIMER_H
+
+#include <time.h>
+
+
+namespace rtflann
+{
+
+/**
+ * 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