i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2015-09-12 01:36:17 -04:00
parent cf0d781020
commit a45aa5d8f7
61 changed files with 266 additions and 7186 deletions
Download Patch File Download Diff File Expand all files Collapse all files

197
corelib/src/rtflann/algorithms/all_indices.h Normal file
View File

@@ -0,0 +1,197 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_ALL_INDICES_H_
#define RTABMAP_FLANN_ALL_INDICES_H_
#include "rtflann/general.h"
#include "rtflann/algorithms/nn_index.h"
#include "rtflann/algorithms/kdtree_index.h"
#include "rtflann/algorithms/kdtree_single_index.h"
#include "rtflann/algorithms/kmeans_index.h"
#include "rtflann/algorithms/composite_index.h"
#include "rtflann/algorithms/linear_index.h"
#include "rtflann/algorithms/hierarchical_clustering_index.h"
#include "rtflann/algorithms/lsh_index.h"
#include "rtflann/algorithms/autotuned_index.h"
#ifdef FLANN_USE_CUDA
#include "rtflann/algorithms/kdtree_cuda_3d_index.h"
#endif
namespace rtflann
{
/**
* enable_if sfinae helper
*/
template<bool, typename T = void> struct enable_if{};
template<typename T> struct enable_if<true,T> { typedef T type; };
/**
* disable_if sfinae helper
*/
template<bool, typename T> struct disable_if{ typedef T type; };
template<typename T> struct disable_if<true,T> { };
/**
* Check if two type are the same
*/
template <typename T, typename U>
struct same_type
{
enum {value = false};
};
template<typename T>
struct same_type<T,T>
{
enum {value = true};
};
#define HAS_MEMBER(member) \
template<typename T> \
struct member { \
typedef char No; \
typedef long Yes; \
template<typename C> static Yes test( typename C::member* ); \
template<typename C> static No test( ... ); \
enum { value = sizeof (test<T>(0))==sizeof(Yes) }; \
};
HAS_MEMBER(needs_kdtree_distance)
HAS_MEMBER(needs_vector_space_distance)
HAS_MEMBER(is_kdtree_distance)
HAS_MEMBER(is_vector_space_distance)
struct DummyDistance
{
typedef float ElementType;
typedef float ResultType;
template <typename Iterator1, typename Iterator2>
ResultType operator()(Iterator1 a, Iterator2 b, size_t size, ResultType /*worst_dist*/ = -1) const
{
return ResultType(0);
}
template <typename U, typename V>
inline ResultType accum_dist(const U& a, const V& b, int) const
{
return ResultType(0);
}
};
/**
* Checks if an index and a distance can be used together
*/
template<template <typename> class Index, typename Distance, typename ElemType>
struct valid_combination
{
static const bool value = same_type<ElemType,typename Distance::ElementType>::value &&
(!needs_kdtree_distance<Index<DummyDistance> >::value || is_kdtree_distance<Distance>::value) &&
(!needs_vector_space_distance<Index<DummyDistance> >::value || is_kdtree_distance<Distance>::value || is_vector_space_distance<Distance>::value);
};
/*********************************************************
* Create index
**********************************************************/
template <template<typename> class Index, typename Distance, typename T>
inline NNIndex<Distance>* create_index_(rtflann::Matrix<T> data, const rtflann::IndexParams& params, const Distance& distance,
typename enable_if<valid_combination<Index,Distance,T>::value,void>::type* = 0)
{
return new Index<Distance>(data, params, distance);
}
template <template<typename> class Index, typename Distance, typename T>
inline NNIndex<Distance>* create_index_(rtflann::Matrix<T> data, const rtflann::IndexParams& params, const Distance& distance,
typename disable_if<valid_combination<Index,Distance,T>::value,void>::type* = 0)
{
return NULL;
}
template<typename Distance>
inline NNIndex<Distance>*
create_index_by_type(const flann_algorithm_t index_type,
const Matrix<typename Distance::ElementType>& dataset, const IndexParams& params, const Distance& distance)
{
typedef typename Distance::ElementType ElementType;
NNIndex<Distance>* nnIndex;
switch (index_type) {
case FLANN_INDEX_LINEAR:
nnIndex = create_index_<LinearIndex,Distance,ElementType>(dataset, params, distance);
break;
case FLANN_INDEX_KDTREE_SINGLE:
nnIndex = create_index_<KDTreeSingleIndex,Distance,ElementType>(dataset, params, distance);
break;
case FLANN_INDEX_KDTREE:
nnIndex = create_index_<KDTreeIndex,Distance,ElementType>(dataset, params, distance);
break;
//! #define this symbol before including flann.h to enable GPU search algorithms. But you have
//! to link libflann_cuda then!
#ifdef FLANN_USE_CUDA
case FLANN_INDEX_KDTREE_CUDA:
nnIndex = create_index_<KDTreeCuda3dIndex,Distance,ElementType>(dataset, params, distance);
break;
#endif
case FLANN_INDEX_KMEANS:
nnIndex = create_index_<KMeansIndex,Distance,ElementType>(dataset, params, distance);
break;
case FLANN_INDEX_COMPOSITE:
nnIndex = create_index_<CompositeIndex,Distance,ElementType>(dataset, params, distance);
break;
case FLANN_INDEX_AUTOTUNED:
nnIndex = create_index_<AutotunedIndex,Distance,ElementType>(dataset, params, distance);
break;
case FLANN_INDEX_HIERARCHICAL:
nnIndex = create_index_<HierarchicalClusteringIndex,Distance,ElementType>(dataset, params, distance);
break;
case FLANN_INDEX_LSH:
nnIndex = create_index_<LshIndex,Distance,ElementType>(dataset, params, distance);
break;
default:
throw FLANNException("Unknown index type");
}
if (nnIndex==NULL) {
throw FLANNException("Unsupported index/distance combination");
}
return nnIndex;
}
}
#endif /* RTABMAP_FLANN_ALL_INDICES_H_ */

763
corelib/src/rtflann/algorithms/autotuned_index.h Normal file
View File

@@ -0,0 +1,763 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_AUTOTUNED_INDEX_H_
#define RTABMAP_FLANN_AUTOTUNED_INDEX_H_
#include "rtflann/general.h"
#include "rtflann/algorithms/nn_index.h"
#include "rtflann/nn/ground_truth.h"
#include "rtflann/nn/index_testing.h"
#include "rtflann/util/sampling.h"
#include "rtflann/algorithms/kdtree_index.h"
#include "rtflann/algorithms/kdtree_single_index.h"
#include "rtflann/algorithms/kmeans_index.h"
#include "rtflann/algorithms/composite_index.h"
#include "rtflann/algorithms/linear_index.h"
#include "rtflann/util/logger.h"
namespace rtflann
{
template<typename Distance>
inline NNIndex<Distance>*
create_index_by_type(const flann_algorithm_t index_type,
const Matrix<typename Distance::ElementType>& dataset, const IndexParams& params, const Distance& distance = Distance());
struct AutotunedIndexParams : public IndexParams
{
AutotunedIndexParams(float target_precision = 0.8, float build_weight = 0.01, float memory_weight = 0, float sample_fraction = 0.1)
{
(*this)["algorithm"] = FLANN_INDEX_AUTOTUNED;
// precision desired (used for autotuning, -1 otherwise)
(*this)["target_precision"] = target_precision;
// build tree time weighting factor
(*this)["build_weight"] = build_weight;
// index memory weighting factor
(*this)["memory_weight"] = memory_weight;
// what fraction of the dataset to use for autotuning
(*this)["sample_fraction"] = sample_fraction;
}
};
template <typename Distance>
class AutotunedIndex : public NNIndex<Distance>
{
public:
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
typedef NNIndex<Distance> BaseClass;
typedef AutotunedIndex<Distance> IndexType;
typedef bool needs_kdtree_distance;
AutotunedIndex(const Matrix<ElementType>& inputData, const IndexParams& params = AutotunedIndexParams(), Distance d = Distance()) :
BaseClass(params, d), bestIndex_(NULL), speedup_(0), dataset_(inputData)
{
target_precision_ = get_param(params, "target_precision",0.8f);
build_weight_ = get_param(params,"build_weight", 0.01f);
memory_weight_ = get_param(params, "memory_weight", 0.0f);
sample_fraction_ = get_param(params,"sample_fraction", 0.1f);
}
AutotunedIndex(const IndexParams& params = AutotunedIndexParams(), Distance d = Distance()) :
BaseClass(params, d), bestIndex_(NULL), speedup_(0)
{
target_precision_ = get_param(params, "target_precision",0.8f);
build_weight_ = get_param(params,"build_weight", 0.01f);
memory_weight_ = get_param(params, "memory_weight", 0.0f);
sample_fraction_ = get_param(params,"sample_fraction", 0.1f);
}
AutotunedIndex(const AutotunedIndex& other) : BaseClass(other),
bestParams_(other.bestParams_),
bestSearchParams_(other.bestSearchParams_),
speedup_(other.speedup_),
dataset_(other.dataset_),
target_precision_(other.target_precision_),
build_weight_(other.build_weight_),
memory_weight_(other.memory_weight_),
sample_fraction_(other.sample_fraction_)
{
bestIndex_ = other.bestIndex_->clone();
}
AutotunedIndex& operator=(AutotunedIndex other)
{
this->swap(other);
return * this;
}
virtual ~AutotunedIndex()
{
delete bestIndex_;
}
BaseClass* clone() const
{
return new AutotunedIndex(*this);
}
/**
* Method responsible with building the index.
*/
void buildIndex()
{
bestParams_ = estimateBuildParams();
Logger::info("----------------------------------------------------\n");
Logger::info("Autotuned parameters:\n");
if (Logger::getLevel()>=FLANN_LOG_INFO)
print_params(bestParams_);
Logger::info("----------------------------------------------------\n");
flann_algorithm_t index_type = get_param<flann_algorithm_t>(bestParams_,"algorithm");
bestIndex_ = create_index_by_type(index_type, dataset_, bestParams_, distance_);
bestIndex_->buildIndex();
speedup_ = estimateSearchParams(bestSearchParams_);
Logger::info("----------------------------------------------------\n");
Logger::info("Search parameters:\n");
if (Logger::getLevel()>=FLANN_LOG_INFO)
print_params(bestSearchParams_);
Logger::info("----------------------------------------------------\n");
bestParams_["search_params"] = bestSearchParams_;
bestParams_["speedup"] = speedup_;
}
void buildIndex(const Matrix<ElementType>& dataset)
{
dataset_ = dataset;
this->buildIndex();
}
void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{
if (bestIndex_) {
bestIndex_->addPoints(points, rebuild_threshold);
}
}
void removePoint(size_t id)
{
if (bestIndex_) {
bestIndex_->removePoint(id);
}
}
template<typename Archive>
void serialize(Archive& ar)
{
ar.setObject(this);
ar & *static_cast<NNIndex<Distance>*>(this);
ar & target_precision_;
ar & build_weight_;
ar & memory_weight_;
ar & sample_fraction_;
flann_algorithm_t index_type;
if (Archive::is_saving::value) {
index_type = get_param<flann_algorithm_t>(bestParams_,"algorithm");
}
ar & index_type;
ar & bestSearchParams_.checks;
if (Archive::is_loading::value) {
bestParams_["algorithm"] = index_type;
index_params_["algorithm"] = getType();
index_params_["target_precision_"] = target_precision_;
index_params_["build_weight_"] = build_weight_;
index_params_["memory_weight_"] = memory_weight_;
index_params_["sample_fraction_"] = sample_fraction_;
}
}
void saveIndex(FILE* stream)
{
{
serialization::SaveArchive sa(stream);
sa & *this;
}
bestIndex_->saveIndex(stream);
}
void loadIndex(FILE* stream)
{
{
serialization::LoadArchive la(stream);
la & *this;
}
IndexParams params;
flann_algorithm_t index_type = get_param<flann_algorithm_t>(bestParams_,"algorithm");
bestIndex_ = create_index_by_type<Distance>((flann_algorithm_t)index_type, dataset_, params, distance_);
bestIndex_->loadIndex(stream);
}
int knnSearch(const Matrix<ElementType>& queries,
Matrix<size_t>& indices,
Matrix<DistanceType>& dists,
size_t knn,
const SearchParams& params) const
{
if (params.checks == FLANN_CHECKS_AUTOTUNED) {
return bestIndex_->knnSearch(queries, indices, dists, knn, bestSearchParams_);
}
else {
return bestIndex_->knnSearch(queries, indices, dists, knn, params);
}
}
int knnSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<size_t> >& indices,
std::vector<std::vector<DistanceType> >& dists,
size_t knn,
const SearchParams& params) const
{
if (params.checks == FLANN_CHECKS_AUTOTUNED) {
return bestIndex_->knnSearch(queries, indices, dists, knn, bestSearchParams_);
}
else {
return bestIndex_->knnSearch(queries, indices, dists, knn, params);
}
}
int radiusSearch(const Matrix<ElementType>& queries,
Matrix<size_t>& indices,
Matrix<DistanceType>& dists,
DistanceType radius,
const SearchParams& params) const
{
if (params.checks == FLANN_CHECKS_AUTOTUNED) {
return bestIndex_->radiusSearch(queries, indices, dists, radius, bestSearchParams_);
}
else {
return bestIndex_->radiusSearch(queries, indices, dists, radius, params);
}
}
int radiusSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<size_t> >& indices,
std::vector<std::vector<DistanceType> >& dists,
DistanceType radius,
const SearchParams& params) const
{
if (params.checks == FLANN_CHECKS_AUTOTUNED) {
return bestIndex_->radiusSearch(queries, indices, dists, radius, bestSearchParams_);
}
else {
return bestIndex_->radiusSearch(queries, indices, dists, radius, params);
}
}
/**
* Method that searches for nearest-neighbors
*/
void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
{
// should not get here
assert(false);
}
IndexParams getParameters() const
{
return bestParams_;
}
FLANN_DEPRECATED SearchParams getSearchParameters() const
{
return bestSearchParams_;
}
FLANN_DEPRECATED float getSpeedup() const
{
return speedup_;
}
/**
* Number of features in this index.
*/
size_t size() const
{
return bestIndex_->size();
}
/**
* The length of each vector in this index.
*/
size_t veclen() const
{
return bestIndex_->veclen();
}
/**
* The amount of memory (in bytes) this index uses.
*/
int usedMemory() const
{
return bestIndex_->usedMemory();
}
/**
* Algorithm name
*/
flann_algorithm_t getType() const
{
return FLANN_INDEX_AUTOTUNED;
}
protected:
void buildIndexImpl()
{
/* nothing to do here */
}
void freeIndex()
{
/* nothing to do here */
}
private:
struct CostData
{
float searchTimeCost;
float buildTimeCost;
float memoryCost;
float totalCost;
IndexParams params;
};
void evaluate_kmeans(CostData& cost)
{
StartStopTimer t;
int checks;
const int nn = 1;
Logger::info("KMeansTree using params: max_iterations=%d, branching=%d\n",
get_param<int>(cost.params,"iterations"),
get_param<int>(cost.params,"branching"));
KMeansIndex<Distance> kmeans(sampledDataset_, cost.params, distance_);
// measure index build time
t.start();
kmeans.buildIndex();
t.stop();
float buildTime = (float)t.value;
// measure search time
float searchTime = test_index_precision(kmeans, sampledDataset_, testDataset_, gt_matches_, target_precision_, checks, distance_, nn);
float datasetMemory = float(sampledDataset_.rows * sampledDataset_.cols * sizeof(float));
cost.memoryCost = (kmeans.usedMemory() + datasetMemory) / datasetMemory;
cost.searchTimeCost = searchTime;
cost.buildTimeCost = buildTime;
Logger::info("KMeansTree buildTime=%g, searchTime=%g, build_weight=%g\n", buildTime, searchTime, build_weight_);
}
void evaluate_kdtree(CostData& cost)
{
StartStopTimer t;
int checks;
const int nn = 1;
Logger::info("KDTree using params: trees=%d\n", get_param<int>(cost.params,"trees"));
KDTreeIndex<Distance> kdtree(sampledDataset_, cost.params, distance_);
t.start();
kdtree.buildIndex();
t.stop();
float buildTime = (float)t.value;
//measure search time
float searchTime = test_index_precision(kdtree, sampledDataset_, testDataset_, gt_matches_, target_precision_, checks, distance_, nn);
float datasetMemory = float(sampledDataset_.rows * sampledDataset_.cols * sizeof(float));
cost.memoryCost = (kdtree.usedMemory() + datasetMemory) / datasetMemory;
cost.searchTimeCost = searchTime;
cost.buildTimeCost = buildTime;
Logger::info("KDTree buildTime=%g, searchTime=%g\n", buildTime, searchTime);
}
// struct KMeansSimpleDownhillFunctor {
//
// Autotune& autotuner;
// KMeansSimpleDownhillFunctor(Autotune& autotuner_) : autotuner(autotuner_) {};
//
// float operator()(int* params) {
//
// float maxFloat = numeric_limits<float>::max();
//
// if (params[0]<2) return maxFloat;
// if (params[1]<0) return maxFloat;
//
// CostData c;
// c.params["algorithm"] = KMEANS;
// c.params["centers-init"] = CENTERS_RANDOM;
// c.params["branching"] = params[0];
// c.params["max-iterations"] = params[1];
//
// autotuner.evaluate_kmeans(c);
//
// return c.timeCost;
//
// }
// };
//
// struct KDTreeSimpleDownhillFunctor {
//
// Autotune& autotuner;
// KDTreeSimpleDownhillFunctor(Autotune& autotuner_) : autotuner(autotuner_) {};
//
// float operator()(int* params) {
// float maxFloat = numeric_limits<float>::max();
//
// if (params[0]<1) return maxFloat;
//
// CostData c;
// c.params["algorithm"] = KDTREE;
// c.params["trees"] = params[0];
//
// autotuner.evaluate_kdtree(c);
//
// return c.timeCost;
//
// }
// };
void optimizeKMeans(std::vector<CostData>& costs)
{
Logger::info("KMEANS, Step 1: Exploring parameter space\n");
// explore kmeans parameters space using combinations of the parameters below
int maxIterations[] = { 1, 5, 10, 15 };
int branchingFactors[] = { 16, 32, 64, 128, 256 };
int kmeansParamSpaceSize = FLANN_ARRAY_LEN(maxIterations) * FLANN_ARRAY_LEN(branchingFactors);
costs.reserve(costs.size() + kmeansParamSpaceSize);
// evaluate kmeans for all parameter combinations
for (size_t i = 0; i < FLANN_ARRAY_LEN(maxIterations); ++i) {
for (size_t j = 0; j < FLANN_ARRAY_LEN(branchingFactors); ++j) {
CostData cost;
cost.params["algorithm"] = FLANN_INDEX_KMEANS;
cost.params["centers_init"] = FLANN_CENTERS_RANDOM;
cost.params["iterations"] = maxIterations[i];
cost.params["branching"] = branchingFactors[j];
evaluate_kmeans(cost);
costs.push_back(cost);
}
}
// Logger::info("KMEANS, Step 2: simplex-downhill optimization\n");
//
// const int n = 2;
// // choose initial simplex points as the best parameters so far
// int kmeansNMPoints[n*(n+1)];
// float kmeansVals[n+1];
// for (int i=0;i<n+1;++i) {
// kmeansNMPoints[i*n] = (int)kmeansCosts[i].params["branching"];
// kmeansNMPoints[i*n+1] = (int)kmeansCosts[i].params["max-iterations"];
// kmeansVals[i] = kmeansCosts[i].timeCost;
// }
// KMeansSimpleDownhillFunctor kmeans_cost_func(*this);
// // run optimization
// optimizeSimplexDownhill(kmeansNMPoints,n,kmeans_cost_func,kmeansVals);
// // store results
// for (int i=0;i<n+1;++i) {
// kmeansCosts[i].params["branching"] = kmeansNMPoints[i*2];
// kmeansCosts[i].params["max-iterations"] = kmeansNMPoints[i*2+1];
// kmeansCosts[i].timeCost = kmeansVals[i];
// }
}
void optimizeKDTree(std::vector<CostData>& costs)
{
Logger::info("KD-TREE, Step 1: Exploring parameter space\n");
// explore kd-tree parameters space using the parameters below
int testTrees[] = { 1, 4, 8, 16, 32 };
// evaluate kdtree for all parameter combinations
for (size_t i = 0; i < FLANN_ARRAY_LEN(testTrees); ++i) {
CostData cost;
cost.params["algorithm"] = FLANN_INDEX_KDTREE;
cost.params["trees"] = testTrees[i];
evaluate_kdtree(cost);
costs.push_back(cost);
}
// Logger::info("KD-TREE, Step 2: simplex-downhill optimization\n");
//
// const int n = 1;
// // choose initial simplex points as the best parameters so far
// int kdtreeNMPoints[n*(n+1)];
// float kdtreeVals[n+1];
// for (int i=0;i<n+1;++i) {
// kdtreeNMPoints[i] = (int)kdtreeCosts[i].params["trees"];
// kdtreeVals[i] = kdtreeCosts[i].timeCost;
// }
// KDTreeSimpleDownhillFunctor kdtree_cost_func(*this);
// // run optimization
// optimizeSimplexDownhill(kdtreeNMPoints,n,kdtree_cost_func,kdtreeVals);
// // store results
// for (int i=0;i<n+1;++i) {
// kdtreeCosts[i].params["trees"] = kdtreeNMPoints[i];
// kdtreeCosts[i].timeCost = kdtreeVals[i];
// }
}
/**
* Chooses the best nearest-neighbor algorithm and estimates the optimal
* parameters to use when building the index (for a given precision).
* Returns a dictionary with the optimal parameters.
*/
IndexParams estimateBuildParams()
{
std::vector<CostData> costs;
int sampleSize = int(sample_fraction_ * dataset_.rows);
int testSampleSize = std::min(sampleSize / 10, 1000);
Logger::info("Entering autotuning, dataset size: %d, sampleSize: %d, testSampleSize: %d, target precision: %g\n", dataset_.rows, sampleSize, testSampleSize, target_precision_);
// For a very small dataset, it makes no sense to build any fancy index, just
// use linear search
if (testSampleSize < 10) {
Logger::info("Choosing linear, dataset too small\n");
return LinearIndexParams();
}
// We use a fraction of the original dataset to speedup the autotune algorithm
sampledDataset_ = random_sample(dataset_, sampleSize);
// We use a cross-validation approach, first we sample a testset from the dataset
testDataset_ = random_sample(sampledDataset_, testSampleSize, true);
// We compute the ground truth using linear search
Logger::info("Computing ground truth... \n");
gt_matches_ = Matrix<size_t>(new size_t[testDataset_.rows], testDataset_.rows, 1);
StartStopTimer t;
int repeats = 0;
t.reset();
while (t.value<0.2) {
repeats++;
t.start();
compute_ground_truth<Distance>(sampledDataset_, testDataset_, gt_matches_, 0, distance_);
t.stop();
}
CostData linear_cost;
linear_cost.searchTimeCost = (float)t.value/repeats;
linear_cost.buildTimeCost = 0;
linear_cost.memoryCost = 0;
linear_cost.params["algorithm"] = FLANN_INDEX_LINEAR;
costs.push_back(linear_cost);
// Start parameter autotune process
Logger::info("Autotuning parameters...\n");
optimizeKMeans(costs);
optimizeKDTree(costs);
float bestTimeCost = costs[0].buildTimeCost * build_weight_ + costs[0].searchTimeCost;
for (size_t i = 0; i < costs.size(); ++i) {
float timeCost = costs[i].buildTimeCost * build_weight_ + costs[i].searchTimeCost;
Logger::debug("Time cost: %g\n", timeCost);
if (timeCost < bestTimeCost) {
bestTimeCost = timeCost;
}
}
Logger::debug("Best time cost: %g\n", bestTimeCost);
IndexParams bestParams = costs[0].params;
if (bestTimeCost > 0) {
float bestCost = (costs[0].buildTimeCost * build_weight_ + costs[0].searchTimeCost) / bestTimeCost;
for (size_t i = 0; i < costs.size(); ++i) {
float crtCost = (costs[i].buildTimeCost * build_weight_ + costs[i].searchTimeCost) / bestTimeCost +
memory_weight_ * costs[i].memoryCost;
Logger::debug("Cost: %g\n", crtCost);
if (crtCost < bestCost) {
bestCost = crtCost;
bestParams = costs[i].params;
}
}
Logger::debug("Best cost: %g\n", bestCost);
}
delete[] gt_matches_.ptr();
delete[] testDataset_.ptr();
delete[] sampledDataset_.ptr();
return bestParams;
}
/**
* Estimates the search time parameters needed to get the desired precision.
* Precondition: the index is built
* Postcondition: the searchParams will have the optimum params set, also the speedup obtained over linear search.
*/
float estimateSearchParams(SearchParams& searchParams)
{
const int nn = 1;
const size_t SAMPLE_COUNT = 1000;
assert(bestIndex_ != NULL); // must have a valid index
float speedup = 0;
int samples = (int)std::min(dataset_.rows / 10, SAMPLE_COUNT);
if (samples > 0) {
Matrix<ElementType> testDataset = random_sample(dataset_, samples);
Logger::info("Computing ground truth\n");
// we need to compute the ground truth first
Matrix<size_t> gt_matches(new size_t[testDataset.rows], testDataset.rows, 1);
StartStopTimer t;
int repeats = 0;
t.reset();
while (t.value<0.2) {
repeats++;
t.start();
compute_ground_truth<Distance>(dataset_, testDataset, gt_matches, 1, distance_);
t.stop();
}
float linear = (float)t.value/repeats;
int checks;
Logger::info("Estimating number of checks\n");
float searchTime;
float cb_index;
if (bestIndex_->getType() == FLANN_INDEX_KMEANS) {
Logger::info("KMeans algorithm, estimating cluster border factor\n");
KMeansIndex<Distance>* kmeans = static_cast<KMeansIndex<Distance>*>(bestIndex_);
float bestSearchTime = -1;
float best_cb_index = -1;
int best_checks = -1;
for (cb_index = 0; cb_index < 1.1f; cb_index += 0.2f) {
kmeans->set_cb_index(cb_index);
searchTime = test_index_precision(*kmeans, dataset_, testDataset, gt_matches, target_precision_, checks, distance_, nn, 1);
if ((searchTime < bestSearchTime) || (bestSearchTime == -1)) {
bestSearchTime = searchTime;
best_cb_index = cb_index;
best_checks = checks;
}
}
searchTime = bestSearchTime;
cb_index = best_cb_index;
checks = best_checks;
kmeans->set_cb_index(best_cb_index);
Logger::info("Optimum cb_index: %g\n", cb_index);
bestParams_["cb_index"] = cb_index;
}
else {
searchTime = test_index_precision(*bestIndex_, dataset_, testDataset, gt_matches, target_precision_, checks, distance_, nn, 1);
}
Logger::info("Required number of checks: %d \n", checks);
searchParams.checks = checks;
speedup = linear / searchTime;
delete[] gt_matches.ptr();
delete[] testDataset.ptr();
}
return speedup;
}
void swap(AutotunedIndex& other)
{
BaseClass::swap(other);
std::swap(bestIndex_, other.bestIndex_);
std::swap(bestParams_, other.bestParams_);
std::swap(bestSearchParams_, other.bestSearchParams_);
std::swap(speedup_, other.speedup_);
std::swap(dataset_, other.dataset_);
std::swap(target_precision_, other.target_precision_);
std::swap(build_weight_, other.build_weight_);
std::swap(memory_weight_, other.memory_weight_);
std::swap(sample_fraction_, other.sample_fraction_);
}
private:
NNIndex<Distance>* bestIndex_;
IndexParams bestParams_;
SearchParams bestSearchParams_;
Matrix<ElementType> sampledDataset_;
Matrix<ElementType> testDataset_;
Matrix<size_t> gt_matches_;
float speedup_;
/**
* The dataset used by this index
*/
Matrix<ElementType> dataset_;
/**
* Index parameters
*/
float target_precision_;
float build_weight_;
float memory_weight_;
float sample_fraction_;
USING_BASECLASS_SYMBOLS
};
}
#endif /* RTABMAP_FLANN_AUTOTUNED_INDEX_H_ */

385
corelib/src/rtflann/algorithms/center_chooser.h Normal file
View File

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

239
corelib/src/rtflann/algorithms/composite_index.h Normal file
View File

@@ -0,0 +1,239 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_COMPOSITE_INDEX_H_
#define RTABMAP_FLANN_COMPOSITE_INDEX_H_
#include "rtflann/general.h"
#include "rtflann/algorithms/nn_index.h"
#include "rtflann/algorithms/kdtree_index.h"
#include "rtflann/algorithms/kmeans_index.h"
namespace rtflann
{
/**
* Index parameters for the CompositeIndex.
*/
struct CompositeIndexParams : public IndexParams
{
CompositeIndexParams(int trees = 4, int branching = 32, int iterations = 11,
flann_centers_init_t centers_init = FLANN_CENTERS_RANDOM, float cb_index = 0.2 )
{
(*this)["algorithm"] = FLANN_INDEX_KMEANS;
// number of randomized trees to use (for kdtree)
(*this)["trees"] = trees;
// branching factor
(*this)["branching"] = branching;
// max iterations to perform in one kmeans clustering (kmeans tree)
(*this)["iterations"] = iterations;
// algorithm used for picking the initial cluster centers for kmeans tree
(*this)["centers_init"] = centers_init;
// cluster boundary index. Used when searching the kmeans tree
(*this)["cb_index"] = cb_index;
}
};
/**
* This index builds a kd-tree index and a k-means index and performs nearest
* neighbour search both indexes. This gives a slight boost in search performance
* as some of the neighbours that are missed by one index are found by the other.
*/
template <typename Distance>
class CompositeIndex : public NNIndex<Distance>
{
public:
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
typedef NNIndex<Distance> BaseClass;
typedef bool needs_kdtree_distance;
/**
* Index constructor
* @param inputData dataset containing the points to index
* @param params Index parameters
* @param d Distance functor
* @return
*/
CompositeIndex(const IndexParams& params = CompositeIndexParams(), Distance d = Distance()) :
BaseClass(params, d)
{
kdtree_index_ = new KDTreeIndex<Distance>(params, d);
kmeans_index_ = new KMeansIndex<Distance>(params, d);
}
CompositeIndex(const Matrix<ElementType>& inputData, const IndexParams& params = CompositeIndexParams(),
Distance d = Distance()) : BaseClass(params, d)
{
kdtree_index_ = new KDTreeIndex<Distance>(inputData, params, d);
kmeans_index_ = new KMeansIndex<Distance>(inputData, params, d);
}
CompositeIndex(const CompositeIndex& other) : BaseClass(other),
kmeans_index_(other.kmeans_index_), kdtree_index_(other.kdtree_index_)
{
}
CompositeIndex& operator=(CompositeIndex other)
{
this->swap(other);
return *this;
}
virtual ~CompositeIndex()
{
delete kdtree_index_;
delete kmeans_index_;
}
BaseClass* clone() const
{
return new CompositeIndex(*this);
}
/**
* @return The index type
*/
flann_algorithm_t getType() const
{
return FLANN_INDEX_COMPOSITE;
}
/**
* @return Size of the index
*/
size_t size() const
{
return kdtree_index_->size();
}
/**
* \returns The dimensionality of the features in this index.
*/
size_t veclen() const
{
return kdtree_index_->veclen();
}
/**
* \returns The amount of memory (in bytes) used by the index.
*/
int usedMemory() const
{
return kmeans_index_->usedMemory() + kdtree_index_->usedMemory();
}
using NNIndex<Distance>::buildIndex;
/**
* \brief Builds the index
*/
void buildIndex()
{
Logger::info("Building kmeans tree...\n");
kmeans_index_->buildIndex();
Logger::info("Building kdtree tree...\n");
kdtree_index_->buildIndex();
}
void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{
kmeans_index_->addPoints(points, rebuild_threshold);
kdtree_index_->addPoints(points, rebuild_threshold);
}
void removePoint(size_t index)
{
kmeans_index_->removePoint(index);
kdtree_index_->removePoint(index);
}
/**
* \brief Saves the index to a stream
* \param stream The stream to save the index to
*/
void saveIndex(FILE* stream)
{
kmeans_index_->saveIndex(stream);
kdtree_index_->saveIndex(stream);
}
/**
* \brief Loads the index from a stream
* \param stream The stream from which the index is loaded
*/
void loadIndex(FILE* stream)
{
kmeans_index_->loadIndex(stream);
kdtree_index_->loadIndex(stream);
}
/**
* \brief Method that searches for nearest-neighbours
*/
void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
{
kmeans_index_->findNeighbors(result, vec, searchParams);
kdtree_index_->findNeighbors(result, vec, searchParams);
}
protected:
void swap(CompositeIndex& other)
{
std::swap(kmeans_index_, other.kmeans_index_);
std::swap(kdtree_index_, other.kdtree_index_);
}
void buildIndexImpl()
{
/* nothing to do here */
}
void freeIndex()
{
/* nothing to do here */
}
private:
/** The k-means index */
KMeansIndex<Distance>* kmeans_index_;
/** The kd-tree index */
KDTreeIndex<Distance>* kdtree_index_;
};
}
#endif //FLANN_COMPOSITE_INDEX_H_

790
corelib/src/rtflann/algorithms/dist.h Normal file
View File

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

724
corelib/src/rtflann/algorithms/hierarchical_clustering_index.h Normal file
View File

@@ -0,0 +1,724 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2011 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2011 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_HIERARCHICAL_CLUSTERING_INDEX_H_
#define RTABMAP_FLANN_HIERARCHICAL_CLUSTERING_INDEX_H_
#include <algorithm>
#include <string>
#include <map>
#include <cassert>
#include <limits>
#include <cmath>
#ifndef SIZE_MAX
#define SIZE_MAX ((size_t) -1)
#endif
#include "rtflann/general.h"
#include "rtflann/algorithms/nn_index.h"
#include "rtflann/algorithms/dist.h"
#include "rtflann/util/matrix.h"
#include "rtflann/util/result_set.h"
#include "rtflann/util/heap.h"
#include "rtflann/util/allocator.h"
#include "rtflann/util/random.h"
#include "rtflann/util/saving.h"
#include "rtflann/util/serialization.h"
namespace rtflann
{
struct HierarchicalClusteringIndexParams : public IndexParams
{
HierarchicalClusteringIndexParams(int branching = 32,
flann_centers_init_t centers_init = FLANN_CENTERS_RANDOM,
int trees = 4, int leaf_max_size = 100)
{
(*this)["algorithm"] = FLANN_INDEX_HIERARCHICAL;
// The branching factor used in the hierarchical clustering
(*this)["branching"] = branching;
// Algorithm used for picking the initial cluster centers
(*this)["centers_init"] = centers_init;
// number of parallel trees to build
(*this)["trees"] = trees;
// maximum leaf size
(*this)["leaf_max_size"] = leaf_max_size;
}
};
/**
* Hierarchical index
*
* Contains a tree constructed through a hierarchical clustering
* and other information for indexing a set of points for nearest-neighbour matching.
*/
template <typename Distance>
class HierarchicalClusteringIndex : public NNIndex<Distance>
{
public:
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
typedef NNIndex<Distance> BaseClass;
/**
* Constructor.
*
* @param index_params
* @param d
*/
HierarchicalClusteringIndex(const IndexParams& index_params = HierarchicalClusteringIndexParams(), Distance d = Distance())
: BaseClass(index_params, d)
{
memoryCounter_ = 0;
branching_ = get_param(index_params_,"branching",32);
centers_init_ = get_param(index_params_,"centers_init", FLANN_CENTERS_RANDOM);
trees_ = get_param(index_params_,"trees",4);
leaf_max_size_ = get_param(index_params_,"leaf_max_size",100);
initCenterChooser();
}
/**
* Index constructor
*
* Params:
* inputData = dataset with the input features
* params = parameters passed to the hierarchical k-means algorithm
*/
HierarchicalClusteringIndex(const Matrix<ElementType>& inputData, const IndexParams& index_params = HierarchicalClusteringIndexParams(),
Distance d = Distance())
: BaseClass(index_params, d)
{
memoryCounter_ = 0;
branching_ = get_param(index_params_,"branching",32);
centers_init_ = get_param(index_params_,"centers_init", FLANN_CENTERS_RANDOM);
trees_ = get_param(index_params_,"trees",4);
leaf_max_size_ = get_param(index_params_,"leaf_max_size",100);
initCenterChooser();
setDataset(inputData);
chooseCenters_->setDataSize(veclen_);
}
HierarchicalClusteringIndex(const HierarchicalClusteringIndex& other) : BaseClass(other),
memoryCounter_(other.memoryCounter_),
branching_(other.branching_),
trees_(other.trees_),
centers_init_(other.centers_init_),
leaf_max_size_(other.leaf_max_size_)
{
initCenterChooser();
tree_roots_.resize(other.tree_roots_.size());
for (size_t i=0;i<tree_roots_.size();++i) {
copyTree(tree_roots_[i], other.tree_roots_[i]);
}
}
HierarchicalClusteringIndex& operator=(HierarchicalClusteringIndex other)
{
this->swap(other);
return *this;
}
void initCenterChooser()
{
switch(centers_init_) {
case FLANN_CENTERS_RANDOM:
chooseCenters_ = new RandomCenterChooser<Distance>(distance_, points_);
break;
case FLANN_CENTERS_GONZALES:
chooseCenters_ = new GonzalesCenterChooser<Distance>(distance_, points_);
break;
case FLANN_CENTERS_KMEANSPP:
chooseCenters_ = new KMeansppCenterChooser<Distance>(distance_, points_);
break;
case FLANN_CENTERS_GROUPWISE:
chooseCenters_ = new GroupWiseCenterChooser<Distance>(distance_, points_);
break;
default:
throw FLANNException("Unknown algorithm for choosing initial centers.");
}
}
/**
* Index destructor.
*
* Release the memory used by the index.
*/
virtual ~HierarchicalClusteringIndex()
{
delete chooseCenters_;
freeIndex();
}
BaseClass* clone() const
{
return new HierarchicalClusteringIndex(*this);
}
/**
* Computes the inde memory usage
* Returns: memory used by the index
*/
int usedMemory() const
{
return pool_.usedMemory+pool_.wastedMemory+memoryCounter_;
}
using BaseClass::buildIndex;
void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{
assert(points.cols==veclen_);
size_t old_size = size_;
extendDataset(points);
if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
buildIndex();
}
else {
for (size_t i=0;i<points.rows;++i) {
for (int j = 0; j < trees_; j++) {
addPointToTree(tree_roots_[j], old_size + i);
}
}
}
}
flann_algorithm_t getType() const
{
return FLANN_INDEX_HIERARCHICAL;
}
template<typename Archive>
void serialize(Archive& ar)
{
ar.setObject(this);
ar & *static_cast<NNIndex<Distance>*>(this);
ar & branching_;
ar & trees_;
ar & centers_init_;
ar & leaf_max_size_;
if (Archive::is_loading::value) {
tree_roots_.resize(trees_);
}
for (size_t i=0;i<tree_roots_.size();++i) {
if (Archive::is_loading::value) {
tree_roots_[i] = new(pool_) Node();
}
ar & *tree_roots_[i];
}
if (Archive::is_loading::value) {
index_params_["algorithm"] = getType();
index_params_["branching"] = branching_;
index_params_["trees"] = trees_;
index_params_["centers_init"] = centers_init_;
index_params_["leaf_size"] = leaf_max_size_;
}
}
void saveIndex(FILE* stream)
{
serialization::SaveArchive sa(stream);
sa & *this;
}
void loadIndex(FILE* stream)
{
serialization::LoadArchive la(stream);
la & *this;
}
/**
* Find set of nearest neighbors to vec. Their indices are stored inside
* the result object.
*
* Params:
* result = the result object in which the indices of the nearest-neighbors are stored
* vec = the vector for which to search the nearest neighbors
* searchParams = parameters that influence the search algorithm (checks)
*/
void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
{
if (removed_) {
findNeighborsWithRemoved<true>(result, vec, searchParams);
}
else {
findNeighborsWithRemoved<false>(result, vec, searchParams);
}
}
protected:
/**
* Builds the index
*/
void buildIndexImpl()
{
chooseCenters_->setDataSize(veclen_);
if (branching_<2) {
throw FLANNException("Branching factor must be at least 2");
}
tree_roots_.resize(trees_);
std::vector<int> indices(size_);
for (int i=0; i<trees_; ++i) {
for (size_t j=0; j<size_; ++j) {
indices[j] = j;
}
tree_roots_[i] = new(pool_) Node();
computeClustering(tree_roots_[i], &indices[0], size_);
}
}
private:
struct PointInfo
{
/** Point index */
size_t index;
/** Point data */
ElementType* point;
private:
template<typename Archive>
void serialize(Archive& ar)
{
typedef HierarchicalClusteringIndex<Distance> Index;
Index* obj = static_cast<Index*>(ar.getObject());
ar & index;
// ar & point;
if (Archive::is_loading::value) {
point = obj->points_[index];
}
}
friend struct serialization::access;
};
/**
* Struture representing a node in the hierarchical k-means tree.
*/
struct Node
{
/**
* The cluster center
*/
ElementType* pivot;
size_t pivot_index;
/**
* Child nodes (only for non-terminal nodes)
*/
std::vector<Node*> childs;
/**
* Node points (only for terminal nodes)
*/
std::vector<PointInfo> points;
Node(){
pivot = NULL;
pivot_index = SIZE_MAX;
}
/**
* destructor
* calling Node destructor explicitly
*/
~Node()
{
for(size_t i=0; i<childs.size(); i++){
childs[i]->~Node();
pivot = NULL;
pivot_index = -1;
}
};
private:
template<typename Archive>
void serialize(Archive& ar)
{
typedef HierarchicalClusteringIndex<Distance> Index;
Index* obj = static_cast<Index*>(ar.getObject());
ar & pivot_index;
if (Archive::is_loading::value) {
if (pivot_index != SIZE_MAX)
pivot = obj->points_[pivot_index];
else
pivot = NULL;
}
size_t childs_size;
if (Archive::is_saving::value) {
childs_size = childs.size();
}
ar & childs_size;
if (childs_size==0) {
ar & points;
}
else {
if (Archive::is_loading::value) {
childs.resize(childs_size);
}
for (size_t i=0;i<childs_size;++i) {
if (Archive::is_loading::value) {
childs[i] = new(obj->pool_) Node();
}
ar & *childs[i];
}
}
}
friend struct serialization::access;
};
typedef Node* NodePtr;
/**
* Alias definition for a nicer syntax.
*/
typedef BranchStruct<NodePtr, DistanceType> BranchSt;
/**
* Clears Node tree
* calling Node destructor explicitly
*/
void freeIndex(){
for (size_t i=0; i<tree_roots_.size(); ++i) {
tree_roots_[i]->~Node();
}
pool_.free();
}
void copyTree(NodePtr& dst, const NodePtr& src)
{
dst = new(pool_) Node();
dst->pivot_index = src->pivot_index;
dst->pivot = points_[dst->pivot_index];
if (src->childs.size()==0) {
dst->points = src->points;
}
else {
dst->childs.resize(src->childs.size());
for (size_t i=0;i<src->childs.size();++i) {
copyTree(dst->childs[i], src->childs[i]);
}
}
}
void computeLabels(int* indices, int indices_length, int* centers, int centers_length, int* labels, DistanceType& cost)
{
cost = 0;
for (int i=0; i<indices_length; ++i) {
ElementType* point = points_[indices[i]];
DistanceType dist = distance_(point, points_[centers[0]], veclen_);
labels[i] = 0;
for (int j=1; j<centers_length; ++j) {
DistanceType new_dist = distance_(point, points_[centers[j]], veclen_);
if (dist>new_dist) {
labels[i] = j;
dist = new_dist;
}
}
cost += dist;
}
}
/**
* The method responsible with actually doing the recursive hierarchical
* clustering
*
* Params:
* node = the node to cluster
* indices = indices of the points belonging to the current node
* branching = the branching factor to use in the clustering
*
*/
void computeClustering(NodePtr node, int* indices, int indices_length)
{
if (indices_length < leaf_max_size_) { // leaf node
node->points.resize(indices_length);
for (int i=0;i<indices_length;++i) {
node->points[i].index = indices[i];
node->points[i].point = points_[indices[i]];
}
node->childs.clear();
return;
}
std::vector<int> centers(branching_);
std::vector<int> labels(indices_length);
int centers_length;
(*chooseCenters_)(branching_, indices, indices_length, &centers[0], centers_length);
if (centers_length<branching_) {
node->points.resize(indices_length);
for (int i=0;i<indices_length;++i) {
node->points[i].index = indices[i];
node->points[i].point = points_[indices[i]];
}
node->childs.clear();
return;
}
// assign points to clusters
DistanceType cost;
computeLabels(indices, indices_length, &centers[0], centers_length, &labels[0], cost);
node->childs.resize(branching_);
int start = 0;
int end = start;
for (int i=0; i<branching_; ++i) {
for (int j=0; j<indices_length; ++j) {
if (labels[j]==i) {
std::swap(indices[j],indices[end]);
std::swap(labels[j],labels[end]);
end++;
}
}
node->childs[i] = new(pool_) Node();
node->childs[i]->pivot_index = centers[i];
node->childs[i]->pivot = points_[centers[i]];
node->childs[i]->points.clear();
computeClustering(node->childs[i],indices+start, end-start);
start=end;
}
}
template<bool with_removed>
void findNeighborsWithRemoved(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
{
int maxChecks = searchParams.checks;
// Priority queue storing intermediate branches in the best-bin-first search
Heap<BranchSt>* heap = new Heap<BranchSt>(size_);
DynamicBitset checked(size_);
int checks = 0;
for (int i=0; i<trees_; ++i) {
findNN<with_removed>(tree_roots_[i], result, vec, checks, maxChecks, heap, checked);
}
BranchSt branch;
while (heap->popMin(branch) && (checks<maxChecks || !result.full())) {
NodePtr node = branch.node;
findNN<with_removed>(node, result, vec, checks, maxChecks, heap, checked);
}
delete heap;
}
/**
* Performs one descent in the hierarchical k-means tree. The branches not
* visited are stored in a priority queue.
*
* Params:
* node = node to explore
* result = container for the k-nearest neighbors found
* vec = query points
* checks = how many points in the dataset have been checked so far
* maxChecks = maximum dataset points to checks
*/
template<bool with_removed>
void findNN(NodePtr node, ResultSet<DistanceType>& result, const ElementType* vec, int& checks, int maxChecks,
Heap<BranchSt>* heap, DynamicBitset& checked) const
{
if (node->childs.empty()) {
if (checks>=maxChecks) {
if (result.full()) return;
}
for (size_t i=0; i<node->points.size(); ++i) {
PointInfo& pointInfo = node->points[i];
if (with_removed) {
if (removed_points_.test(pointInfo.index)) continue;
}
if (checked.test(pointInfo.index)) continue;
DistanceType dist = distance_(pointInfo.point, vec, veclen_);
result.addPoint(dist, pointInfo.index);
checked.set(pointInfo.index);
++checks;
}
}
else {
DistanceType* domain_distances = new DistanceType[branching_];
int best_index = 0;
domain_distances[best_index] = distance_(vec, node->childs[best_index]->pivot, veclen_);
for (int i=1; i<branching_; ++i) {
domain_distances[i] = distance_(vec, node->childs[i]->pivot, veclen_);
if (domain_distances[i]<domain_distances[best_index]) {
best_index = i;
}
}
for (int i=0; i<branching_; ++i) {
if (i!=best_index) {
heap->insert(BranchSt(node->childs[i],domain_distances[i]));
}
}
delete[] domain_distances;
findNN<with_removed>(node->childs[best_index],result,vec, checks, maxChecks, heap, checked);
}
}
void addPointToTree(NodePtr node, size_t index)
{
ElementType* point = points_[index];
if (node->childs.empty()) { // leaf node
PointInfo pointInfo;
pointInfo.point = point;
pointInfo.index = index;
node->points.push_back(pointInfo);
if (node->points.size()>=size_t(branching_)) {
std::vector<int> indices(node->points.size());
for (size_t i=0;i<node->points.size();++i) {
indices[i] = node->points[i].index;
}
computeClustering(node, &indices[0], indices.size());
}
}
else {
// find the closest child
int closest = 0;
ElementType* center = node->childs[closest]->pivot;
DistanceType dist = distance_(center, point, veclen_);
for (size_t i=1;i<size_t(branching_);++i) {
center = node->childs[i]->pivot;
DistanceType crt_dist = distance_(center, point, veclen_);
if (crt_dist<dist) {
dist = crt_dist;
closest = i;
}
}
addPointToTree(node->childs[closest], index);
}
}
void swap(HierarchicalClusteringIndex& other)
{
BaseClass::swap(other);
std::swap(tree_roots_, other.tree_roots_);
std::swap(pool_, other.pool_);
std::swap(memoryCounter_, other.memoryCounter_);
std::swap(branching_, other.branching_);
std::swap(trees_, other.trees_);
std::swap(centers_init_, other.centers_init_);
std::swap(leaf_max_size_, other.leaf_max_size_);
std::swap(chooseCenters_, other.chooseCenters_);
}
private:
/**
* The root nodes in the tree.
*/
std::vector<Node*> tree_roots_;
/**
* Pooled memory allocator.
*
* Using a pooled memory allocator is more efficient
* than allocating memory directly when there is a large
* number small of memory allocations.
*/
PooledAllocator pool_;
/**
* Memory occupied by the index.
*/
int memoryCounter_;
/** index parameters */
/**
* Branching factor to use for clustering
*/
int branching_;
/**
* How many parallel trees to build
*/
int trees_;
/**
* Algorithm to use for choosing cluster centers
*/
flann_centers_init_t centers_init_;
/**
* Max size of leaf nodes
*/
int leaf_max_size_;
/**
* Algorithm used to choose initial centers
*/
CenterChooser<Distance>* chooseCenters_;
USING_BASECLASS_SYMBOLS
};
}
#endif /* FLANN_HIERARCHICAL_CLUSTERING_INDEX_H_ */

765
corelib/src/rtflann/algorithms/kdtree_index.h Normal file
View File

@@ -0,0 +1,765 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_KDTREE_INDEX_H_
#define RTABMAP_FLANN_KDTREE_INDEX_H_
#include <algorithm>
#include <map>
#include <cassert>
#include <cstring>
#include <stdarg.h>
#include <cmath>
#include "rtflann/general.h"
#include "rtflann/algorithms/nn_index.h"
#include "rtflann/util/dynamic_bitset.h"
#include "rtflann/util/matrix.h"
#include "rtflann/util/result_set.h"
#include "rtflann/util/heap.h"
#include "rtflann/util/allocator.h"
#include "rtflann/util/random.h"
#include "rtflann/util/saving.h"
namespace rtflann
{
struct KDTreeIndexParams : public IndexParams
{
KDTreeIndexParams(int trees = 4)
{
(*this)["algorithm"] = FLANN_INDEX_KDTREE;
(*this)["trees"] = trees;
}
};
/**
* Randomized kd-tree index
*
* Contains the k-d trees and other information for indexing a set of points
* for nearest-neighbor matching.
*/
template <typename Distance>
class KDTreeIndex : public NNIndex<Distance>
{
public:
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
typedef NNIndex<Distance> BaseClass;
typedef bool needs_kdtree_distance;
/**
* KDTree constructor
*
* Params:
* inputData = dataset with the input features
* params = parameters passed to the kdtree algorithm
*/
KDTreeIndex(const IndexParams& params = KDTreeIndexParams(), Distance d = Distance() ) :
BaseClass(params, d), mean_(NULL), var_(NULL)
{
trees_ = get_param(index_params_,"trees",4);
}
/**
* KDTree constructor
*
* Params:
* inputData = dataset with the input features
* params = parameters passed to the kdtree algorithm
*/
KDTreeIndex(const Matrix<ElementType>& dataset, const IndexParams& params = KDTreeIndexParams(),
Distance d = Distance() ) : BaseClass(params,d ), mean_(NULL), var_(NULL)
{
trees_ = get_param(index_params_,"trees",4);
setDataset(dataset);
}
KDTreeIndex(const KDTreeIndex& other) : BaseClass(other),
trees_(other.trees_)
{
tree_roots_.resize(other.tree_roots_.size());
for (size_t i=0;i<tree_roots_.size();++i) {
copyTree(tree_roots_[i], other.tree_roots_[i]);
}
}
KDTreeIndex& operator=(KDTreeIndex other)
{
this->swap(other);
return *this;
}
/**
* Standard destructor
*/
virtual ~KDTreeIndex()
{
freeIndex();
}
BaseClass* clone() const
{
return new KDTreeIndex(*this);
}
using BaseClass::buildIndex;
void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{
assert(points.cols==veclen_);
size_t old_size = size_;
extendDataset(points);
if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
buildIndex();
}
else {
for (size_t i=old_size;i<size_;++i) {
for (int j = 0; j < trees_; j++) {
addPointToTree(tree_roots_[j], i);
}
}
}
}
flann_algorithm_t getType() const
{
return FLANN_INDEX_KDTREE;
}
template<typename Archive>
void serialize(Archive& ar)
{
ar.setObject(this);
ar & *static_cast<NNIndex<Distance>*>(this);
ar & trees_;
if (Archive::is_loading::value) {
tree_roots_.resize(trees_);
}
for (size_t i=0;i<tree_roots_.size();++i) {
if (Archive::is_loading::value) {
tree_roots_[i] = new(pool_) Node();
}
ar & *tree_roots_[i];
}
if (Archive::is_loading::value) {
index_params_["algorithm"] = getType();
index_params_["trees"] = trees_;
}
}
void saveIndex(FILE* stream)
{
serialization::SaveArchive sa(stream);
sa & *this;
}
void loadIndex(FILE* stream)
{
freeIndex();
serialization::LoadArchive la(stream);
la & *this;
}
/**
* Computes the inde memory usage
* Returns: memory used by the index
*/
int usedMemory() const
{
return int(pool_.usedMemory+pool_.wastedMemory+size_*sizeof(int)); // pool memory and vind array memory
}
/**
* Find set of nearest neighbors to vec. Their indices are stored inside
* the result object.
*
* Params:
* result = the result object in which the indices of the nearest-neighbors are stored
* vec = the vector for which to search the nearest neighbors
* maxCheck = the maximum number of restarts (in a best-bin-first manner)
*/
void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
{
int maxChecks = searchParams.checks;
float epsError = 1+searchParams.eps;
if (maxChecks==FLANN_CHECKS_UNLIMITED) {
if (removed_) {
getExactNeighbors<true>(result, vec, epsError);
}
else {
getExactNeighbors<false>(result, vec, epsError);
}
}
else {
if (removed_) {
getNeighbors<true>(result, vec, maxChecks, epsError);
}
else {
getNeighbors<false>(result, vec, maxChecks, epsError);
}
}
}
protected:
/**
* Builds the index
*/
void buildIndexImpl()
{
// Create a permutable array of indices to the input vectors.
std::vector<int> ind(size_);
for (size_t i = 0; i < size_; ++i) {
ind[i] = int(i);
}
mean_ = new DistanceType[veclen_];
var_ = new DistanceType[veclen_];
tree_roots_.resize(trees_);
/* Construct the randomized trees. */
for (int i = 0; i < trees_; i++) {
/* Randomize the order of vectors to allow for unbiased sampling. */
std::random_shuffle(ind.begin(), ind.end());
tree_roots_[i] = divideTree(&ind[0], int(size_) );
}
delete[] mean_;
delete[] var_;
}
void freeIndex()
{
for (size_t i=0;i<tree_roots_.size();++i) {
// using placement new, so call destructor explicitly
if (tree_roots_[i]!=NULL) tree_roots_[i]->~Node();
}
pool_.free();
}
private:
/*--------------------- Internal Data Structures --------------------------*/
struct Node
{
/**
* Dimension used for subdivision.
*/
int divfeat;
/**
* The values used for subdivision.
*/
DistanceType divval;
/**
* Point data
*/
ElementType* point;
/**
* The child nodes.
*/
Node* child1, *child2;
Node(){
child1 = NULL;
child2 = NULL;
}
~Node() {
if (child1 != NULL) { child1->~Node(); child1 = NULL; }
if (child2 != NULL) { child2->~Node(); child2 = NULL; }
}
private:
template<typename Archive>
void serialize(Archive& ar)
{
typedef KDTreeIndex<Distance> Index;
Index* obj = static_cast<Index*>(ar.getObject());
ar & divfeat;
ar & divval;
bool leaf_node = false;
if (Archive::is_saving::value) {
leaf_node = ((child1==NULL) && (child2==NULL));
}
ar & leaf_node;
if (leaf_node) {
if (Archive::is_loading::value) {
point = obj->points_[divfeat];
}
}
if (!leaf_node) {
if (Archive::is_loading::value) {
child1 = new(obj->pool_) Node();
child2 = new(obj->pool_) Node();
}
ar & *child1;
ar & *child2;
}
}
friend struct serialization::access;
};
typedef Node* NodePtr;
typedef BranchStruct<NodePtr, DistanceType> BranchSt;
typedef BranchSt* Branch;
void copyTree(NodePtr& dst, const NodePtr& src)
{
dst = new(pool_) Node();
dst->divfeat = src->divfeat;
dst->divval = src->divval;
if (src->child1==NULL && src->child2==NULL) {
dst->point = points_[dst->divfeat];
dst->child1 = NULL;
dst->child2 = NULL;
}
else {
copyTree(dst->child1, src->child1);
copyTree(dst->child2, src->child2);
}
}
/**
* Create a tree node that subdivides the list of vecs from vind[first]
* to vind[last]. The routine is called recursively on each sublist.
* Place a pointer to this new tree node in the location pTree.
*
* Params: pTree = the new node to create
* first = index of the first vector
* last = index of the last vector
*/
NodePtr divideTree(int* ind, int count)
{
NodePtr node = new(pool_) Node(); // allocate memory
/* If too few exemplars remain, then make this a leaf node. */
if (count == 1) {
node->child1 = node->child2 = NULL; /* Mark as leaf node. */
node->divfeat = *ind; /* Store index of this vec. */
node->point = points_[*ind];
}
else {
int idx;
int cutfeat;
DistanceType cutval;
meanSplit(ind, count, idx, cutfeat, cutval);
node->divfeat = cutfeat;
node->divval = cutval;
node->child1 = divideTree(ind, idx);
node->child2 = divideTree(ind+idx, count-idx);
}
return node;
}
/**
* Choose which feature to use in order to subdivide this set of vectors.
* Make a random choice among those with the highest variance, and use
* its variance as the threshold value.
*/
void meanSplit(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval)
{
memset(mean_,0,veclen_*sizeof(DistanceType));
memset(var_,0,veclen_*sizeof(DistanceType));
/* Compute mean values. Only the first SAMPLE_MEAN values need to be
sampled to get a good estimate.
*/
int cnt = std::min((int)SAMPLE_MEAN+1, count);
for (int j = 0; j < cnt; ++j) {
ElementType* v = points_[ind[j]];
for (size_t k=0; k<veclen_; ++k) {
mean_[k] += v[k];
}
}
DistanceType div_factor = DistanceType(1)/cnt;
for (size_t k=0; k<veclen_; ++k) {
mean_[k] *= div_factor;
}
/* Compute variances (no need to divide by count). */
for (int j = 0; j < cnt; ++j) {
ElementType* v = points_[ind[j]];
for (size_t k=0; k<veclen_; ++k) {
DistanceType dist = v[k] - mean_[k];
var_[k] += dist * dist;
}
}
/* Select one of the highest variance indices at random. */
cutfeat = selectDivision(var_);
cutval = mean_[cutfeat];
int lim1, lim2;
planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
if (lim1>count/2) index = lim1;
else if (lim2<count/2) index = lim2;
else index = count/2;
/* If either list is empty, it means that all remaining features
* are identical. Split in the middle to maintain a balanced tree.
*/
if ((lim1==count)||(lim2==0)) index = count/2;
}
/**
* Select the top RAND_DIM largest values from v and return the index of
* one of these selected at random.
*/
int selectDivision(DistanceType* v)
{
int num = 0;
size_t topind[RAND_DIM];
/* Create a list of the indices of the top RAND_DIM values. */
for (size_t i = 0; i < veclen_; ++i) {
if ((num < RAND_DIM)||(v[i] > v[topind[num-1]])) {
/* Put this element at end of topind. */
if (num < RAND_DIM) {
topind[num++] = i; /* Add to list. */
}
else {
topind[num-1] = i; /* Replace last element. */
}
/* Bubble end value down to right location by repeated swapping. */
int j = num - 1;
while (j > 0 && v[topind[j]] > v[topind[j-1]]) {
std::swap(topind[j], topind[j-1]);
--j;
}
}
}
/* Select a random integer in range [0,num-1], and return that index. */
int rnd = rand_int(num);
return (int)topind[rnd];
}
/**
* Subdivide the list of points by a plane perpendicular on axe corresponding
* to the 'cutfeat' dimension at 'cutval' position.
*
* On return:
* dataset[ind[0..lim1-1]][cutfeat]<cutval
* dataset[ind[lim1..lim2-1]][cutfeat]==cutval
* dataset[ind[lim2..count]][cutfeat]>cutval
*/
void planeSplit(int* ind, int count, int cutfeat, DistanceType cutval, int& lim1, int& lim2)
{
/* Move vector indices for left subtree to front of list. */
int left = 0;
int right = count-1;
for (;; ) {
while (left<=right && points_[ind[left]][cutfeat]<cutval) ++left;
while (left<=right && points_[ind[right]][cutfeat]>=cutval) --right;
if (left>right) break;
std::swap(ind[left], ind[right]); ++left; --right;
}
lim1 = left;
right = count-1;
for (;; ) {
while (left<=right && points_[ind[left]][cutfeat]<=cutval) ++left;
while (left<=right && points_[ind[right]][cutfeat]>cutval) --right;
if (left>right) break;
std::swap(ind[left], ind[right]); ++left; --right;
}
lim2 = left;
}
/**
* Performs an exact nearest neighbor search. The exact search performs a full
* traversal of the tree.
*/
template<bool with_removed>
void getExactNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, float epsError) const
{
// checkID -= 1; /* Set a different unique ID for each search. */
if (trees_ > 1) {
fprintf(stderr,"It doesn't make any sense to use more than one tree for exact search");
}
if (trees_>0) {
searchLevelExact<with_removed>(result, vec, tree_roots_[0], 0.0, epsError);
}
}
/**
* Performs the approximate nearest-neighbor search. The search is approximate
* because the tree traversal is abandoned after a given number of descends in
* the tree.
*/
template<bool with_removed>
void getNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, int maxCheck, float epsError) const
{
int i;
BranchSt branch;
int checkCount = 0;
Heap<BranchSt>* heap = new Heap<BranchSt>((int)size_);
DynamicBitset checked(size_);
/* Search once through each tree down to root. */
for (i = 0; i < trees_; ++i) {
searchLevel<with_removed>(result, vec, tree_roots_[i], 0, checkCount, maxCheck, epsError, heap, checked);
}
/* Keep searching other branches from heap until finished. */
while ( heap->popMin(branch) && (checkCount < maxCheck || !result.full() )) {
searchLevel<with_removed>(result, vec, branch.node, branch.mindist, checkCount, maxCheck, epsError, heap, checked);
}
delete heap;
}
/**
* Search starting from a given node of the tree. Based on any mismatches at
* higher levels, all exemplars below this level must have a distance of
* at least "mindistsq".
*/
template<bool with_removed>
void searchLevel(ResultSet<DistanceType>& result_set, const ElementType* vec, NodePtr node, DistanceType mindist, int& checkCount, int maxCheck,
float epsError, Heap<BranchSt>* heap, DynamicBitset& checked) const
{
if (result_set.worstDist()<mindist) {
// printf("Ignoring branch, too far\n");
return;
}
/* If this is a leaf node, then do check and return. */
if ((node->child1 == NULL)&&(node->child2 == NULL)) {
int index = node->divfeat;
if (with_removed) {
if (removed_points_.test(index)) return;
}
/* Do not check same node more than once when searching multiple trees. */
if ( checked.test(index) || ((checkCount>=maxCheck)&& result_set.full()) ) return;
checked.set(index);
checkCount++;
DistanceType dist = distance_(node->point, vec, veclen_);
result_set.addPoint(dist,index);
return;
}
/* Which child branch should be taken first? */
ElementType val = vec[node->divfeat];
DistanceType diff = val - node->divval;
NodePtr bestChild = (diff < 0) ? node->child1 : node->child2;
NodePtr otherChild = (diff < 0) ? node->child2 : node->child1;
/* Create a branch record for the branch not taken. Add distance
of this feature boundary (we don't attempt to correct for any
use of this feature in a parent node, which is unlikely to
happen and would have only a small effect). Don't bother
adding more branches to heap after halfway point, as cost of
adding exceeds their value.
*/
DistanceType new_distsq = mindist + distance_.accum_dist(val, node->divval, node->divfeat);
// if (2 * checkCount < maxCheck || !result.full()) {
if ((new_distsq*epsError < result_set.worstDist())|| !result_set.full()) {
heap->insert( BranchSt(otherChild, new_distsq) );
}
/* Call recursively to search next level down. */
searchLevel<with_removed>(result_set, vec, bestChild, mindist, checkCount, maxCheck, epsError, heap, checked);
}
/**
* Performs an exact search in the tree starting from a node.
*/
template<bool with_removed>
void searchLevelExact(ResultSet<DistanceType>& result_set, const ElementType* vec, const NodePtr node, DistanceType mindist, const float epsError) const
{
/* If this is a leaf node, then do check and return. */
if ((node->child1 == NULL)&&(node->child2 == NULL)) {
int index = node->divfeat;
if (with_removed) {
if (removed_points_.test(index)) return; // ignore removed points
}
DistanceType dist = distance_(node->point, vec, veclen_);
result_set.addPoint(dist,index);
return;
}
/* Which child branch should be taken first? */
ElementType val = vec[node->divfeat];
DistanceType diff = val - node->divval;
NodePtr bestChild = (diff < 0) ? node->child1 : node->child2;
NodePtr otherChild = (diff < 0) ? node->child2 : node->child1;
/* Create a branch record for the branch not taken. Add distance
of this feature boundary (we don't attempt to correct for any
use of this feature in a parent node, which is unlikely to
happen and would have only a small effect). Don't bother
adding more branches to heap after halfway point, as cost of
adding exceeds their value.
*/
DistanceType new_distsq = mindist + distance_.accum_dist(val, node->divval, node->divfeat);
/* Call recursively to search next level down. */
searchLevelExact<with_removed>(result_set, vec, bestChild, mindist, epsError);
if (mindist*epsError<=result_set.worstDist()) {
searchLevelExact<with_removed>(result_set, vec, otherChild, new_distsq, epsError);
}
}
void addPointToTree(NodePtr node, int ind)
{
ElementType* point = points_[ind];
if ((node->child1==NULL) && (node->child2==NULL)) {
ElementType* leaf_point = node->point;
ElementType max_span = 0;
size_t div_feat = 0;
for (size_t i=0;i<veclen_;++i) {
ElementType span = std::abs(point[i]-leaf_point[i]);
if (span > max_span) {
max_span = span;
div_feat = i;
}
}
NodePtr left = new(pool_) Node();
left->child1 = left->child2 = NULL;
NodePtr right = new(pool_) Node();
right->child1 = right->child2 = NULL;
if (point[div_feat]<leaf_point[div_feat]) {
left->divfeat = ind;
left->point = point;
right->divfeat = node->divfeat;
right->point = node->point;
}
else {
left->divfeat = node->divfeat;
left->point = node->point;
right->divfeat = ind;
right->point = point;
}
node->divfeat = div_feat;
node->divval = (point[div_feat]+leaf_point[div_feat])/2;
node->child1 = left;
node->child2 = right;
}
else {
if (point[node->divfeat]<node->divval) {
addPointToTree(node->child1,ind);
}
else {
addPointToTree(node->child2,ind);
}
}
}
private:
void swap(KDTreeIndex& other)
{
BaseClass::swap(other);
std::swap(trees_, other.trees_);
std::swap(tree_roots_, other.tree_roots_);
std::swap(pool_, other.pool_);
}
private:
enum
{
/**
* To improve efficiency, only SAMPLE_MEAN random values are used to
* compute the mean and variance at each level when building a tree.
* A value of 100 seems to perform as well as using all values.
*/
SAMPLE_MEAN = 100,
/**
* Top random dimensions to consider
*
* When creating random trees, the dimension on which to subdivide is
* selected at random from among the top RAND_DIM dimensions with the
* highest variance. A value of 5 works well.
*/
RAND_DIM=5
};
/**
* Number of randomized trees that are used
*/
int trees_;
DistanceType* mean_;
DistanceType* var_;
/**
* Array of k-d trees used to find neighbours.
*/
std::vector<NodePtr> tree_roots_;
/**
* Pooled memory allocator.
*
* Using a pooled memory allocator is more efficient
* than allocating memory directly when there is a large
* number small of memory allocations.
*/
PooledAllocator pool_;
USING_BASECLASS_SYMBOLS
}; // class KDTreeIndex
}
#endif //FLANN_KDTREE_INDEX_H_

698
corelib/src/rtflann/algorithms/kdtree_single_index.h Normal file
View File

@@ -0,0 +1,698 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_KDTREE_SINGLE_INDEX_H_
#define RTABMAP_FLANN_KDTREE_SINGLE_INDEX_H_
#include <algorithm>
#include <map>
#include <cassert>
#include <cstring>
#include "rtflann/general.h"
#include "rtflann/algorithms/nn_index.h"
#include "rtflann/util/matrix.h"
#include "rtflann/util/result_set.h"
#include "rtflann/util/heap.h"
#include "rtflann/util/allocator.h"
#include "rtflann/util/random.h"
#include "rtflann/util/saving.h"
namespace rtflann
{
struct KDTreeSingleIndexParams : public IndexParams
{
KDTreeSingleIndexParams(int leaf_max_size = 10, bool reorder = true)
{
(*this)["algorithm"] = FLANN_INDEX_KDTREE_SINGLE;
(*this)["leaf_max_size"] = leaf_max_size;
(*this)["reorder"] = reorder;
}
};
/**
* Single kd-tree index
*
* Contains the k-d trees and other information for indexing a set of points
* for nearest-neighbor matching.
*/
template <typename Distance>
class KDTreeSingleIndex : public NNIndex<Distance>
{
public:
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
typedef NNIndex<Distance> BaseClass;
typedef bool needs_kdtree_distance;
/**
* KDTree constructor
*
* Params:
* params = parameters passed to the kdtree algorithm
*/
KDTreeSingleIndex(const IndexParams& params = KDTreeSingleIndexParams(), Distance d = Distance() ) :
BaseClass(params, d), root_node_(NULL)
{
leaf_max_size_ = get_param(params,"leaf_max_size",10);
reorder_ = get_param(params, "reorder", true);
}
/**
* KDTree constructor
*
* Params:
* inputData = dataset with the input features
* params = parameters passed to the kdtree algorithm
*/
KDTreeSingleIndex(const Matrix<ElementType>& inputData, const IndexParams& params = KDTreeSingleIndexParams(),
Distance d = Distance() ) : BaseClass(params, d), root_node_(NULL)
{
leaf_max_size_ = get_param(params,"leaf_max_size",10);
reorder_ = get_param(params, "reorder", true);
setDataset(inputData);
}
KDTreeSingleIndex(const KDTreeSingleIndex& other) : BaseClass(other),
leaf_max_size_(other.leaf_max_size_),
reorder_(other.reorder_),
vind_(other.vind_),
root_bbox_(other.root_bbox_)
{
if (reorder_) {
data_ = rtflann::Matrix<ElementType>(new ElementType[size_*veclen_], size_, veclen_);
std::copy(other.data_[0], other.data_[0]+size_*veclen_, data_[0]);
}
copyTree(root_node_, other.root_node_);
}
KDTreeSingleIndex& operator=(KDTreeSingleIndex other)
{
this->swap(other);
return *this;
}
/**
* Standard destructor
*/
virtual ~KDTreeSingleIndex()
{
freeIndex();
}
BaseClass* clone() const
{
return new KDTreeSingleIndex(*this);
}
using BaseClass::buildIndex;
void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{
assert(points.cols==veclen_);
extendDataset(points);
buildIndex();
}
flann_algorithm_t getType() const
{
return FLANN_INDEX_KDTREE_SINGLE;
}
template<typename Archive>
void serialize(Archive& ar)
{
ar.setObject(this);
if (reorder_) index_params_["save_dataset"] = false;
ar & *static_cast<NNIndex<Distance>*>(this);
ar & reorder_;
ar & leaf_max_size_;
ar & root_bbox_;
ar & vind_;
if (reorder_) {
ar & data_;
}
if (Archive::is_loading::value) {
root_node_ = new(pool_) Node();
}
ar & *root_node_;
if (Archive::is_loading::value) {
index_params_["algorithm"] = getType();
index_params_["leaf_max_size"] = leaf_max_size_;
index_params_["reorder"] = reorder_;
}
}
void saveIndex(FILE* stream)
{
serialization::SaveArchive sa(stream);
sa & *this;
}
void loadIndex(FILE* stream)
{
freeIndex();
serialization::LoadArchive la(stream);
la & *this;
}
/**
* Computes the inde memory usage
* Returns: memory used by the index
*/
int usedMemory() const
{
return pool_.usedMemory+pool_.wastedMemory+size_*sizeof(int); // pool memory and vind array memory
}
/**
* Find set of nearest neighbors to vec. Their indices are stored inside
* the result object.
*
* Params:
* result = the result object in which the indices of the nearest-neighbors are stored
* vec = the vector for which to search the nearest neighbors
* maxCheck = the maximum number of restarts (in a best-bin-first manner)
*/
void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const
{
float epsError = 1+searchParams.eps;
std::vector<DistanceType> dists(veclen_,0);
DistanceType distsq = computeInitialDistances(vec, dists);
if (removed_) {
searchLevel<true>(result, vec, root_node_, distsq, dists, epsError);
}
else {
searchLevel<false>(result, vec, root_node_, distsq, dists, epsError);
}
}
protected:
/**
* Builds the index
*/
void buildIndexImpl()
{
// Create a permutable array of indices to the input vectors.
vind_.resize(size_);
for (size_t i = 0; i < size_; i++) {
vind_[i] = i;
}
computeBoundingBox(root_bbox_);
root_node_ = divideTree(0, size_, root_bbox_ ); // construct the tree
if (reorder_) {
data_ = rtflann::Matrix<ElementType>(new ElementType[size_*veclen_], size_, veclen_);
for (size_t i=0; i<size_; ++i) {
std::copy(points_[vind_[i]], points_[vind_[i]]+veclen_, data_[i]);
}
}
}
private:
/*--------------------- Internal Data Structures --------------------------*/
struct Node
{
/**
* Indices of points in leaf node
*/
int left, right;
/**
* Dimension used for subdivision.
*/
int divfeat;
/**
* The values used for subdivision.
*/
DistanceType divlow, divhigh;
/**
* The child nodes.
*/
Node* child1, * child2;
~Node()
{
if (child1) child1->~Node();
if (child2) child2->~Node();
}
private:
template<typename Archive>
void serialize(Archive& ar)
{
typedef KDTreeSingleIndex<Distance> Index;
Index* obj = static_cast<Index*>(ar.getObject());
ar & left;
ar & right;
ar & divfeat;
ar & divlow;
ar & divhigh;
bool leaf_node = false;
if (Archive::is_saving::value) {
leaf_node = ((child1==NULL) && (child2==NULL));
}
ar & leaf_node;
if (!leaf_node) {
if (Archive::is_loading::value) {
child1 = new(obj->pool_) Node();
child2 = new(obj->pool_) Node();
}
ar & *child1;
ar & *child2;
}
}
friend struct serialization::access;
};
typedef Node* NodePtr;
struct Interval
{
DistanceType low, high;
private:
template <typename Archive>
void serialize(Archive& ar)
{
ar & low;
ar & high;
}
friend struct serialization::access;
};
typedef std::vector<Interval> BoundingBox;
typedef BranchStruct<NodePtr, DistanceType> BranchSt;
typedef BranchSt* Branch;
void freeIndex()
{
if (data_.ptr()) {
delete[] data_.ptr();
data_ = rtflann::Matrix<ElementType>();
}
if (root_node_) root_node_->~Node();
pool_.free();
}
void copyTree(NodePtr& dst, const NodePtr& src)
{
dst = new(pool_) Node();
*dst = *src;
if (src->child1!=NULL && src->child2!=NULL) {
copyTree(dst->child1, src->child1);
copyTree(dst->child2, src->child2);
}
}
void computeBoundingBox(BoundingBox& bbox)
{
bbox.resize(veclen_);
for (size_t i=0; i<veclen_; ++i) {
bbox[i].low = (DistanceType)points_[0][i];
bbox[i].high = (DistanceType)points_[0][i];
}
for (size_t k=1; k<size_; ++k) {
for (size_t i=0; i<veclen_; ++i) {
if (points_[k][i]<bbox[i].low) bbox[i].low = (DistanceType)points_[k][i];
if (points_[k][i]>bbox[i].high) bbox[i].high = (DistanceType)points_[k][i];
}
}
}
/**
* Create a tree node that subdivides the list of vecs from vind[first]
* to vind[last]. The routine is called recursively on each sublist.
* Place a pointer to this new tree node in the location pTree.
*
* Params: pTree = the new node to create
* first = index of the first vector
* last = index of the last vector
*/
NodePtr divideTree(int left, int right, BoundingBox& bbox)
{
NodePtr node = new (pool_) Node(); // allocate memory
/* If too few exemplars remain, then make this a leaf node. */
if ( (right-left) <= leaf_max_size_) {
node->child1 = node->child2 = NULL; /* Mark as leaf node. */
node->left = left;
node->right = right;
// compute bounding-box of leaf points
for (size_t i=0; i<veclen_; ++i) {
bbox[i].low = (DistanceType)points_[vind_[left]][i];
bbox[i].high = (DistanceType)points_[vind_[left]][i];
}
for (int k=left+1; k<right; ++k) {
for (size_t i=0; i<veclen_; ++i) {
if (bbox[i].low>points_[vind_[k]][i]) bbox[i].low=(DistanceType)points_[vind_[k]][i];
if (bbox[i].high<points_[vind_[k]][i]) bbox[i].high=(DistanceType)points_[vind_[k]][i];
}
}
}
else {
int idx;
int cutfeat;
DistanceType cutval;
middleSplit(&vind_[0]+left, right-left, idx, cutfeat, cutval, bbox);
node->divfeat = cutfeat;
BoundingBox left_bbox(bbox);
left_bbox[cutfeat].high = cutval;
node->child1 = divideTree(left, left+idx, left_bbox);
BoundingBox right_bbox(bbox);
right_bbox[cutfeat].low = cutval;
node->child2 = divideTree(left+idx, right, right_bbox);
node->divlow = left_bbox[cutfeat].high;
node->divhigh = right_bbox[cutfeat].low;
for (size_t i=0; i<veclen_; ++i) {
bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
}
}
return node;
}
void computeMinMax(int* ind, int count, int dim, ElementType& min_elem, ElementType& max_elem)
{
min_elem = points_[ind[0]][dim];
max_elem = points_[ind[0]][dim];
for (int i=1; i<count; ++i) {
ElementType val = points_[ind[i]][dim];
if (val<min_elem) min_elem = val;
if (val>max_elem) max_elem = val;
}
}
void middleSplit(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
{
// find the largest span from the approximate bounding box
ElementType max_span = bbox[0].high-bbox[0].low;
cutfeat = 0;
cutval = (bbox[0].high+bbox[0].low)/2;
for (size_t i=1; i<veclen_; ++i) {
ElementType span = bbox[i].high-bbox[i].low;
if (span>max_span) {
max_span = span;
cutfeat = i;
cutval = (bbox[i].high+bbox[i].low)/2;
}
}
// compute exact span on the found dimension
ElementType min_elem, max_elem;
computeMinMax(ind, count, cutfeat, min_elem, max_elem);
cutval = (min_elem+max_elem)/2;
max_span = max_elem - min_elem;
// check if a dimension of a largest span exists
size_t k = cutfeat;
for (size_t i=0; i<veclen_; ++i) {
if (i==k) continue;
ElementType span = bbox[i].high-bbox[i].low;
if (span>max_span) {
computeMinMax(ind, count, i, min_elem, max_elem);
span = max_elem - min_elem;
if (span>max_span) {
max_span = span;
cutfeat = i;
cutval = (min_elem+max_elem)/2;
}
}
}
int lim1, lim2;
planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
if (lim1>count/2) index = lim1;
else if (lim2<count/2) index = lim2;
else index = count/2;
assert(index > 0 && index < count);
}
void middleSplit_(int* ind, int count, int& index, int& cutfeat, DistanceType& cutval, const BoundingBox& bbox)
{
const float eps_val=0.00001f;
DistanceType max_span = bbox[0].high-bbox[0].low;
for (size_t i=1; i<veclen_; ++i) {
DistanceType span = bbox[i].high-bbox[i].low;
if (span>max_span) {
max_span = span;
}
}
DistanceType max_spread = -1;
cutfeat = 0;
for (size_t i=0; i<veclen_; ++i) {
DistanceType span = bbox[i].high-bbox[i].low;
if (span>(DistanceType)((1-eps_val)*max_span)) {
ElementType min_elem, max_elem;
computeMinMax(ind, count, cutfeat, min_elem, max_elem);
DistanceType spread = (DistanceType)(max_elem-min_elem);
if (spread>max_spread) {
cutfeat = i;
max_spread = spread;
}
}
}
// split in the middle
DistanceType split_val = (bbox[cutfeat].low+bbox[cutfeat].high)/2;
ElementType min_elem, max_elem;
computeMinMax(ind, count, cutfeat, min_elem, max_elem);
if (split_val<min_elem) cutval = (DistanceType)min_elem;
else if (split_val>max_elem) cutval = (DistanceType)max_elem;
else cutval = split_val;
int lim1, lim2;
planeSplit(ind, count, cutfeat, cutval, lim1, lim2);
if (lim1>count/2) index = lim1;
else if (lim2<count/2) index = lim2;
else index = count/2;
assert(index > 0 && index < count);
}
/**
* Subdivide the list of points by a plane perpendicular on axe corresponding
* to the 'cutfeat' dimension at 'cutval' position.
*
* On return:
* dataset[ind[0..lim1-1]][cutfeat]<cutval
* dataset[ind[lim1..lim2-1]][cutfeat]==cutval
* dataset[ind[lim2..count]][cutfeat]>cutval
*/
void planeSplit(int* ind, int count, int cutfeat, DistanceType cutval, int& lim1, int& lim2)
{
int left = 0;
int right = count-1;
for (;; ) {
while (left<=right && points_[ind[left]][cutfeat]<cutval) ++left;
while (left<=right && points_[ind[right]][cutfeat]>=cutval) --right;
if (left>right) break;
std::swap(ind[left], ind[right]); ++left; --right;
}
lim1 = left;
right = count-1;
for (;; ) {
while (left<=right && points_[ind[left]][cutfeat]<=cutval) ++left;
while (left<=right && points_[ind[right]][cutfeat]>cutval) --right;
if (left>right) break;
std::swap(ind[left], ind[right]); ++left; --right;
}
lim2 = left;
}
DistanceType computeInitialDistances(const ElementType* vec, std::vector<DistanceType>& dists) const
{
DistanceType distsq = 0.0;
for (size_t i = 0; i < veclen_; ++i) {
if (vec[i] < root_bbox_[i].low) {
dists[i] = distance_.accum_dist(vec[i], root_bbox_[i].low, i);
distsq += dists[i];
}
if (vec[i] > root_bbox_[i].high) {
dists[i] = distance_.accum_dist(vec[i], root_bbox_[i].high, i);
distsq += dists[i];
}
}
return distsq;
}
/**
* Performs an exact search in the tree starting from a node.
*/
template <bool with_removed>
void searchLevel(ResultSet<DistanceType>& result_set, const ElementType* vec, const NodePtr node, DistanceType mindistsq,
std::vector<DistanceType>& dists, const float epsError) const
{
/* If this is a leaf node, then do check and return. */
if ((node->child1 == NULL)&&(node->child2 == NULL)) {
DistanceType worst_dist = result_set.worstDist();
for (int i=node->left; i<node->right; ++i) {
if (with_removed) {
if (removed_points_.test(vind_[i])) continue;
}
ElementType* point = reorder_ ? data_[i] : points_[vind_[i]];
DistanceType dist = distance_(vec, point, veclen_, worst_dist);
if (dist<worst_dist) {
result_set.addPoint(dist,vind_[i]);
}
}
return;
}
/* Which child branch should be taken first? */
int idx = node->divfeat;
ElementType val = vec[idx];
DistanceType diff1 = val - node->divlow;
DistanceType diff2 = val - node->divhigh;
NodePtr bestChild;
NodePtr otherChild;
DistanceType cut_dist;
if ((diff1+diff2)<0) {
bestChild = node->child1;
otherChild = node->child2;
cut_dist = distance_.accum_dist(val, node->divhigh, idx);
}
else {
bestChild = node->child2;
otherChild = node->child1;
cut_dist = distance_.accum_dist( val, node->divlow, idx);
}
/* Call recursively to search next level down. */
searchLevel<with_removed>(result_set, vec, bestChild, mindistsq, dists, epsError);
DistanceType dst = dists[idx];
mindistsq = mindistsq + cut_dist - dst;
dists[idx] = cut_dist;
if (mindistsq*epsError<=result_set.worstDist()) {
searchLevel<with_removed>(result_set, vec, otherChild, mindistsq, dists, epsError);
}
dists[idx] = dst;
}
void swap(KDTreeSingleIndex& other)
{
BaseClass::swap(other);
std::swap(leaf_max_size_, other.leaf_max_size_);
std::swap(reorder_, other.reorder_);
std::swap(vind_, other.vind_);
std::swap(data_, other.data_);
std::swap(root_node_, other.root_node_);
std::swap(root_bbox_, other.root_bbox_);
std::swap(pool_, other.pool_);
}
private:
int leaf_max_size_;
bool reorder_;
/**
* Array of indices to vectors in the dataset.
*/
std::vector<int> vind_;
Matrix<ElementType> data_;
/**
* Array of k-d trees used to find neighbours.
*/
NodePtr root_node_;
/**
* Root bounding box
*/
BoundingBox root_bbox_;
/**
* Pooled memory allocator.
*
* Using a pooled memory allocator is more efficient
* than allocating memory directly when there is a large
* number small of memory allocations.
*/
PooledAllocator pool_;
USING_BASECLASS_SYMBOLS
}; // class KDTreeSingleIndex
}
#endif //FLANN_KDTREE_SINGLE_INDEX_H_

1066
corelib/src/rtflann/algorithms/kmeans_index.h Normal file
View File

File diff suppressed because it is too large Load Diff

163
corelib/src/rtflann/algorithms/linear_index.h Normal file
View File

@@ -0,0 +1,163 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_LINEAR_INDEX_H_
#define RTABMAP_FLANN_LINEAR_INDEX_H_
#include "rtflann/general.h"
#include "nn_index.h"
namespace rtflann
{
struct LinearIndexParams : public IndexParams
{
LinearIndexParams()
{
(* this)["algorithm"] = FLANN_INDEX_LINEAR;
}
};
template <typename Distance>
class LinearIndex : public NNIndex<Distance>
{
public:
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
typedef NNIndex<Distance> BaseClass;
LinearIndex(const IndexParams& params = LinearIndexParams(), Distance d = Distance()) :
BaseClass(params, d)
{
}
LinearIndex(const Matrix<ElementType>& input_data, const IndexParams& params = LinearIndexParams(), Distance d = Distance()) :
BaseClass(params, d)
{
setDataset(input_data);
}
LinearIndex(const LinearIndex& other) : BaseClass(other)
{
}
LinearIndex& operator=(LinearIndex other)
{
this->swap(other);
return *this;
}
virtual ~LinearIndex()
{
}
BaseClass* clone() const
{
return new LinearIndex(*this);
}
void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{
assert(points.cols==veclen_);
extendDataset(points);
}
flann_algorithm_t getType() const
{
return FLANN_INDEX_LINEAR;
}
int usedMemory() const
{
return 0;
}
template<typename Archive>
void serialize(Archive& ar)
{
ar.setObject(this);
ar & *static_cast<NNIndex<Distance>*>(this);
if (Archive::is_loading::value) {
index_params_["algorithm"] = getType();
}
}
void saveIndex(FILE* stream)
{
serialization::SaveArchive sa(stream);
sa & *this;
}
void loadIndex(FILE* stream)
{
serialization::LoadArchive la(stream);
la & *this;
}
void findNeighbors(ResultSet<DistanceType>& resultSet, const ElementType* vec, const SearchParams& /*searchParams*/) const
{
if (removed_) {
for (size_t i = 0; i < points_.size(); ++i) {
if (removed_points_.test(i)) continue;
DistanceType dist = distance_(points_[i], vec, veclen_);
resultSet.addPoint(dist, i);
}
}
else {
for (size_t i = 0; i < points_.size(); ++i) {
DistanceType dist = distance_(points_[i], vec, veclen_);
resultSet.addPoint(dist, i);
}
}
}
protected:
void buildIndexImpl()
{
/* nothing to do here for linear search */
}
void freeIndex()
{
/* nothing to do here for linear search */
}
private:
USING_BASECLASS_SYMBOLS
};
}
#endif // FLANN_LINEAR_INDEX_H_

548
corelib/src/rtflann/algorithms/lsh_index.h Normal file
View File

@@ -0,0 +1,548 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
/***********************************************************************
* Author: Vincent Rabaud
*************************************************************************/
#ifndef RTABMAP_FLANN_LSH_INDEX_H_
#define RTABMAP_FLANN_LSH_INDEX_H_
#include <algorithm>
#include <cassert>
#include <cstring>
#include <map>
#include <vector>
#include "rtflann/general.h"
#include "rtflann/algorithms/nn_index.h"
#include "rtflann/util/matrix.h"
#include "rtflann/util/result_set.h"
#include "rtflann/util/heap.h"
#include "rtflann/util/lsh_table.h"
#include "rtflann/util/allocator.h"
#include "rtflann/util/random.h"
#include "rtflann/util/saving.h"
namespace rtflann
{
struct LshIndexParams : public IndexParams
{
LshIndexParams(unsigned int table_number = 12, unsigned int key_size = 20, unsigned int multi_probe_level = 2)
{
(* this)["algorithm"] = FLANN_INDEX_LSH;
// The number of hash tables to use
(*this)["table_number"] = table_number;
// The length of the key in the hash tables
(*this)["key_size"] = key_size;
// Number of levels to use in multi-probe (0 for standard LSH)
(*this)["multi_probe_level"] = multi_probe_level;
}
};
/**
* Randomized kd-tree index
*
* Contains the k-d trees and other information for indexing a set of points
* for nearest-neighbor matching.
*/
template<typename Distance>
class LshIndex : public NNIndex<Distance>
{
public:
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
typedef NNIndex<Distance> BaseClass;
/** Constructor
* @param params parameters passed to the LSH algorithm
* @param d the distance used
*/
LshIndex(const IndexParams& params = LshIndexParams(), Distance d = Distance()) :
BaseClass(params, d)
{
table_number_ = get_param<unsigned int>(index_params_,"table_number",12);
key_size_ = get_param<unsigned int>(index_params_,"key_size",20);
multi_probe_level_ = get_param<unsigned int>(index_params_,"multi_probe_level",2);
fill_xor_mask(0, key_size_, multi_probe_level_, xor_masks_);
}
/** Constructor
* @param input_data dataset with the input features
* @param params parameters passed to the LSH algorithm
* @param d the distance used
*/
LshIndex(const Matrix<ElementType>& input_data, const IndexParams& params = LshIndexParams(), Distance d = Distance()) :
BaseClass(params, d)
{
table_number_ = get_param<unsigned int>(index_params_,"table_number",12);
key_size_ = get_param<unsigned int>(index_params_,"key_size",20);
multi_probe_level_ = get_param<unsigned int>(index_params_,"multi_probe_level",2);
fill_xor_mask(0, key_size_, multi_probe_level_, xor_masks_);
setDataset(input_data);
}
LshIndex(const LshIndex& other) : BaseClass(other),
tables_(other.tables_),
table_number_(other.table_number_),
key_size_(other.key_size_),
multi_probe_level_(other.multi_probe_level_),
xor_masks_(other.xor_masks_)
{
}
LshIndex& operator=(LshIndex other)
{
this->swap(other);
return *this;
}
virtual ~LshIndex()
{
freeIndex();
}
BaseClass* clone() const
{
return new LshIndex(*this);
}
using BaseClass::buildIndex;
void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{
assert(points.cols==veclen_);
size_t old_size = size_;
extendDataset(points);
if (rebuild_threshold>1 && size_at_build_*rebuild_threshold<size_) {
buildIndex();
}
else {
for (unsigned int i = 0; i < table_number_; ++i) {
lsh::LshTable<ElementType>& table = tables_[i];
for (size_t i=old_size;i<size_;++i) {
table.add(i, points_[i]);
}
}
}
}
flann_algorithm_t getType() const
{
return FLANN_INDEX_LSH;
}
template<typename Archive>
void serialize(Archive& ar)
{
ar.setObject(this);
ar & *static_cast<NNIndex<Distance>*>(this);
ar & table_number_;
ar & key_size_;
ar & multi_probe_level_;
ar & xor_masks_;
ar & tables_;
if (Archive::is_loading::value) {
index_params_["algorithm"] = getType();
index_params_["table_number"] = table_number_;
index_params_["key_size"] = key_size_;
index_params_["multi_probe_level"] = multi_probe_level_;
}
}
void saveIndex(FILE* stream)
{
serialization::SaveArchive sa(stream);
sa & *this;
}
void loadIndex(FILE* stream)
{
serialization::LoadArchive la(stream);
la & *this;
}
/**
* Computes the index memory usage
* Returns: memory used by the index
*/
int usedMemory() const
{
return size_ * sizeof(int);
}
/**
* \brief Perform k-nearest neighbor search
* \param[in] queries The query points for which to find the nearest neighbors
* \param[out] indices The indices of the nearest neighbors found
* \param[out] dists Distances to the nearest neighbors found
* \param[in] knn Number of nearest neighbors to return
* \param[in] params Search parameters
*/
int knnSearch(const Matrix<ElementType>& queries,
Matrix<size_t>& indices,
Matrix<DistanceType>& dists,
size_t knn,
const SearchParams& params) const
{
assert(queries.cols == veclen_);
assert(indices.rows >= queries.rows);
assert(dists.rows >= queries.rows);
assert(indices.cols >= knn);
assert(dists.cols >= knn);
int count = 0;
if (params.use_heap==FLANN_True) {
#pragma omp parallel num_threads(params.cores)
{
KNNUniqueResultSet<DistanceType> resultSet(knn);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = std::min(resultSet.size(), knn);
resultSet.copy(indices[i], dists[i], n, params.sorted);
indices_to_ids(indices[i], indices[i], n);
count += n;
}
}
}
else {
#pragma omp parallel num_threads(params.cores)
{
KNNResultSet<DistanceType> resultSet(knn);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = std::min(resultSet.size(), knn);
resultSet.copy(indices[i], dists[i], n, params.sorted);
indices_to_ids(indices[i], indices[i], n);
count += n;
}
}
}
return count;
}
/**
* \brief Perform k-nearest neighbor search
* \param[in] queries The query points for which to find the nearest neighbors
* \param[out] indices The indices of the nearest neighbors found
* \param[out] dists Distances to the nearest neighbors found
* \param[in] knn Number of nearest neighbors to return
* \param[in] params Search parameters
*/
int knnSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<size_t> >& indices,
std::vector<std::vector<DistanceType> >& dists,
size_t knn,
const SearchParams& params) const
{
assert(queries.cols == veclen_);
if (indices.size() < queries.rows ) indices.resize(queries.rows);
if (dists.size() < queries.rows ) dists.resize(queries.rows);
int count = 0;
if (params.use_heap==FLANN_True) {
#pragma omp parallel num_threads(params.cores)
{
KNNUniqueResultSet<DistanceType> resultSet(knn);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = std::min(resultSet.size(), knn);
indices[i].resize(n);
dists[i].resize(n);
if (n > 0) {
resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
indices_to_ids(&indices[i][0], &indices[i][0], n);
}
count += n;
}
}
}
else {
#pragma omp parallel num_threads(params.cores)
{
KNNResultSet<DistanceType> resultSet(knn);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = std::min(resultSet.size(), knn);
indices[i].resize(n);
dists[i].resize(n);
if (n > 0) {
resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
indices_to_ids(&indices[i][0], &indices[i][0], n);
}
count += n;
}
}
}
return count;
}
/**
* Find set of nearest neighbors to vec. Their indices are stored inside
* the result object.
*
* Params:
* result = the result object in which the indices of the nearest-neighbors are stored
* vec = the vector for which to search the nearest neighbors
* maxCheck = the maximum number of restarts (in a best-bin-first manner)
*/
void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& /*searchParams*/) const
{
getNeighbors(vec, result);
}
protected:
/**
* Builds the index
*/
void buildIndexImpl()
{
tables_.resize(table_number_);
std::vector<std::pair<size_t,ElementType*> > features;
features.reserve(points_.size());
for (size_t i=0;i<points_.size();++i) {
features.push_back(std::make_pair(i, points_[i]));
}
for (unsigned int i = 0; i < table_number_; ++i) {
lsh::LshTable<ElementType>& table = tables_[i];
table = lsh::LshTable<ElementType>(veclen_, key_size_);
// Add the features to the table
table.add(features);
}
}
void freeIndex()
{
/* nothing to do here */
}
private:
/** Defines the comparator on score and index
*/
typedef std::pair<float, unsigned int> ScoreIndexPair;
struct SortScoreIndexPairOnSecond
{
bool operator()(const ScoreIndexPair& left, const ScoreIndexPair& right) const
{
return left.second < right.second;
}
};
/** Fills the different xor masks to use when getting the neighbors in multi-probe LSH
* @param key the key we build neighbors from
* @param lowest_index the lowest index of the bit set
* @param level the multi-probe level we are at
* @param xor_masks all the xor mask
*/
void fill_xor_mask(lsh::BucketKey key, int lowest_index, unsigned int level,
std::vector<lsh::BucketKey>& xor_masks)
{
xor_masks.push_back(key);
if (level == 0) return;
for (int index = lowest_index - 1; index >= 0; --index) {
// Create a new key
lsh::BucketKey new_key = key | (lsh::BucketKey(1) << index);
fill_xor_mask(new_key, index, level - 1, xor_masks);
}
}
/** Performs the approximate nearest-neighbor search.
* @param vec the feature to analyze
* @param do_radius flag indicating if we check the radius too
* @param radius the radius if it is a radius search
* @param do_k flag indicating if we limit the number of nn
* @param k_nn the number of nearest neighbors
* @param checked_average used for debugging
*/
void getNeighbors(const ElementType* vec, bool do_radius, float radius, bool do_k, unsigned int k_nn,
float& checked_average)
{
static std::vector<ScoreIndexPair> score_index_heap;
if (do_k) {
unsigned int worst_score = std::numeric_limits<unsigned int>::max();
typename std::vector<lsh::LshTable<ElementType> >::const_iterator table = tables_.begin();
typename std::vector<lsh::LshTable<ElementType> >::const_iterator table_end = tables_.end();
for (; table != table_end; ++table) {
size_t key = table->getKey(vec);
std::vector<lsh::BucketKey>::const_iterator xor_mask = xor_masks_.begin();
std::vector<lsh::BucketKey>::const_iterator xor_mask_end = xor_masks_.end();
for (; xor_mask != xor_mask_end; ++xor_mask) {
size_t sub_key = key ^ (*xor_mask);
const lsh::Bucket* bucket = table->getBucketFromKey(sub_key);
if (bucket == 0) continue;
// Go over each descriptor index
std::vector<lsh::FeatureIndex>::const_iterator training_index = bucket->begin();
std::vector<lsh::FeatureIndex>::const_iterator last_training_index = bucket->end();
DistanceType hamming_distance;
// Process the rest of the candidates
for (; training_index < last_training_index; ++training_index) {
if (removed_ && removed_points_.test(*training_index)) continue;
hamming_distance = distance_(vec, points_[*training_index].point, veclen_);
if (hamming_distance < worst_score) {
// Insert the new element
score_index_heap.push_back(ScoreIndexPair(hamming_distance, training_index));
std::push_heap(score_index_heap.begin(), score_index_heap.end());
if (score_index_heap.size() > (unsigned int)k_nn) {
// Remove the highest distance value as we have too many elements
std::pop_heap(score_index_heap.begin(), score_index_heap.end());
score_index_heap.pop_back();
// Keep track of the worst score
worst_score = score_index_heap.front().first;
}
}
}
}
}
}
else {
typename std::vector<lsh::LshTable<ElementType> >::const_iterator table = tables_.begin();
typename std::vector<lsh::LshTable<ElementType> >::const_iterator table_end = tables_.end();
for (; table != table_end; ++table) {
size_t key = table->getKey(vec);
std::vector<lsh::BucketKey>::const_iterator xor_mask = xor_masks_.begin();
std::vector<lsh::BucketKey>::const_iterator xor_mask_end = xor_masks_.end();
for (; xor_mask != xor_mask_end; ++xor_mask) {
size_t sub_key = key ^ (*xor_mask);
const lsh::Bucket* bucket = table->getBucketFromKey(sub_key);
if (bucket == 0) continue;
// Go over each descriptor index
std::vector<lsh::FeatureIndex>::const_iterator training_index = bucket->begin();
std::vector<lsh::FeatureIndex>::const_iterator last_training_index = bucket->end();
DistanceType hamming_distance;
// Process the rest of the candidates
for (; training_index < last_training_index; ++training_index) {
if (removed_ && removed_points_.test(*training_index)) continue;
// Compute the Hamming distance
hamming_distance = distance_(vec, points_[*training_index].point, veclen_);
if (hamming_distance < radius) score_index_heap.push_back(ScoreIndexPair(hamming_distance, training_index));
}
}
}
}
}
/** Performs the approximate nearest-neighbor search.
* This is a slower version than the above as it uses the ResultSet
* @param vec the feature to analyze
*/
void getNeighbors(const ElementType* vec, ResultSet<DistanceType>& result) const
{
typename std::vector<lsh::LshTable<ElementType> >::const_iterator table = tables_.begin();
typename std::vector<lsh::LshTable<ElementType> >::const_iterator table_end = tables_.end();
for (; table != table_end; ++table) {
size_t key = table->getKey(vec);
std::vector<lsh::BucketKey>::const_iterator xor_mask = xor_masks_.begin();
std::vector<lsh::BucketKey>::const_iterator xor_mask_end = xor_masks_.end();
for (; xor_mask != xor_mask_end; ++xor_mask) {
size_t sub_key = key ^ (*xor_mask);
const lsh::Bucket* bucket = table->getBucketFromKey(sub_key);
if (bucket == 0) continue;
// Go over each descriptor index
std::vector<lsh::FeatureIndex>::const_iterator training_index = bucket->begin();
std::vector<lsh::FeatureIndex>::const_iterator last_training_index = bucket->end();
DistanceType hamming_distance;
// Process the rest of the candidates
for (; training_index < last_training_index; ++training_index) {
if (removed_ && removed_points_.test(*training_index)) continue;
// Compute the Hamming distance
hamming_distance = distance_(vec, points_[*training_index], veclen_);
result.addPoint(hamming_distance, *training_index);
}
}
}
}
void swap(LshIndex& other)
{
BaseClass::swap(other);
std::swap(tables_, other.tables_);
std::swap(size_at_build_, other.size_at_build_);
std::swap(table_number_, other.table_number_);
std::swap(key_size_, other.key_size_);
std::swap(multi_probe_level_, other.multi_probe_level_);
std::swap(xor_masks_, other.xor_masks_);
}
/** The different hash tables */
std::vector<lsh::LshTable<ElementType> > tables_;
/** table number */
unsigned int table_number_;
/** key size */
unsigned int key_size_;
/** How far should we look for neighbors in multi-probe LSH */
unsigned int multi_probe_level_;
/** The XOR masks to apply to a key to get the neighboring buckets */
std::vector<lsh::BucketKey> xor_masks_;
USING_BASECLASS_SYMBOLS
};
}
#endif //FLANN_LSH_INDEX_H_

907
corelib/src/rtflann/algorithms/nn_index.h Normal file
View File

@@ -0,0 +1,907 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_NNINDEX_H
#define RTABMAP_FLANN_NNINDEX_H
#include <vector>
#include "rtflann/general.h"
#include "rtflann/util/matrix.h"
#include "rtflann/util/params.h"
#include "rtflann/util/result_set.h"
#include "rtflann/util/dynamic_bitset.h"
#include "rtflann/util/saving.h"
namespace rtflann
{
#define KNN_HEAP_THRESHOLD 250
class IndexBase
{
public:
virtual ~IndexBase() {};
virtual size_t veclen() const = 0;
virtual size_t size() const = 0;
virtual flann_algorithm_t getType() const = 0;
virtual int usedMemory() const = 0;
virtual IndexParams getParameters() const = 0;
virtual void loadIndex(FILE* stream) = 0;
virtual void saveIndex(FILE* stream) = 0;
};
/**
* Nearest-neighbour index base class
*/
template <typename Distance>
class NNIndex : public IndexBase
{
public:
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
NNIndex(Distance d) : distance_(d), last_id_(0), size_(0), size_at_build_(0), veclen_(0),
removed_(false), removed_count_(0), data_ptr_(NULL)
{
}
NNIndex(const IndexParams& params, Distance d) : distance_(d), last_id_(0), size_(0), size_at_build_(0), veclen_(0),
index_params_(params), removed_(false), removed_count_(0), data_ptr_(NULL)
{
}
NNIndex(const NNIndex& other) :
distance_(other.distance_),
last_id_(other.last_id_),
size_(other.size_),
size_at_build_(other.size_at_build_),
veclen_(other.veclen_),
index_params_(other.index_params_),
removed_(other.removed_),
removed_points_(other.removed_points_),
removed_count_(other.removed_count_),
ids_(other.ids_),
points_(other.points_),
data_ptr_(NULL)
{
if (other.data_ptr_) {
data_ptr_ = new ElementType[size_*veclen_];
std::copy(other.data_ptr_, other.data_ptr_+size_*veclen_, data_ptr_);
for (size_t i=0;i<size_;++i) {
points_[i] = data_ptr_ + i*veclen_;
}
}
}
virtual ~NNIndex()
{
if (data_ptr_) {
delete[] data_ptr_;
}
}
virtual NNIndex* clone() const = 0;
/**
* Builds the index
*/
virtual void buildIndex()
{
freeIndex();
cleanRemovedPoints();
// building index
buildIndexImpl();
size_at_build_ = size_;
}
/**
* Builds the index using the specified dataset
* @param dataset the dataset to use
*/
virtual void buildIndex(const Matrix<ElementType>& dataset)
{
setDataset(dataset);
this->buildIndex();
}
/**
* @brief Incrementally add points to the index.
* @param points Matrix with points to be added
* @param rebuild_threshold
*/
virtual void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{
throw FLANNException("Functionality not supported by this index");
}
/**
* Remove point from the index
* @param index Index of point to be removed
*/
virtual void removePoint(size_t id)
{
if (!removed_) {
ids_.resize(size_);
for (size_t i=0;i<size_;++i) {
ids_[i] = i;
}
removed_points_.resize(size_);
removed_points_.reset();
last_id_ = size_;
removed_ = true;
}
size_t point_index = id_to_index(id);
if (point_index!=size_t(-1) && !removed_points_.test(point_index)) {
removed_points_.set(point_index);
removed_count_++;
}
}
/**
* Get point with specific id
* @param id
* @return
*/
virtual ElementType* getPoint(size_t id)
{
size_t index = id_to_index(id);
if (index!=size_t(-1)) {
return points_[index];
}
else {
return NULL;
}
}
/**
* @return number of features in this index.
*/
inline size_t size() const
{
return size_ - removed_count_;
}
/**
* @return The dimensionality of the features in this index.
*/
inline size_t veclen() const
{
return veclen_;
}
/**
* Returns the parameters used by the index.
*
* @return The index parameters
*/
IndexParams getParameters() const
{
return index_params_;
}
template<typename Archive>
void serialize(Archive& ar)
{
IndexHeader header;
if (Archive::is_saving::value) {
header.h.data_type = flann_datatype_value<ElementType>::value;
header.h.index_type = getType();
header.h.rows = size_;
header.h.cols = veclen_;
}
ar & header;
// sanity checks
if (Archive::is_loading::value) {
if (strncmp(header.h.signature,
FLANN_SIGNATURE_,
strlen(FLANN_SIGNATURE_) - strlen("v0.0")) != 0) {
throw FLANNException("Invalid index file, wrong signature");
}
if (header.h.data_type != flann_datatype_value<ElementType>::value) {
throw FLANNException("Datatype of saved index is different than of the one to be created.");
}
if (header.h.index_type != getType()) {
throw FLANNException("Saved index type is different then the current index type.");
}
// TODO: check for distance type
}
ar & size_;
ar & veclen_;
ar & size_at_build_;
bool save_dataset;
if (Archive::is_saving::value) {
save_dataset = get_param(index_params_,"save_dataset", false);
}
ar & save_dataset;
if (save_dataset) {
if (Archive::is_loading::value) {
if (data_ptr_) {
delete[] data_ptr_;
}
data_ptr_ = new ElementType[size_*veclen_];
points_.resize(size_);
for (size_t i=0;i<size_;++i) {
points_[i] = data_ptr_ + i*veclen_;
}
}
for (size_t i=0;i<size_;++i) {
ar & serialization::make_binary_object (points_[i], veclen_*sizeof(ElementType));
}
} else {
if (points_.size()!=size_) {
throw FLANNException("Saved index does not contain the dataset and no dataset was provided.");
}
}
ar & last_id_;
ar & ids_;
ar & removed_;
if (removed_) {
ar & removed_points_;
}
ar & removed_count_;
}
/**
* @brief Perform k-nearest neighbor search
* @param[in] queries The query points for which to find the nearest neighbors
* @param[out] indices The indices of the nearest neighbors found
* @param[out] dists Distances to the nearest neighbors found
* @param[in] knn Number of nearest neighbors to return
* @param[in] params Search parameters
*/
virtual int knnSearch(const Matrix<ElementType>& queries,
Matrix<size_t>& indices,
Matrix<DistanceType>& dists,
size_t knn,
const SearchParams& params) const
{
assert(queries.cols == veclen());
assert(indices.rows >= queries.rows);
assert(dists.rows >= queries.rows);
assert(indices.cols >= knn);
assert(dists.cols >= knn);
bool use_heap;
if (params.use_heap==FLANN_Undefined) {
use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
}
else {
use_heap = (params.use_heap==FLANN_True)?true:false;
}
int count = 0;
if (use_heap) {
#pragma omp parallel num_threads(params.cores)
{
KNNResultSet2<DistanceType> resultSet(knn);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = std::min(resultSet.size(), knn);
resultSet.copy(indices[i], dists[i], n, params.sorted);
indices_to_ids(indices[i], indices[i], n);
count += n;
}
}
}
else {
#pragma omp parallel num_threads(params.cores)
{
KNNSimpleResultSet<DistanceType> resultSet(knn);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = std::min(resultSet.size(), knn);
resultSet.copy(indices[i], dists[i], n, params.sorted);
indices_to_ids(indices[i], indices[i], n);
count += n;
}
}
}
return count;
}
/**
*
* @param queries
* @param indices
* @param dists
* @param knn
* @param params
* @return
*/
int knnSearch(const Matrix<ElementType>& queries,
Matrix<int>& indices,
Matrix<DistanceType>& dists,
size_t knn,
const SearchParams& params) const
{
rtflann::Matrix<size_t> indices_(new size_t[indices.rows*indices.cols], indices.rows, indices.cols);
int result = knnSearch(queries, indices_, dists, knn, params);
for (size_t i=0;i<indices.rows;++i) {
for (size_t j=0;j<indices.cols;++j) {
indices[i][j] = indices_[i][j];
}
}
delete[] indices_.ptr();
return result;
}
/**
* @brief Perform k-nearest neighbor search
* @param[in] queries The query points for which to find the nearest neighbors
* @param[out] indices The indices of the nearest neighbors found
* @param[out] dists Distances to the nearest neighbors found
* @param[in] knn Number of nearest neighbors to return
* @param[in] params Search parameters
*/
int knnSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<size_t> >& indices,
std::vector<std::vector<DistanceType> >& dists,
size_t knn,
const SearchParams& params) const
{
assert(queries.cols == veclen());
bool use_heap;
if (params.use_heap==FLANN_Undefined) {
use_heap = (knn>KNN_HEAP_THRESHOLD)?true:false;
}
else {
use_heap = (params.use_heap==FLANN_True)?true:false;
}
if (indices.size() < queries.rows ) indices.resize(queries.rows);
if (dists.size() < queries.rows ) dists.resize(queries.rows);
int count = 0;
if (use_heap) {
#pragma omp parallel num_threads(params.cores)
{
KNNResultSet2<DistanceType> resultSet(knn);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = std::min(resultSet.size(), knn);
indices[i].resize(n);
dists[i].resize(n);
if (n>0) {
resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
indices_to_ids(&indices[i][0], &indices[i][0], n);
}
count += n;
}
}
}
else {
#pragma omp parallel num_threads(params.cores)
{
KNNSimpleResultSet<DistanceType> resultSet(knn);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = std::min(resultSet.size(), knn);
indices[i].resize(n);
dists[i].resize(n);
if (n>0) {
resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
indices_to_ids(&indices[i][0], &indices[i][0], n);
}
count += n;
}
}
}
return count;
}
/**
*
* @param queries
* @param indices
* @param dists
* @param knn
* @param params
* @return
*/
int knnSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<int> >& indices,
std::vector<std::vector<DistanceType> >& dists,
size_t knn,
const SearchParams& params) const
{
std::vector<std::vector<size_t> > indices_;
int result = knnSearch(queries, indices_, dists, knn, params);
indices.resize(indices_.size());
for (size_t i=0;i<indices_.size();++i) {
indices[i].assign(indices_[i].begin(), indices_[i].end());
}
return result;
}
/**
* @brief Perform radius search
* @param[in] query The query point
* @param[out] indices The indices of the neighbors found within the given radius
* @param[out] dists The distances to the nearest neighbors found
* @param[in] radius The radius used for search
* @param[in] params Search parameters
* @return Number of neighbors found
*/
int radiusSearch(const Matrix<ElementType>& queries,
Matrix<size_t>& indices,
Matrix<DistanceType>& dists,
float radius,
const SearchParams& params) const
{
assert(queries.cols == veclen());
int count = 0;
size_t num_neighbors = std::min(indices.cols, dists.cols);
int max_neighbors = params.max_neighbors;
if (max_neighbors<0) max_neighbors = num_neighbors;
else max_neighbors = std::min(max_neighbors,(int)num_neighbors);
if (max_neighbors==0) {
#pragma omp parallel num_threads(params.cores)
{
CountRadiusResultSet<DistanceType> resultSet(radius);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
count += resultSet.size();
}
}
}
else {
// explicitly indicated to use unbounded radius result set
// and we know there'll be enough room for resulting indices and dists
if (params.max_neighbors<0 && (num_neighbors>=size())) {
#pragma omp parallel num_threads(params.cores)
{
RadiusResultSet<DistanceType> resultSet(radius);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = resultSet.size();
count += n;
if (n>num_neighbors) n = num_neighbors;
resultSet.copy(indices[i], dists[i], n, params.sorted);
// mark the next element in the output buffers as unused
if (n<indices.cols) indices[i][n] = size_t(-1);
if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
indices_to_ids(indices[i], indices[i], n);
}
}
}
else {
// number of neighbors limited to max_neighbors
#pragma omp parallel num_threads(params.cores)
{
KNNRadiusResultSet<DistanceType> resultSet(radius, max_neighbors);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = resultSet.size();
count += n;
if ((int)n>max_neighbors) n = max_neighbors;
resultSet.copy(indices[i], dists[i], n, params.sorted);
// mark the next element in the output buffers as unused
if (n<indices.cols) indices[i][n] = size_t(-1);
if (n<dists.cols) dists[i][n] = std::numeric_limits<DistanceType>::infinity();
indices_to_ids(indices[i], indices[i], n);
}
}
}
}
return count;
}
/**
*
* @param queries
* @param indices
* @param dists
* @param radius
* @param params
* @return
*/
int radiusSearch(const Matrix<ElementType>& queries,
Matrix<int>& indices,
Matrix<DistanceType>& dists,
float radius,
const SearchParams& params) const
{
rtflann::Matrix<size_t> indices_(new size_t[indices.rows*indices.cols], indices.rows, indices.cols);
int result = radiusSearch(queries, indices_, dists, radius, params);
for (size_t i=0;i<indices.rows;++i) {
for (size_t j=0;j<indices.cols;++j) {
indices[i][j] = indices_[i][j];
}
}
delete[] indices_.ptr();
return result;
}
/**
* @brief Perform radius search
* @param[in] query The query point
* @param[out] indices The indices of the neighbors found within the given radius
* @param[out] dists The distances to the nearest neighbors found
* @param[in] radius The radius used for search
* @param[in] params Search parameters
* @return Number of neighbors found
*/
int radiusSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<size_t> >& indices,
std::vector<std::vector<DistanceType> >& dists,
float radius,
const SearchParams& params) const
{
assert(queries.cols == veclen());
int count = 0;
// just count neighbors
if (params.max_neighbors==0) {
#pragma omp parallel num_threads(params.cores)
{
CountRadiusResultSet<DistanceType> resultSet(radius);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
count += resultSet.size();
}
}
}
else {
if (indices.size() < queries.rows ) indices.resize(queries.rows);
if (dists.size() < queries.rows ) dists.resize(queries.rows);
if (params.max_neighbors<0) {
// search for all neighbors
#pragma omp parallel num_threads(params.cores)
{
RadiusResultSet<DistanceType> resultSet(radius);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = resultSet.size();
count += n;
indices[i].resize(n);
dists[i].resize(n);
if (n > 0) {
resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
indices_to_ids(&indices[i][0], &indices[i][0], n);
}
}
}
}
else {
// number of neighbors limited to max_neighbors
#pragma omp parallel num_threads(params.cores)
{
KNNRadiusResultSet<DistanceType> resultSet(radius, params.max_neighbors);
#pragma omp for schedule(static) reduction(+:count)
for (int i = 0; i < (int)queries.rows; i++) {
resultSet.clear();
findNeighbors(resultSet, queries[i], params);
size_t n = resultSet.size();
count += n;
if ((int)n>params.max_neighbors) n = params.max_neighbors;
indices[i].resize(n);
dists[i].resize(n);
if (n > 0) {
resultSet.copy(&indices[i][0], &dists[i][0], n, params.sorted);
indices_to_ids(&indices[i][0], &indices[i][0], n);
}
}
}
}
}
return count;
}
/**
*
* @param queries
* @param indices
* @param dists
* @param radius
* @param params
* @return
*/
int radiusSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<int> >& indices,
std::vector<std::vector<DistanceType> >& dists,
float radius,
const SearchParams& params) const
{
std::vector<std::vector<size_t> > indices_;
int result = radiusSearch(queries, indices_, dists, radius, params);
indices.resize(indices_.size());
for (size_t i=0;i<indices_.size();++i) {
indices[i].assign(indices_[i].begin(), indices_[i].end());
}
return result;
}
virtual void findNeighbors(ResultSet<DistanceType>& result, const ElementType* vec, const SearchParams& searchParams) const = 0;
protected:
virtual void freeIndex() = 0;
virtual void buildIndexImpl() = 0;
size_t id_to_index(size_t id)
{
if (ids_.size()==0) {
return id;
}
size_t point_index = size_t(-1);
if (id < ids_.size() && ids_[id]==id) {
return id;
}
else {
// binary search
size_t start = 0;
size_t end = ids_.size();
while (start<end) {
size_t mid = (start+end)/2;
if (ids_[mid]==id) {
point_index = mid;
break;
}
else if (ids_[mid]<id) {
start = mid + 1;
}
else {
end = mid;
}
}
}
return point_index;
}
void indices_to_ids(const size_t* in, size_t* out, size_t size) const
{
if (removed_) {
for (size_t i=0;i<size;++i) {
out[i] = ids_[in[i]];
}
}
}
void setDataset(const Matrix<ElementType>& dataset)
{
size_ = dataset.rows;
veclen_ = dataset.cols;
last_id_ = 0;
ids_.clear();
removed_points_.clear();
removed_ = false;
removed_count_ = 0;
points_.resize(size_);
for (size_t i=0;i<size_;++i) {
points_[i] = dataset[i];
}
}
void extendDataset(const Matrix<ElementType>& new_points)
{
size_t new_size = size_ + new_points.rows;
if (removed_) {
removed_points_.resize(new_size);
ids_.resize(new_size);
}
points_.resize(new_size);
for (size_t i=size_;i<new_size;++i) {
points_[i] = new_points[i-size_];
if (removed_) {
ids_[i] = last_id_++;
removed_points_.reset(i);
}
}
size_ = new_size;
}
void cleanRemovedPoints()
{
if (!removed_) return;
size_t last_idx = 0;
for (size_t i=0;i<size_;++i) {
if (!removed_points_.test(i)) {
points_[last_idx] = points_[i];
ids_[last_idx] = ids_[i];
removed_points_.reset(last_idx);
++last_idx;
}
}
points_.resize(last_idx);
ids_.resize(last_idx);
removed_points_.resize(last_idx);
size_ = last_idx;
removed_count_ = 0;
}
void swap(NNIndex& other)
{
std::swap(distance_, other.distance_);
std::swap(last_id_, other.last_id_);
std::swap(size_, other.size_);
std::swap(size_at_build_, other.size_at_build_);
std::swap(veclen_, other.veclen_);
std::swap(index_params_, other.index_params_);
std::swap(removed_, other.removed_);
std::swap(removed_points_, other.removed_points_);
std::swap(removed_count_, other.removed_count_);
std::swap(ids_, other.ids_);
std::swap(points_, other.points_);
std::swap(data_ptr_, other.data_ptr_);
}
protected:
/**
* The distance functor
*/
Distance distance_;
/**
* Each index point has an associated ID. IDs are assigned sequentially in
* increasing order. This indicates the ID assigned to the last point added to the
* index.
*/
size_t last_id_;
/**
* Number of points in the index (and database)
*/
size_t size_;
/**
* Number of features in the dataset when the index was last built.
*/
size_t size_at_build_;
/**
* Size of one point in the index (and database)
*/
size_t veclen_;
/**
* Parameters of the index.
*/
IndexParams index_params_;
/**
* Flag indicating if at least a point was removed from the index
*/
bool removed_;
/**
* Array used to mark points removed from the index
*/
DynamicBitset removed_points_;
/**
* Number of points removed from the index
*/
size_t removed_count_;
/**
* Array of point IDs, returned by nearest-neighbour operations
*/
std::vector<size_t> ids_;
/**
* Point data
*/
std::vector<ElementType*> points_;
/**
* Pointer to dataset memory if allocated by this index, otherwise NULL
*/
ElementType* data_ptr_;
};
#define USING_BASECLASS_SYMBOLS \
using NNIndex<Distance>::distance_;\
using NNIndex<Distance>::size_;\
using NNIndex<Distance>::size_at_build_;\
using NNIndex<Distance>::veclen_;\
using NNIndex<Distance>::index_params_;\
using NNIndex<Distance>::removed_points_;\
using NNIndex<Distance>::ids_;\
using NNIndex<Distance>::removed_;\
using NNIndex<Distance>::points_;\
using NNIndex<Distance>::extendDataset;\
using NNIndex<Distance>::setDataset;\
using NNIndex<Distance>::cleanRemovedPoints;\
using NNIndex<Distance>::indices_to_ids;
}
#endif //FLANN_NNINDEX_H