i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

38
corelib/src/rtflann/config.h Normal file
View File

@@ -0,0 +1,38 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2011 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2011 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_CONFIG_H_
#define RTABMAP_FLANN_CONFIG_H_
#ifdef FLANN_VERSION_
#undef FLANN_VERSION_
#endif
#define FLANN_VERSION_ "1.8.4"
#endif /* RTABMAP_FLANN_CONFIG_H_ */

156
corelib/src/rtflann/defines.h Normal file
View File

@@ -0,0 +1,156 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2011 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2011 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_DEFINES_H_
#define RTABMAP_FLANN_DEFINES_H_
#include "config.h"
#ifdef FLANN_EXPORT
#undef FLANN_EXPORT
#endif
#ifdef WIN32
/* win32 dll export/import directives */
#ifdef FLANN_EXPORTS
#define FLANN_EXPORT __declspec(dllexport)
#elif defined(FLANN_STATIC)
#define FLANN_EXPORT
#else
#define FLANN_EXPORT __declspec(dllimport)
#endif
#else
/* unix needs nothing */
#define FLANN_EXPORT
#endif
#ifdef FLANN_DEPRECATED
#undef FLANN_DEPRECATED
#endif
#ifdef __GNUC__
#define FLANN_DEPRECATED __attribute__ ((deprecated))
#elif defined(_MSC_VER)
#define FLANN_DEPRECATED __declspec(deprecated)
#else
#pragma message("WARNING: You need to implement FLANN_DEPRECATED for this compiler")
#define FLANN_DEPRECATED
#endif
#undef FLANN_PLATFORM_64_BIT
#undef FLANN_PLATFORM_32_BIT
#if __amd64__ || __x86_64__ || _WIN64 || _M_X64
#define FLANN_PLATFORM_64_BIT
#else
#define FLANN_PLATFORM_32_BIT
#endif
#undef FLANN_ARRAY_LEN
#define FLANN_ARRAY_LEN(a) (sizeof(a)/sizeof(a[0]))
//#ifdef __cplusplus
namespace rtflann {
//#endif
/* Nearest neighbour index algorithms */
enum flann_algorithm_t
{
FLANN_INDEX_LINEAR = 0,
FLANN_INDEX_KDTREE = 1,
FLANN_INDEX_KMEANS = 2,
FLANN_INDEX_COMPOSITE = 3,
FLANN_INDEX_KDTREE_SINGLE = 4,
FLANN_INDEX_HIERARCHICAL = 5,
FLANN_INDEX_LSH = 6,
#ifdef FLANN_USE_CUDA
FLANN_INDEX_KDTREE_CUDA = 7,
#endif
FLANN_INDEX_SAVED = 254,
FLANN_INDEX_AUTOTUNED = 255,
};
enum flann_centers_init_t
{
FLANN_CENTERS_RANDOM = 0,
FLANN_CENTERS_GONZALES = 1,
FLANN_CENTERS_KMEANSPP = 2,
FLANN_CENTERS_GROUPWISE = 3,
};
enum flann_log_level_t
{
FLANN_LOG_NONE = 0,
FLANN_LOG_FATAL = 1,
FLANN_LOG_ERROR = 2,
FLANN_LOG_WARN = 3,
FLANN_LOG_INFO = 4,
FLANN_LOG_DEBUG = 5
};
enum flann_distance_t
{
FLANN_DIST_EUCLIDEAN = 1,
FLANN_DIST_L2 = 1,
FLANN_DIST_MANHATTAN = 2,
FLANN_DIST_L1 = 2,
FLANN_DIST_MINKOWSKI = 3,
FLANN_DIST_MAX = 4,
FLANN_DIST_HIST_INTERSECT = 5,
FLANN_DIST_HELLINGER = 6,
FLANN_DIST_CHI_SQUARE = 7,
FLANN_DIST_KULLBACK_LEIBLER = 8,
FLANN_DIST_HAMMING = 9,
FLANN_DIST_HAMMING_LUT = 10,
FLANN_DIST_HAMMING_POPCNT = 11,
FLANN_DIST_L2_SIMPLE = 12,
};
enum flann_datatype_t
{
FLANN_NONE = -1,
FLANN_INT8 = 0,
FLANN_INT16 = 1,
FLANN_INT32 = 2,
FLANN_INT64 = 3,
FLANN_UINT8 = 4,
FLANN_UINT16 = 5,
FLANN_UINT32 = 6,
FLANN_UINT64 = 7,
FLANN_FLOAT32 = 8,
FLANN_FLOAT64 = 9
};
enum flann_checks_t {
FLANN_CHECKS_UNLIMITED = -1,
FLANN_CHECKS_AUTOTUNED = -2,
};
//#ifdef __cplusplus
}
//#endif
#endif /* RTABMAP_FLANN_DEFINES_H_ */

1516
corelib/src/rtflann/ext/lz4.c Normal file
View File

File diff suppressed because it is too large Load Diff

360
corelib/src/rtflann/ext/lz4.h Normal file
View File

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

731
corelib/src/rtflann/ext/lz4hc.c Normal file
View File

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

189
corelib/src/rtflann/ext/lz4hc.h Normal file
View File

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

435
corelib/src/rtflann/flann.hpp Normal file
View File

@@ -0,0 +1,435 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_HPP_
#define RTABMAP_FLANN_HPP_
#include <vector>
#include <string>
#include <cassert>
#include <cstdio>
#include "general.h"
#include "util/matrix.h"
#include "util/params.h"
#include "util/saving.h"
#include "algorithms/all_indices.h"
namespace rtflann
{
/**
* Sets the log level used for all flann functions
* @param level Verbosity level
*/
inline void log_verbosity(int level)
{
if (level >= 0) {
Logger::setLevel(level);
}
}
/**
* (Deprecated) Index parameters for creating a saved index.
*/
struct SavedIndexParams : public IndexParams
{
SavedIndexParams(std::string filename)
{
(*this)["algorithm"] = FLANN_INDEX_SAVED;
(*this)["filename"] = filename;
}
};
template<typename Distance>
class Index
{
public:
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
typedef NNIndex<Distance> IndexType;
Index(const IndexParams& params, Distance distance = Distance() )
: index_params_(params)
{
flann_algorithm_t index_type = get_param<flann_algorithm_t>(params,"algorithm");
loaded_ = false;
Matrix<ElementType> features;
if (index_type == FLANN_INDEX_SAVED) {
nnIndex_ = load_saved_index(features, get_param<std::string>(params,"filename"), distance);
loaded_ = true;
}
else {
flann_algorithm_t index_type = get_param<flann_algorithm_t>(params, "algorithm");
nnIndex_ = create_index_by_type<Distance>(index_type, features, params, distance);
}
}
Index(const Matrix<ElementType>& features, const IndexParams& params, Distance distance = Distance() )
: index_params_(params)
{
flann_algorithm_t index_type = get_param<flann_algorithm_t>(params,"algorithm");
loaded_ = false;
if (index_type == FLANN_INDEX_SAVED) {
nnIndex_ = load_saved_index(features, get_param<std::string>(params,"filename"), distance);
loaded_ = true;
}
else {
flann_algorithm_t index_type = get_param<flann_algorithm_t>(params, "algorithm");
nnIndex_ = create_index_by_type<Distance>(index_type, features, params, distance);
}
}
Index(const Index& other) : loaded_(other.loaded_), index_params_(other.index_params_)
{
nnIndex_ = other.nnIndex_->clone();
}
Index& operator=(Index other)
{
this->swap(other);
return *this;
}
virtual ~Index()
{
delete nnIndex_;
}
/**
* Builds the index.
*/
void buildIndex()
{
if (!loaded_) {
nnIndex_->buildIndex();
}
}
void buildIndex(const Matrix<ElementType>& points)
{
nnIndex_->buildIndex(points);
}
void addPoints(const Matrix<ElementType>& points, float rebuild_threshold = 2)
{
nnIndex_->addPoints(points, rebuild_threshold);
}
/**
* Remove point from the index
* @param index Index of point to be removed
*/
void removePoint(size_t point_id)
{
nnIndex_->removePoint(point_id);
}
/**
* Returns pointer to a data point with the specified id.
* @param point_id the id of point to retrieve
* @return
*/
ElementType* getPoint(size_t point_id)
{
return nnIndex_->getPoint(point_id);
}
/**
* Save index to file
* @param filename
*/
void save(std::string filename)
{
FILE* fout = fopen(filename.c_str(), "wb");
if (fout == NULL) {
throw FLANNException("Cannot open file");
}
nnIndex_->saveIndex(fout);
fclose(fout);
}
/**
* \returns number of features in this index.
*/
size_t veclen() const
{
return nnIndex_->veclen();
}
/**
* \returns The dimensionality of the features in this index.
*/
size_t size() const
{
return nnIndex_->size();
}
/**
* \returns The index type (kdtree, kmeans,...)
*/
flann_algorithm_t getType() const
{
return nnIndex_->getType();
}
/**
* \returns The amount of memory (in bytes) used by the index.
*/
int usedMemory() const
{
return nnIndex_->usedMemory();
}
/**
* \returns The index parameters
*/
IndexParams getParameters() const
{
return nnIndex_->getParameters();
}
/**
* \brief Perform k-nearest neighbor search
* \param[in] queries The query points for which to find the nearest neighbors
* \param[out] indices The indices of the nearest neighbors found
* \param[out] dists Distances to the nearest neighbors found
* \param[in] knn Number of nearest neighbors to return
* \param[in] params Search parameters
*/
int knnSearch(const Matrix<ElementType>& queries,
Matrix<size_t>& indices,
Matrix<DistanceType>& dists,
size_t knn,
const SearchParams& params) const
{
return nnIndex_->knnSearch(queries, indices, dists, knn, params);
}
/**
*
* @param queries
* @param indices
* @param dists
* @param knn
* @param params
* @return
*/
int knnSearch(const Matrix<ElementType>& queries,
Matrix<int>& indices,
Matrix<DistanceType>& dists,
size_t knn,
const SearchParams& params) const
{
return nnIndex_->knnSearch(queries, indices, dists, knn, params);
}
/**
* \brief Perform k-nearest neighbor search
* \param[in] queries The query points for which to find the nearest neighbors
* \param[out] indices The indices of the nearest neighbors found
* \param[out] dists Distances to the nearest neighbors found
* \param[in] knn Number of nearest neighbors to return
* \param[in] params Search parameters
*/
int knnSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<size_t> >& indices,
std::vector<std::vector<DistanceType> >& dists,
size_t knn,
const SearchParams& params) const
{
return nnIndex_->knnSearch(queries, indices, dists, knn, params);
}
/**
*
* @param queries
* @param indices
* @param dists
* @param knn
* @param params
* @return
*/
int knnSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<int> >& indices,
std::vector<std::vector<DistanceType> >& dists,
size_t knn,
const SearchParams& params) const
{
return nnIndex_->knnSearch(queries, indices, dists, knn, params);
}
/**
* \brief Perform radius search
* \param[in] queries The query points
* \param[out] indices The indices of the neighbors found within the given radius
* \param[out] dists The distances to the nearest neighbors found
* \param[in] radius The radius used for search
* \param[in] params Search parameters
* \returns Number of neighbors found
*/
int radiusSearch(const Matrix<ElementType>& queries,
Matrix<size_t>& indices,
Matrix<DistanceType>& dists,
float radius,
const SearchParams& params) const
{
return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
}
/**
*
* @param queries
* @param indices
* @param dists
* @param radius
* @param params
* @return
*/
int radiusSearch(const Matrix<ElementType>& queries,
Matrix<int>& indices,
Matrix<DistanceType>& dists,
float radius,
const SearchParams& params) const
{
return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
}
/**
* \brief Perform radius search
* \param[in] queries The query points
* \param[out] indices The indices of the neighbors found within the given radius
* \param[out] dists The distances to the nearest neighbors found
* \param[in] radius The radius used for search
* \param[in] params Search parameters
* \returns Number of neighbors found
*/
int radiusSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<size_t> >& indices,
std::vector<std::vector<DistanceType> >& dists,
float radius,
const SearchParams& params) const
{
return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
}
/**
*
* @param queries
* @param indices
* @param dists
* @param radius
* @param params
* @return
*/
int radiusSearch(const Matrix<ElementType>& queries,
std::vector< std::vector<int> >& indices,
std::vector<std::vector<DistanceType> >& dists,
float radius,
const SearchParams& params) const
{
return nnIndex_->radiusSearch(queries, indices, dists, radius, params);
}
private:
IndexType* load_saved_index(const Matrix<ElementType>& dataset, const std::string& filename, Distance distance)
{
FILE* fin = fopen(filename.c_str(), "rb");
if (fin == NULL) {
return NULL;
}
IndexHeader header = load_header(fin);
if (header.h.data_type != flann_datatype_value<ElementType>::value) {
throw FLANNException("Datatype of saved index is different than of the one to be loaded.");
}
IndexParams params;
params["algorithm"] = header.h.index_type;
IndexType* nnIndex = create_index_by_type<Distance>(header.h.index_type, dataset, params, distance);
rewind(fin);
nnIndex->loadIndex(fin);
fclose(fin);
return nnIndex;
}
void swap( Index& other)
{
std::swap(nnIndex_, other.nnIndex_);
std::swap(loaded_, other.loaded_);
std::swap(index_params_, other.index_params_);
}
private:
/** Pointer to actual index class */
IndexType* nnIndex_;
/** Indices if the index was loaded from a file */
bool loaded_;
/** Parameters passed to the index */
IndexParams index_params_;
};
/**
* Performs a hierarchical clustering of the points passed as argument and then takes a cut in the
* the clustering tree to return a flat clustering.
* @param[in] points Points to be clustered
* @param centers The computed cluster centres. Matrix should be preallocated and centers.rows is the
* number of clusters requested.
* @param params Clustering parameters (The same as for flann::KMeansIndex)
* @param d Distance to be used for clustering (eg: flann::L2)
* @return number of clusters computed (can be different than clusters.rows and is the highest number
* of the form (branching-1)*K+1 smaller than clusters.rows).
*/
template <typename Distance>
int hierarchicalClustering(const Matrix<typename Distance::ElementType>& points, Matrix<typename Distance::ResultType>& centers,
const KMeansIndexParams& params, Distance d = Distance())
{
KMeansIndex<Distance> kmeans(points, params, d);
kmeans.buildIndex();
int clusterNum = kmeans.getClusterCenters(centers);
return clusterNum;
}
}
#endif /* RTABMAP_FLANN_HPP_ */

227
corelib/src/rtflann/general.h Normal file
View File

@@ -0,0 +1,227 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_GENERAL_H_
#define RTABMAP_FLANN_GENERAL_H_
#include "defines.h"
#include <stdexcept>
#include <cassert>
#include <limits.h>
namespace rtflann
{
class FLANNException : public std::runtime_error
{
public:
FLANNException(const char* message) : std::runtime_error(message) { }
FLANNException(const std::string& message) : std::runtime_error(message) { }
};
template <typename T>
struct flann_datatype_value
{
static const flann_datatype_t value = FLANN_NONE;
};
template<>
struct flann_datatype_value<char>
{
static const flann_datatype_t value = FLANN_INT8;
};
template<>
struct flann_datatype_value<short>
{
static const flann_datatype_t value = FLANN_INT16;
};
template<>
struct flann_datatype_value<int>
{
static const flann_datatype_t value = FLANN_INT32;
};
#ifdef LLONG_MAX
template<>
struct flann_datatype_value<long long>
{
static const flann_datatype_t value = FLANN_INT64;
};
#endif
template<>
struct flann_datatype_value<unsigned char>
{
static const flann_datatype_t value = FLANN_UINT8;
};
template<>
struct flann_datatype_value<unsigned short>
{
static const flann_datatype_t value = FLANN_UINT16;
};
template<>
struct flann_datatype_value<unsigned int>
{
static const flann_datatype_t value = FLANN_UINT32;
};
#ifdef ULLONG_MAX
template<>
struct flann_datatype_value<unsigned long long>
{
static const flann_datatype_t value = FLANN_UINT64;
};
#endif
template<>
struct flann_datatype_value<float>
{
static const flann_datatype_t value = FLANN_FLOAT32;
};
template<>
struct flann_datatype_value<double>
{
static const flann_datatype_t value = FLANN_FLOAT64;
};
template <flann_datatype_t datatype>
struct flann_datatype_type
{
typedef void type;
};
template<>
struct flann_datatype_type<FLANN_INT8>
{
typedef char type;
};
template<>
struct flann_datatype_type<FLANN_INT16>
{
typedef short type;
};
template<>
struct flann_datatype_type<FLANN_INT32>
{
typedef int type;
};
#ifdef LLONG_MAX
template<>
struct flann_datatype_type<FLANN_INT64>
{
typedef long long type;
};
#endif
template<>
struct flann_datatype_type<FLANN_UINT8>
{
typedef unsigned char type;
};
template<>
struct flann_datatype_type<FLANN_UINT16>
{
typedef unsigned short type;
};
template<>
struct flann_datatype_type<FLANN_UINT32>
{
typedef unsigned int type;
};
#ifdef ULLONG_MAX
template<>
struct flann_datatype_type<FLANN_UINT64>
{
typedef unsigned long long type;
};
#endif
template<>
struct flann_datatype_type<FLANN_FLOAT32>
{
typedef float type;
};
template<>
struct flann_datatype_type<FLANN_FLOAT64>
{
typedef double type;
};
inline size_t flann_datatype_size(flann_datatype_t type)
{
switch (type) {
case FLANN_INT8:
return sizeof(flann_datatype_type<FLANN_INT8>::type);
case FLANN_INT16:
return sizeof(flann_datatype_type<FLANN_INT16>::type);
case FLANN_INT32:
return sizeof(flann_datatype_type<FLANN_INT32>::type);
case FLANN_INT64:
return sizeof(flann_datatype_type<FLANN_INT64>::type);
case FLANN_UINT8:
return sizeof(flann_datatype_type<FLANN_UINT8>::type);
case FLANN_UINT16:
return sizeof(flann_datatype_type<FLANN_UINT16>::type);
case FLANN_UINT32:
return sizeof(flann_datatype_type<FLANN_UINT32>::type);
case FLANN_UINT64:
return sizeof(flann_datatype_type<FLANN_UINT64>::type);
case FLANN_FLOAT32:
return sizeof(flann_datatype_type<FLANN_FLOAT32>::type);
case FLANN_FLOAT64:
return sizeof(flann_datatype_type<FLANN_FLOAT64>::type);
default:
return 0;
}
}
}
#endif /* RTABMAP_FLANN_GENERAL_H_ */

98
corelib/src/rtflann/nn/ground_truth.h Normal file
View File

@@ -0,0 +1,98 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_GROUND_TRUTH_H_
#define RTABMAP_FLANN_GROUND_TRUTH_H_
#include "rtflann/algorithms/dist.h"
#include "rtflann/util/matrix.h"
namespace rtflann
{
template <typename Distance>
void find_nearest(const Matrix<typename Distance::ElementType>& dataset, typename Distance::ElementType* query, size_t* matches, size_t nn,
size_t skip = 0, Distance distance = Distance())
{
//typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
int n = nn + skip;
int* match = new int[n];
DistanceType* dists = new DistanceType[n];
dists[0] = distance(dataset[0], query, dataset.cols);
match[0] = 0;
int dcnt = 1;
for (size_t i=1; i<dataset.rows; ++i) {
DistanceType tmp = distance(dataset[i], query, dataset.cols);
if (dcnt<n) {
match[dcnt] = i;
dists[dcnt++] = tmp;
}
else if (tmp < dists[dcnt-1]) {
dists[dcnt-1] = tmp;
match[dcnt-1] = i;
}
int j = dcnt-1;
// bubble up
while (j>=1 && dists[j]<dists[j-1]) {
std::swap(dists[j],dists[j-1]);
std::swap(match[j],match[j-1]);
j--;
}
}
for (size_t i=0; i<nn; ++i) {
matches[i] = match[i+skip];
}
delete[] match;
delete[] dists;
}
template <typename Distance>
void compute_ground_truth(const Matrix<typename Distance::ElementType>& dataset, const Matrix<typename Distance::ElementType>& testset, Matrix<size_t>& matches,
int skip=0, Distance d = Distance())
{
for (size_t i=0; i<testset.rows; ++i) {
find_nearest<Distance>(dataset, testset[i], matches[i], matches.cols, skip, d);
}
}
}
#endif //FLANN_GROUND_TRUTH_H_

323
corelib/src/rtflann/nn/index_testing.h Normal file
View File

@@ -0,0 +1,323 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_INDEX_TESTING_H_
#define RTABMAP_FLANN_INDEX_TESTING_H_
#include <cstring>
#include <cassert>
#include <cmath>
#include "rtflann/util/matrix.h"
#include "rtflann/algorithms/nn_index.h"
#include "rtflann/util/result_set.h"
#include "rtflann/util/logger.h"
#include "rtflann/util/timer.h"
namespace rtflann
{
inline int countCorrectMatches(size_t* neighbors, size_t* groundTruth, int n)
{
int count = 0;
for (int i=0; i<n; ++i) {
for (int k=0; k<n; ++k) {
if (neighbors[i]==groundTruth[k]) {
count++;
break;
}
}
}
return count;
}
template <typename Distance>
typename Distance::ResultType computeDistanceRaport(const Matrix<typename Distance::ElementType>& inputData, typename Distance::ElementType* target,
size_t* neighbors, size_t* groundTruth, int veclen, int n, const Distance& distance)
{
typedef typename Distance::ResultType DistanceType;
DistanceType ret = 0;
for (int i=0; i<n; ++i) {
DistanceType den = distance(inputData[groundTruth[i]], target, veclen);
DistanceType num = distance(inputData[neighbors[i]], target, veclen);
if ((den==0)&&(num==0)) {
ret += 1;
}
else {
ret += num/den;
}
}
return ret;
}
template <typename Index, typename Distance>
float search_with_ground_truth(Index& index, const Matrix<typename Distance::ElementType>& inputData,
const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches, int nn, int checks,
float& time, typename Distance::ResultType& dist, const Distance& distance, int skipMatches)
{
typedef typename Distance::ElementType ElementType;
typedef typename Distance::ResultType DistanceType;
if (matches.cols<size_t(nn)) {
Logger::info("matches.cols=%d, nn=%d\n",matches.cols,nn);
throw FLANNException("Ground truth is not computed for as many neighbors as requested");
}
SearchParams searchParams(checks);
size_t* indices = new size_t[nn+skipMatches];
DistanceType* dists = new DistanceType[nn+skipMatches];
Matrix<size_t> indices_mat(indices, 1, nn+skipMatches);
Matrix<DistanceType> dists_mat(dists, 1, nn+skipMatches);
size_t* neighbors = indices + skipMatches;
int correct = 0;
DistanceType distR = 0;
StartStopTimer t;
int repeats = 0;
while (t.value<0.2) {
repeats++;
t.start();
correct = 0;
distR = 0;
for (size_t i = 0; i < testData.rows; i++) {
index.knnSearch(Matrix<ElementType>(testData[i], 1, testData.cols), indices_mat, dists_mat, nn+skipMatches, searchParams);
correct += countCorrectMatches(neighbors,matches[i], nn);
distR += computeDistanceRaport<Distance>(inputData, testData[i], neighbors, matches[i], testData.cols, nn, distance);
}
t.stop();
}
time = float(t.value/repeats);
delete[] indices;
delete[] dists;
float precicion = (float)correct/(nn*testData.rows);
dist = distR/(testData.rows*nn);
Logger::info("%8d %10.4g %10.5g %10.5g %10.5g\n",
checks, precicion, time, 1000.0 * time / testData.rows, dist);
return precicion;
}
template <typename Index, typename Distance>
float test_index_checks(Index& index, const Matrix<typename Distance::ElementType>& inputData,
const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches,
int checks, float& precision, const Distance& distance, int nn = 1, int skipMatches = 0)
{
typedef typename Distance::ResultType DistanceType;
Logger::info(" Nodes Precision(%) Time(s) Time/vec(ms) Mean dist\n");
Logger::info("---------------------------------------------------------\n");
float time = 0;
DistanceType dist = 0;
precision = search_with_ground_truth(index, inputData, testData, matches, nn, checks, time, dist, distance, skipMatches);
return time;
}
template <typename Index, typename Distance>
float test_index_precision(Index& index, const Matrix<typename Distance::ElementType>& inputData,
const Matrix<typename Distance::ElementType>& testData, const Matrix<size_t>& matches,
float precision, int& checks, const Distance& distance, int nn = 1, int skipMatches = 0)
{
typedef typename Distance::ResultType DistanceType;
const float SEARCH_EPS = 0.001f;
Logger::info(" Nodes Precision(%) Time(s) Time/vec(ms) Mean dist\n");
Logger::info("---------------------------------------------------------\n");
int c2 = 1;
float p2;
int c1 = 1;
// float p1;
float time;
DistanceType dist;
p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
if (p2>precision) {
Logger::info("Got as close as I can\n");
checks = c2;
return time;
}
while (p2<precision) {
c1 = c2;
// p1 = p2;
c2 *=2;
p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
}
int cx;
float realPrecision;
if (fabs(p2-precision)>SEARCH_EPS) {
Logger::info("Start linear estimation\n");
// after we got to values in the vecinity of the desired precision
// use linear approximation get a better estimation
cx = (c1+c2)/2;
realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
while (fabs(realPrecision-precision)>SEARCH_EPS) {
if (realPrecision<precision) {
c1 = cx;
}
else {
c2 = cx;
}
cx = (c1+c2)/2;
if (cx==c1) {
Logger::info("Got as close as I can\n");
break;
}
realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
}
c2 = cx;
p2 = realPrecision;
}
else {
Logger::info("No need for linear estimation\n");
cx = c2;
realPrecision = p2;
}
checks = cx;
return time;
}
template <typename Index, typename Distance>
void test_index_precisions(Index& index, const Matrix<typename Distance::ElementType>& inputData,
const Matrix<typename Distance::ElementType>& testData, const Matrix<int>& matches,
float* precisions, int precisions_length, const Distance& distance, int nn = 1, int skipMatches = 0, float maxTime = 0)
{
typedef typename Distance::ResultType DistanceType;
const float SEARCH_EPS = 0.001;
// make sure precisions array is sorted
std::sort(precisions, precisions+precisions_length);
int pindex = 0;
float precision = precisions[pindex];
Logger::info(" Nodes Precision(%) Time(s) Time/vec(ms) Mean dist\n");
Logger::info("---------------------------------------------------------\n");
int c2 = 1;
float p2;
int c1 = 1;
float p1;
float time;
DistanceType dist;
p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
// if precision for 1 run down the tree is already
// better then some of the requested precisions, then
// skip those
while (precisions[pindex]<p2 && pindex<precisions_length) {
pindex++;
}
if (pindex==precisions_length) {
Logger::info("Got as close as I can\n");
return;
}
for (int i=pindex; i<precisions_length; ++i) {
precision = precisions[i];
while (p2<precision) {
c1 = c2;
p1 = p2;
c2 *=2;
p2 = search_with_ground_truth(index, inputData, testData, matches, nn, c2, time, dist, distance, skipMatches);
if ((maxTime> 0)&&(time > maxTime)&&(p2<precision)) return;
}
int cx;
float realPrecision;
if (fabs(p2-precision)>SEARCH_EPS) {
Logger::info("Start linear estimation\n");
// after we got to values in the vecinity of the desired precision
// use linear approximation get a better estimation
cx = (c1+c2)/2;
realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
while (fabs(realPrecision-precision)>SEARCH_EPS) {
if (realPrecision<precision) {
c1 = cx;
}
else {
c2 = cx;
}
cx = (c1+c2)/2;
if (cx==c1) {
Logger::info("Got as close as I can\n");
break;
}
realPrecision = search_with_ground_truth(index, inputData, testData, matches, nn, cx, time, dist, distance, skipMatches);
}
c2 = cx;
p2 = realPrecision;
}
else {
Logger::info("No need for linear estimation\n");
cx = c2;
realPrecision = p2;
}
}
}
}
#endif //FLANN_INDEX_TESTING_H_

186
corelib/src/rtflann/nn/simplex_downhill.h Normal file
View File

@@ -0,0 +1,186 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_SIMPLEX_DOWNHILL_H_
#define RTABMAP_FLANN_SIMPLEX_DOWNHILL_H_
namespace rtflann
{
/**
Adds val to array vals (and point to array points) and keeping the arrays sorted by vals.
*/
template <typename T>
void addValue(int pos, float val, float* vals, T* point, T* points, int n)
{
vals[pos] = val;
for (int i=0; i<n; ++i) {
points[pos*n+i] = point[i];
}
// bubble down
int j=pos;
while (j>0 && vals[j]<vals[j-1]) {
swap(vals[j],vals[j-1]);
for (int i=0; i<n; ++i) {
swap(points[j*n+i],points[(j-1)*n+i]);
}
--j;
}
}
/**
Simplex downhill optimization function.
Preconditions: points is a 2D mattrix of size (n+1) x n
func is the cost function taking n an array of n params and returning float
vals is the cost function in the n+1 simplex points, if NULL it will be computed
Postcondition: returns optimum value and points[0..n] are the optimum parameters
*/
template <typename T, typename F>
float optimizeSimplexDownhill(T* points, int n, F func, float* vals = NULL )
{
const int MAX_ITERATIONS = 10;
assert(n>0);
T* p_o = new T[n];
T* p_r = new T[n];
T* p_e = new T[n];
int alpha = 1;
int iterations = 0;
bool ownVals = false;
if (vals == NULL) {
ownVals = true;
vals = new float[n+1];
for (int i=0; i<n+1; ++i) {
float val = func(points+i*n);
addValue(i, val, vals, points+i*n, points, n);
}
}
int nn = n*n;
while (true) {
if (iterations++ > MAX_ITERATIONS) break;
// compute average of simplex points (except the highest point)
for (int j=0; j<n; ++j) {
p_o[j] = 0;
for (int i=0; i<n; ++i) {
p_o[i] += points[j*n+i];
}
}
for (int i=0; i<n; ++i) {
p_o[i] /= n;
}
bool converged = true;
for (int i=0; i<n; ++i) {
if (p_o[i] != points[nn+i]) {
converged = false;
}
}
if (converged) break;
// trying a reflection
for (int i=0; i<n; ++i) {
p_r[i] = p_o[i] + alpha*(p_o[i]-points[nn+i]);
}
float val_r = func(p_r);
if ((val_r>=vals[0])&&(val_r<vals[n])) {
// reflection between second highest and lowest
// add it to the simplex
Logger::info("Choosing reflection\n");
addValue(n, val_r,vals, p_r, points, n);
continue;
}
if (val_r<vals[0]) {
// value is smaller than smalest in simplex
// expand some more to see if it drops further
for (int i=0; i<n; ++i) {
p_e[i] = 2*p_r[i]-p_o[i];
}
float val_e = func(p_e);
if (val_e<val_r) {
Logger::info("Choosing reflection and expansion\n");
addValue(n, val_e,vals,p_e,points,n);
}
else {
Logger::info("Choosing reflection\n");
addValue(n, val_r,vals,p_r,points,n);
}
continue;
}
if (val_r>=vals[n]) {
for (int i=0; i<n; ++i) {
p_e[i] = (p_o[i]+points[nn+i])/2;
}
float val_e = func(p_e);
if (val_e<vals[n]) {
Logger::info("Choosing contraction\n");
addValue(n,val_e,vals,p_e,points,n);
continue;
}
}
{
Logger::info("Full contraction\n");
for (int j=1; j<=n; ++j) {
for (int i=0; i<n; ++i) {
points[j*n+i] = (points[j*n+i]+points[i])/2;
}
float val = func(points+j*n);
addValue(j,val,vals,points+j*n,points,n);
}
}
}
float bestVal = vals[0];
delete[] p_r;
delete[] p_o;
delete[] p_e;
if (ownVals) delete[] vals;
return bestVal;
}
}
#endif //FLANN_SIMPLEX_DOWNHILL_H_

8
corelib/src/rtflann/readme.txt Normal file
View File

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

202
corelib/src/rtflann/util/allocator.h Normal file
View File

@@ -0,0 +1,202 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_ALLOCATOR_H_
#define RTABMAP_FLANN_ALLOCATOR_H_
#include <stdlib.h>
#include <stdio.h>
namespace rtflann
{
/**
* Allocates (using C's malloc) a generic type T.
*
* Params:
* count = number of instances to allocate.
* Returns: pointer (of type T*) to memory buffer
*/
template <typename T>
T* allocate(size_t count = 1)
{
T* mem = (T*) ::malloc(sizeof(T)*count);
return mem;
}
/**
* Pooled storage allocator
*
* The following routines allow for the efficient allocation of storage in
* small chunks from a specified pool. Rather than allowing each structure
* to be freed individually, an entire pool of storage is freed at once.
* This method has two advantages over just using malloc() and free(). First,
* it is far more efficient for allocating small objects, as there is
* no overhead for remembering all the information needed to free each
* object or consolidating fragmented memory. Second, the decision about
* how long to keep an object is made at the time of allocation, and there
* is no need to track down all the objects to free them.
*
*/
const size_t WORDSIZE=16;
const size_t BLOCKSIZE=8192;
class PooledAllocator
{
/* We maintain memory alignment to word boundaries by requiring that all
allocations be in multiples of the machine wordsize. */
/* Size of machine word in bytes. Must be power of 2. */
/* Minimum number of bytes requested at a time from the system. Must be multiple of WORDSIZE. */
int remaining; /* Number of bytes left in current block of storage. */
void* base; /* Pointer to base of current block of storage. */
void* loc; /* Current location in block to next allocate memory. */
int blocksize;
public:
int usedMemory;
int wastedMemory;
/**
Default constructor. Initializes a new pool.
*/
PooledAllocator(int blocksize = BLOCKSIZE)
{
this->blocksize = blocksize;
remaining = 0;
base = NULL;
usedMemory = 0;
wastedMemory = 0;
}
/**
* Destructor. Frees all the memory allocated in this pool.
*/
~PooledAllocator()
{
free();
}
void free()
{
void* prev;
while (base != NULL) {
prev = *((void**) base); /* Get pointer to prev block. */
::free(base);
base = prev;
}
base = NULL;
remaining = 0;
usedMemory = 0;
wastedMemory = 0;
}
/**
* Returns a pointer to a piece of new memory of the given size in bytes
* allocated from the pool.
*/
void* allocateMemory(int size)
{
int blocksize;
/* Round size up to a multiple of wordsize. The following expression
only works for WORDSIZE that is a power of 2, by masking last bits of
incremented size to zero.
*/
size = (size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
/* Check whether a new block must be allocated. Note that the first word
of a block is reserved for a pointer to the previous block.
*/
if (size > remaining) {
wastedMemory += remaining;
/* Allocate new storage. */
blocksize = (size + sizeof(void*) + (WORDSIZE-1) > BLOCKSIZE) ?
size + sizeof(void*) + (WORDSIZE-1) : BLOCKSIZE;
// use the standard C malloc to allocate memory
void* m = ::malloc(blocksize);
if (!m) {
fprintf(stderr,"Failed to allocate memory.\n");
return NULL;
}
/* Fill first word of new block with pointer to previous block. */
((void**) m)[0] = base;
base = m;
int shift = 0;
//int shift = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);
remaining = blocksize - sizeof(void*) - shift;
loc = ((char*)m + sizeof(void*) + shift);
}
void* rloc = loc;
loc = (char*)loc + size;
remaining -= size;
usedMemory += size;
return rloc;
}
/**
* Allocates (using this pool) a generic type T.
*
* Params:
* count = number of instances to allocate.
* Returns: pointer (of type T*) to memory buffer
*/
template <typename T>
T* allocate(size_t count = 1)
{
T* mem = (T*) this->allocateMemory((int)(sizeof(T)*count));
return mem;
}
};
}
inline void* operator new (std::size_t size, rtflann::PooledAllocator& allocator)
{
return allocator.allocateMemory(size) ;
}
#endif //FLANN_ALLOCATOR_H_

294
corelib/src/rtflann/util/any.h Normal file
View File

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

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

456
corelib/src/rtflann/util/heap.h Normal file
View File

@@ -0,0 +1,456 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_HEAP_H_
#define RTABMAP_FLANN_HEAP_H_
#include <algorithm>
#include <vector>
namespace rtflann
{
/**
* Priority Queue Implementation
*
* The priority queue is implemented with a heap. A heap is a complete
* (full) binary tree in which each parent is less than both of its
* children, but the order of the children is unspecified.
*/
template <typename T>
class Heap
{
/**
* Storage array for the heap.
* Type T must be comparable.
*/
std::vector<T> heap;
int length;
/**
* Number of element in the heap
*/
int count;
public:
/**
* Constructor.
*
* Params:
* size = heap size
*/
Heap(int size)
{
length = size;
heap.reserve(length);
count = 0;
}
/**
*
* Returns: heap size
*/
int size()
{
return count;
}
/**
* Tests if the heap is empty
*
* Returns: true is heap empty, false otherwise
*/
bool empty()
{
return size()==0;
}
/**
* Clears the heap.
*/
void clear()
{
heap.clear();
count = 0;
}
struct CompareT : public std::binary_function<T,T,bool>
{
bool operator()(const T& t_1, const T& t_2) const
{
return t_2 < t_1;
}
};
/**
* Insert a new element in the heap.
*
* We select the next empty leaf node, and then keep moving any larger
* parents down until the right location is found to store this element.
*
* Params:
* value = the new element to be inserted in the heap
*/
void insert(const T& value)
{
/* If heap is full, then return without adding this element. */
if (count == length) {
return;
}
heap.push_back(value);
static CompareT compareT;
std::push_heap(heap.begin(), heap.end(), compareT);
++count;
}
/**
* Returns the node of minimum value from the heap (top of the heap).
*
* Params:
* value = out parameter used to return the min element
* Returns: false if heap empty
*/
bool popMin(T& value)
{
if (count == 0) {
return false;
}
value = heap[0];
static CompareT compareT;
std::pop_heap(heap.begin(), heap.end(), compareT);
heap.pop_back();
--count;
return true; /* Return old last node. */
}
};
template <typename T>
class IntervalHeap
{
struct Interval
{
T left;
T right;
};
/**
* Storage array for the heap.
* Type T must be comparable.
*/
std::vector<Interval> heap;
size_t capacity_;
size_t size_;
public:
/**
* Constructor.
*
* Params:
* size = heap size
*/
IntervalHeap(int capacity) : capacity_(capacity), size_(0)
{
heap.resize(capacity/2 + capacity%2 + 1); // 1-based indexing
}
/**
* @return Heap size
*/
size_t size()
{
return size_;
}
/**
* Tests if the heap is empty
* @return true is heap empty, false otherwise
*/
bool empty()
{
return size_==0;
}
/**
* Clears the heap.
*/
void clear()
{
size_ = 0;
}
void insert(const T& value)
{
/* If heap is full, then return without adding this element. */
if (size_ == capacity_) {
return;
}
// insert into the root
if (size_<2) {
if (size_==0) {
heap[1].left = value;
heap[1].right = value;
}
else {
if (value<heap[1].left) {
heap[1].left = value;
}
else {
heap[1].right = value;
}
}
++size_;
return;
}
size_t last_pos = size_/2 + size_%2;
bool min_heap;
if (size_%2) { // odd number of elements
min_heap = (value<heap[last_pos].left)? true : false;
}
else {
++last_pos;
min_heap = (value<heap[last_pos/2].left)? true : false;
}
if (min_heap) {
size_t pos = last_pos;
size_t par = pos/2;
while (pos>1 && value < heap[par].left) {
heap[pos].left = heap[par].left;
pos = par;
par = pos/2;
}
heap[pos].left = value;
++size_;
if (size_%2) { // duplicate element in last position if size is odd
heap[last_pos].right = heap[last_pos].left;
}
}
else {
size_t pos = last_pos;
size_t par = pos/2;
while (pos>1 && heap[par].right < value) {
heap[pos].right = heap[par].right;
pos = par;
par = pos/2;
}
heap[pos].right = value;
++size_;
if (size_%2) { // duplicate element in last position if size is odd
heap[last_pos].left = heap[last_pos].right;
}
}
}
/**
* Returns the node of minimum value from the heap
* @param value out parameter used to return the min element
* @return false if heap empty
*/
bool popMin(T& value)
{
if (size_ == 0) {
return false;
}
value = heap[1].left;
size_t last_pos = size_/2 + size_%2;
T elem = heap[last_pos].left;
if (size_ % 2) { // odd number of elements
--last_pos;
}
else {
heap[last_pos].left = heap[last_pos].right;
}
--size_;
if (size_<2) return true;
size_t crt=1; // root node
size_t child = crt*2;
while (child <= last_pos) {
if (child < last_pos && heap[child+1].left < heap[child].left) ++child; // pick the child with min
if (!(heap[child].left<elem)) break;
heap[crt].left = heap[child].left;
if (heap[child].right<elem) {
std::swap(elem, heap[child].right);
}
crt = child;
child *= 2;
}
heap[crt].left = elem;
return true;
}
/**
* Returns the element of maximum value from the heap
* @param value
* @return false if heap empty
*/
bool popMax(T& value)
{
if (size_ == 0) {
return false;
}
value = heap[1].right;
size_t last_pos = size_/2 + size_%2;
T elem = heap[last_pos].right;
if (size_%2) { // odd number of elements
--last_pos;
}
else {
heap[last_pos].right = heap[last_pos].left;
}
--size_;
if (size_<2) return true;
size_t crt=1; // root node
size_t child = crt*2;
while (child <= last_pos) {
if (child < last_pos && heap[child].right < heap[child+1].right) ++child; // pick the child with max
if (!(elem < heap[child].right)) break;
heap[crt].right = heap[child].right;
if (elem<heap[child].left) {
std::swap(elem, heap[child].left);
}
crt = child;
child *= 2;
}
heap[crt].right = elem;
return true;
}
bool getMin(T& value)
{
if (size_==0) {
return false;
}
value = heap[1].left;
return true;
}
bool getMax(T& value)
{
if (size_==0) {
return false;
}
value = heap[1].right;
return true;
}
};
template <typename T>
class BoundedHeap
{
IntervalHeap<T> interval_heap_;
size_t capacity_;
public:
BoundedHeap(size_t capacity) : interval_heap_(capacity), capacity_(capacity)
{
}
/**
* Returns: heap size
*/
int size()
{
return interval_heap_.size();
}
/**
* Tests if the heap is empty
* Returns: true is heap empty, false otherwise
*/
bool empty()
{
return interval_heap_.empty();
}
/**
* Clears the heap.
*/
void clear()
{
interval_heap_.clear();
}
void insert(const T& value)
{
if (interval_heap_.size()==capacity_) {
T max;
interval_heap_.getMax(max);
if (max<value) return;
interval_heap_.popMax(max);
}
interval_heap_.insert(value);
}
bool popMin(T& value)
{
return interval_heap_.popMin(value);
}
};
}
#endif //FLANN_HEAP_H_

137
corelib/src/rtflann/util/logger.h Normal file
View File

@@ -0,0 +1,137 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_LOGGER_H
#define RTABMAP_FLANN_LOGGER_H
#include <stdio.h>
#include <stdarg.h>
#include "rtflann/defines.h"
namespace rtflann
{
class Logger
{
Logger() : stream(stdout), logLevel(FLANN_LOG_WARN) {}
~Logger()
{
if ((stream!=NULL)&&(stream!=stdout)) {
fclose(stream);
}
}
static Logger& instance()
{
static Logger logger;
return logger;
}
void _setDestination(const char* name)
{
if (name==NULL) {
stream = stdout;
}
else {
stream = fopen(name,"w");
if (stream == NULL) {
stream = stdout;
}
}
}
int _log(int level, const char* fmt, va_list arglist)
{
if (level > logLevel ) return -1;
int ret = vfprintf(stream, fmt, arglist);
return ret;
}
public:
/**
* Sets the logging level. All messages with lower priority will be ignored.
* @param level Logging level
*/
static void setLevel(int level) { instance().logLevel = level; }
/**
* Returns the currently set logging level.
* @return current logging level
*/
static int getLevel() { return instance().logLevel; }
/**
* Sets the logging destination
* @param name Filename or NULL for console
*/
static void setDestination(const char* name) { instance()._setDestination(name); }
/**
* Print log message
* @param level Log level
* @param fmt Message format
* @return
*/
static int log(int level, const char* fmt, ...)
{
va_list arglist;
va_start(arglist, fmt);
int ret = instance()._log(level,fmt,arglist);
va_end(arglist);
return ret;
}
#define LOG_METHOD(NAME,LEVEL) \
static int NAME(const char* fmt, ...) \
{ \
va_list ap; \
va_start(ap, fmt); \
int ret = instance()._log(LEVEL, fmt, ap); \
va_end(ap); \
return ret; \
}
LOG_METHOD(fatal, FLANN_LOG_FATAL)
LOG_METHOD(error, FLANN_LOG_ERROR)
LOG_METHOD(warn, FLANN_LOG_WARN)
LOG_METHOD(info, FLANN_LOG_INFO)
LOG_METHOD(debug, FLANN_LOG_DEBUG)
private:
FILE* stream;
int logLevel;
};
}
#endif //RTABMAP_FLANN_LOGGER_H

506
corelib/src/rtflann/util/lsh_table.h Normal file
View File

@@ -0,0 +1,506 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
/***********************************************************************
* Author: Vincent Rabaud
*************************************************************************/
#ifndef RTABMAP_FLANN_LSH_TABLE_H_
#define RTABMAP_FLANN_LSH_TABLE_H_
#include <algorithm>
#include <iostream>
#include <iomanip>
#include <limits.h>
// TODO as soon as we use C++0x, use the code in USE_UNORDERED_MAP
#if USE_UNORDERED_MAP
#include <unordered_map>
#else
#include <map>
#endif
#include <math.h>
#include <stddef.h>
#include "rtflann/util/dynamic_bitset.h"
#include "rtflann/util/matrix.h"
namespace rtflann
{
namespace lsh
{
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** What is stored in an LSH bucket
*/
typedef uint32_t FeatureIndex;
/** The id from which we can get a bucket back in an LSH table
*/
typedef unsigned int BucketKey;
/** A bucket in an LSH table
*/
typedef std::vector<FeatureIndex> Bucket;
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** POD for stats about an LSH table
*/
struct LshStats
{
std::vector<unsigned int> bucket_sizes_;
size_t n_buckets_;
size_t bucket_size_mean_;
size_t bucket_size_median_;
size_t bucket_size_min_;
size_t bucket_size_max_;
size_t bucket_size_std_dev;
/** Each contained vector contains three value: beginning/end for interval, number of elements in the bin
*/
std::vector<std::vector<unsigned int> > size_histogram_;
};
/** Overload the << operator for LshStats
* @param out the streams
* @param stats the stats to display
* @return the streams
*/
inline std::ostream& operator <<(std::ostream& out, const LshStats& stats)
{
size_t w = 20;
out << "Lsh Table Stats:\n" << std::setw(w) << std::setiosflags(std::ios::right) << "N buckets : "
<< stats.n_buckets_ << "\n" << std::setw(w) << std::setiosflags(std::ios::right) << "mean size : "
<< std::setiosflags(std::ios::left) << stats.bucket_size_mean_ << "\n" << std::setw(w)
<< std::setiosflags(std::ios::right) << "median size : " << stats.bucket_size_median_ << "\n" << std::setw(w)
<< std::setiosflags(std::ios::right) << "min size : " << std::setiosflags(std::ios::left)
<< stats.bucket_size_min_ << "\n" << std::setw(w) << std::setiosflags(std::ios::right) << "max size : "
<< std::setiosflags(std::ios::left) << stats.bucket_size_max_;
// Display the histogram
out << std::endl << std::setw(w) << std::setiosflags(std::ios::right) << "histogram : "
<< std::setiosflags(std::ios::left);
for (std::vector<std::vector<unsigned int> >::const_iterator iterator = stats.size_histogram_.begin(), end =
stats.size_histogram_.end(); iterator != end; ++iterator) out << (*iterator)[0] << "-" << (*iterator)[1] << ": " << (*iterator)[2] << ", ";
return out;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** Lsh hash table. As its key is a sub-feature, and as usually
* the size of it is pretty small, we keep it as a continuous memory array.
* The value is an index in the corpus of features (we keep it as an unsigned
* int for pure memory reasons, it could be a size_t)
*/
template<typename ElementType>
class LshTable
{
public:
/** A container of all the feature indices. Optimized for space
*/
#if USE_UNORDERED_MAP
typedef std::unordered_map<BucketKey, Bucket> BucketsSpace;
#else
typedef std::map<BucketKey, Bucket> BucketsSpace;
#endif
/** A container of all the feature indices. Optimized for speed
*/
typedef std::vector<Bucket> BucketsSpeed;
/** Default constructor
*/
LshTable()
{
}
/** Default constructor
* Create the mask and allocate the memory
* @param feature_size is the size of the feature (considered as a ElementType[])
* @param key_size is the number of bits that are turned on in the feature
*/
LshTable(unsigned int /*feature_size*/, unsigned int /*key_size*/)
{
std::cerr << "LSH is not implemented for that type" << std::endl;
throw;
}
/** Add a feature to the table
* @param value the value to store for that feature
* @param feature the feature itself
*/
void add(unsigned int value, const ElementType* feature)
{
// Add the value to the corresponding bucket
BucketKey key = getKey(feature);
switch (speed_level_) {
case kArray:
// That means we get the buckets from an array
buckets_speed_[key].push_back(value);
break;
case kBitsetHash:
// That means we can check the bitset for the presence of a key
key_bitset_.set(key);
buckets_space_[key].push_back(value);
break;
case kHash:
{
// That means we have to check for the hash table for the presence of a key
buckets_space_[key].push_back(value);
break;
}
}
}
/** Add a set of features to the table
* @param dataset the values to store
*/
void add(const std::vector< std::pair<size_t, ElementType*> >& features)
{
#if USE_UNORDERED_MAP
buckets_space_.rehash((buckets_space_.size() + features.size()) * 1.2);
#endif
// Add the features to the table
for (size_t i = 0; i < features.size(); ++i) {
add(features[i].first, features[i].second);
}
// Now that the table is full, optimize it for speed/space
optimize();
}
/** Get a bucket given the key
* @param key
* @return
*/
inline const Bucket* getBucketFromKey(BucketKey key) const
{
// Generate other buckets
switch (speed_level_) {
case kArray:
// That means we get the buckets from an array
return &buckets_speed_[key];
break;
case kBitsetHash:
// That means we can check the bitset for the presence of a key
if (key_bitset_.test(key)) return &buckets_space_.find(key)->second;
else return 0;
break;
case kHash:
{
// That means we have to check for the hash table for the presence of a key
BucketsSpace::const_iterator bucket_it, bucket_end = buckets_space_.end();
bucket_it = buckets_space_.find(key);
// Stop here if that bucket does not exist
if (bucket_it == bucket_end) return 0;
else return &bucket_it->second;
break;
}
}
return 0;
}
/** Compute the sub-signature of a feature
*/
size_t getKey(const ElementType* /*feature*/) const
{
std::cerr << "LSH is not implemented for that type" << std::endl;
throw;
return 1;
}
/** Get statistics about the table
* @return
*/
LshStats getStats() const;
private:
/** defines the speed fo the implementation
* kArray uses a vector for storing data
* kBitsetHash uses a hash map but checks for the validity of a key with a bitset
* kHash uses a hash map only
*/
enum SpeedLevel
{
kArray, kBitsetHash, kHash
};
/** Initialize some variables
*/
void initialize(size_t key_size)
{
speed_level_ = kHash;
key_size_ = key_size;
}
/** Optimize the table for speed/space
*/
void optimize()
{
// If we are already using the fast storage, no need to do anything
if (speed_level_ == kArray) return;
// Use an array if it will be more than half full
if (buckets_space_.size() > ((size_t(1) << key_size_) / 2)) {
speed_level_ = kArray;
// Fill the array version of it
buckets_speed_.resize(size_t(1) << key_size_);
for (BucketsSpace::const_iterator key_bucket = buckets_space_.begin(); key_bucket != buckets_space_.end(); ++key_bucket) buckets_speed_[key_bucket->first] = key_bucket->second;
// Empty the hash table
buckets_space_.clear();
return;
}
// If the bitset is going to use less than 10% of the RAM of the hash map (at least 1 size_t for the key and two
// for the vector) or less than 512MB (key_size_ <= 30)
if (((std::max(buckets_space_.size(), buckets_speed_.size()) * CHAR_BIT * 3 * sizeof(BucketKey)) / 10
>= size_t(size_t(1) << key_size_)) || (key_size_ <= 32)) {
speed_level_ = kBitsetHash;
key_bitset_.resize(size_t(1) << key_size_);
key_bitset_.reset();
// Try with the BucketsSpace
for (BucketsSpace::const_iterator key_bucket = buckets_space_.begin(); key_bucket != buckets_space_.end(); ++key_bucket) key_bitset_.set(key_bucket->first);
}
else {
speed_level_ = kHash;
key_bitset_.clear();
}
}
template<typename Archive>
void serialize(Archive& ar)
{
int val;
if (Archive::is_saving::value) {
val = (int)speed_level_;
}
ar & val;
if (Archive::is_loading::value) {
speed_level_ = (SpeedLevel) val;
}
ar & key_size_;
ar & mask_;
if (speed_level_==kArray) {
ar & buckets_speed_;
}
if (speed_level_==kBitsetHash || speed_level_==kHash) {
ar & buckets_space_;
}
if (speed_level_==kBitsetHash) {
ar & key_bitset_;
}
}
friend struct serialization::access;
/** The vector of all the buckets if they are held for speed
*/
BucketsSpeed buckets_speed_;
/** The hash table of all the buckets in case we cannot use the speed version
*/
BucketsSpace buckets_space_;
/** What is used to store the data */
SpeedLevel speed_level_;
/** If the subkey is small enough, it will keep track of which subkeys are set through that bitset
* That is just a speedup so that we don't look in the hash table (which can be mush slower that checking a bitset)
*/
DynamicBitset key_bitset_;
/** The size of the sub-signature in bits
*/
unsigned int key_size_;
// Members only used for the unsigned char specialization
/** The mask to apply to a feature to get the hash key
* Only used in the unsigned char case
*/
std::vector<size_t> mask_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Specialization for unsigned char
template<>
inline LshTable<unsigned char>::LshTable(unsigned int feature_size, unsigned int subsignature_size)
{
initialize(subsignature_size);
// Allocate the mask
mask_ = std::vector<size_t>((size_t)ceil((float)(feature_size * sizeof(char)) / (float)sizeof(size_t)), 0);
// A bit brutal but fast to code
std::vector<size_t> indices(feature_size * CHAR_BIT);
for (size_t i = 0; i < feature_size * CHAR_BIT; ++i) indices[i] = i;
std::random_shuffle(indices.begin(), indices.end());
// Generate a random set of order of subsignature_size_ bits
for (unsigned int i = 0; i < key_size_; ++i) {
size_t index = indices[i];
// Set that bit in the mask
size_t divisor = CHAR_BIT * sizeof(size_t);
size_t idx = index / divisor; //pick the right size_t index
mask_[idx] |= size_t(1) << (index % divisor); //use modulo to find the bit offset
}
// Set to 1 if you want to display the mask for debug
#if 0
{
size_t bcount = 0;
BOOST_FOREACH(size_t mask_block, mask_){
out << std::setw(sizeof(size_t) * CHAR_BIT / 4) << std::setfill('0') << std::hex << mask_block
<< std::endl;
bcount += __builtin_popcountll(mask_block);
}
out << "bit count : " << std::dec << bcount << std::endl;
out << "mask size : " << mask_.size() << std::endl;
return out;
}
#endif
}
/** Return the Subsignature of a feature
* @param feature the feature to analyze
*/
template<>
inline size_t LshTable<unsigned char>::getKey(const unsigned char* feature) const
{
// no need to check if T is dividable by sizeof(size_t) like in the Hamming
// distance computation as we have a mask
const size_t* feature_block_ptr = reinterpret_cast<const size_t*> (feature);
// Figure out the subsignature of the feature
// Given the feature ABCDEF, and the mask 001011, the output will be
// 000CEF
size_t subsignature = 0;
size_t bit_index = 1;
for (std::vector<size_t>::const_iterator pmask_block = mask_.begin(); pmask_block != mask_.end(); ++pmask_block) {
// get the mask and signature blocks
size_t feature_block = *feature_block_ptr;
size_t mask_block = *pmask_block;
while (mask_block) {
// Get the lowest set bit in the mask block
size_t lowest_bit = mask_block & (-(ptrdiff_t)mask_block);
// Add it to the current subsignature if necessary
subsignature += (feature_block & lowest_bit) ? bit_index : 0;
// Reset the bit in the mask block
mask_block ^= lowest_bit;
// increment the bit index for the subsignature
bit_index <<= 1;
}
// Check the next feature block
++feature_block_ptr;
}
return subsignature;
}
template<>
inline LshStats LshTable<unsigned char>::getStats() const
{
LshStats stats;
stats.bucket_size_mean_ = 0;
if ((buckets_speed_.empty()) && (buckets_space_.empty())) {
stats.n_buckets_ = 0;
stats.bucket_size_median_ = 0;
stats.bucket_size_min_ = 0;
stats.bucket_size_max_ = 0;
return stats;
}
if (!buckets_speed_.empty()) {
for (BucketsSpeed::const_iterator pbucket = buckets_speed_.begin(); pbucket != buckets_speed_.end(); ++pbucket) {
stats.bucket_sizes_.push_back(pbucket->size());
stats.bucket_size_mean_ += pbucket->size();
}
stats.bucket_size_mean_ /= buckets_speed_.size();
stats.n_buckets_ = buckets_speed_.size();
}
else {
for (BucketsSpace::const_iterator x = buckets_space_.begin(); x != buckets_space_.end(); ++x) {
stats.bucket_sizes_.push_back(x->second.size());
stats.bucket_size_mean_ += x->second.size();
}
stats.bucket_size_mean_ /= buckets_space_.size();
stats.n_buckets_ = buckets_space_.size();
}
std::sort(stats.bucket_sizes_.begin(), stats.bucket_sizes_.end());
// BOOST_FOREACH(int size, stats.bucket_sizes_)
// std::cout << size << " ";
// std::cout << std::endl;
stats.bucket_size_median_ = stats.bucket_sizes_[stats.bucket_sizes_.size() / 2];
stats.bucket_size_min_ = stats.bucket_sizes_.front();
stats.bucket_size_max_ = stats.bucket_sizes_.back();
// TODO compute mean and std
/*float mean, stddev;
stats.bucket_size_mean_ = mean;
stats.bucket_size_std_dev = stddev;*/
// Include a histogram of the buckets
unsigned int bin_start = 0;
unsigned int bin_end = 20;
bool is_new_bin = true;
for (std::vector<unsigned int>::iterator iterator = stats.bucket_sizes_.begin(), end = stats.bucket_sizes_.end(); iterator
!= end; )
if (*iterator < bin_end) {
if (is_new_bin) {
stats.size_histogram_.push_back(std::vector<unsigned int>(3, 0));
stats.size_histogram_.back()[0] = bin_start;
stats.size_histogram_.back()[1] = bin_end - 1;
is_new_bin = false;
}
++stats.size_histogram_.back()[2];
++iterator;
}
else {
bin_start += 20;
bin_end += 20;
is_new_bin = true;
}
return stats;
}
// End the two namespaces
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#endif /* FLANN_LSH_TABLE_H_ */

135
corelib/src/rtflann/util/matrix.h Normal file
View File

@@ -0,0 +1,135 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_DATASET_H_
#define RTABMAP_FLANN_DATASET_H_
#include "rtflann/general.h"
#include "rtflann/util/serialization.h"
#include <stdio.h>
namespace rtflann
{
typedef unsigned char uchar;
class Matrix_
{
public:
Matrix_() : rows(0), cols(0), stride(0), type(FLANN_NONE), data(NULL)
{
};
Matrix_(void* data_, size_t rows_, size_t cols_, flann_datatype_t type_, size_t stride_ = 0) :
rows(rows_), cols(cols_), stride(stride_), type(type_)
{
data = static_cast<uchar*>(data_);
if (stride==0) stride = flann_datatype_size(type)*cols;
}
/**
* Operator that returns a (pointer to a) row of the data.
*/
inline void* operator[](size_t index) const
{
return data+index*stride;
}
void* ptr() const
{
return data;
}
size_t rows;
size_t cols;
size_t stride;
flann_datatype_t type;
protected:
uchar* data;
template<typename Archive>
void serialize(Archive& ar)
{
ar & rows;
ar & cols;
ar & stride;
ar & type;
if (Archive::is_loading::value) {
data = new uchar[rows*stride];
}
ar & serialization::make_binary_object(data, rows*stride);
}
friend struct serialization::access;
};
/**
* Class that implements a simple rectangular matrix stored in a memory buffer and
* provides convenient matrix-like access using the [] operators.
*
* This class has the same memory structure as the un-templated class flann::Matrix_ and
* it's directly convertible from it.
*/
template <typename T>
class Matrix : public Matrix_
{
public:
typedef T type;
Matrix() : Matrix_()
{
}
Matrix(T* data_, size_t rows_, size_t cols_, size_t stride_ = 0) :
Matrix_(data_, rows_, cols_, flann_datatype_value<T>::value, stride_)
{
if (stride==0) stride = sizeof(T)*cols;
}
/**
* Operator that returns a (pointer to a) row of the data.
*/
inline T* operator[](size_t index) const
{
return reinterpret_cast<T*>(data+index*stride);
}
T* ptr() const
{
return reinterpret_cast<T*>(data);
}
};
}
#endif //FLANN_DATASET_H_

91
corelib/src/rtflann/util/object_factory.h Normal file
View File

@@ -0,0 +1,91 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_OBJECT_FACTORY_H_
#define RTABMAP_FLANN_OBJECT_FACTORY_H_
#include <map>
namespace rtflann
{
class CreatorNotFound
{
};
template<typename BaseClass,
typename UniqueIdType,
typename ObjectCreator = BaseClass* (*)()>
class ObjectFactory
{
typedef ObjectFactory<BaseClass,UniqueIdType,ObjectCreator> ThisClass;
typedef std::map<UniqueIdType, ObjectCreator> ObjectRegistry;
// singleton class, private constructor
ObjectFactory() {}
public:
bool subscribe(UniqueIdType id, ObjectCreator creator)
{
if (object_registry.find(id) != object_registry.end()) return false;
object_registry[id] = creator;
return true;
}
bool unregister(UniqueIdType id)
{
return object_registry.erase(id) == 1;
}
ObjectCreator create(UniqueIdType id)
{
typename ObjectRegistry::const_iterator iter = object_registry.find(id);
if (iter == object_registry.end()) {
throw CreatorNotFound();
}
return iter->second;
}
static ThisClass& instance()
{
static ThisClass the_factory;
return the_factory;
}
private:
ObjectRegistry object_registry;
};
}
#endif /* FLANN_OBJECT_FACTORY_H_ */

139
corelib/src/rtflann/util/params.h Normal file
View File

@@ -0,0 +1,139 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2011 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2011 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_PARAMS_H_
#define RTABMAP_FLANN_PARAMS_H_
#include "rtflann/util/any.h"
#include "rtflann/general.h"
#include <iostream>
#include <map>
namespace rtflann
{
namespace anyimpl
{
SMALL_POLICY(flann_algorithm_t);
SMALL_POLICY(flann_centers_init_t);
SMALL_POLICY(flann_log_level_t);
SMALL_POLICY(flann_datatype_t);
}
typedef std::map<std::string, any> IndexParams;
typedef enum {
FLANN_False = 0,
FLANN_True = 1,
FLANN_Undefined
} tri_type;
struct SearchParams
{
SearchParams(int checks_ = 32, float eps_ = 0.0, bool sorted_ = true ) :
checks(checks_), eps(eps_), sorted(sorted_)
{
max_neighbors = -1;
use_heap = FLANN_Undefined;
cores = 1;
matrices_in_gpu_ram = false;
}
// how many leafs to visit when searching for neighbours (-1 for unlimited)
int checks;
// search for eps-approximate neighbours (default: 0)
float eps;
// only for radius search, require neighbours sorted by distance (default: true)
bool sorted;
// maximum number of neighbors radius search should return (-1 for unlimited)
int max_neighbors;
// use a heap to manage the result set (default: FLANN_Undefined)
tri_type use_heap;
// how many cores to assign to the search (used only if compiled with OpenMP capable compiler) (0 for auto)
int cores;
// for GPU search indicates if matrices are already in GPU ram
bool matrices_in_gpu_ram;
};
inline bool has_param(const IndexParams& params, std::string name)
{
return params.find(name)!=params.end();
}
template<typename T>
T get_param(const IndexParams& params, std::string name, const T& default_value)
{
IndexParams::const_iterator it = params.find(name);
if (it != params.end()) {
return it->second.cast<T>();
}
else {
return default_value;
}
}
template<typename T>
T get_param(const IndexParams& params, std::string name)
{
IndexParams::const_iterator it = params.find(name);
if (it != params.end()) {
return it->second.cast<T>();
}
else {
throw FLANNException(std::string("Missing parameter '")+name+std::string("' in the parameters given"));
}
}
inline void print_params(const IndexParams& params)
{
IndexParams::const_iterator it;
for(it=params.begin(); it!=params.end(); ++it) {
std::cout << it->first << " : " << it->second << std::endl;
}
}
inline void print_params(const SearchParams& params)
{
std::cout << "checks : " << params.checks << std::endl;
std::cout << "eps : " << params.eps << std::endl;
std::cout << "sorted : " << params.sorted << std::endl;
std::cout << "max_neighbors : " << params.max_neighbors << std::endl;
}
}
#endif /* FLANN_PARAMS_H_ */

145
corelib/src/rtflann/util/random.h Normal file
View File

@@ -0,0 +1,145 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_RANDOM_H
#define RTABMAP_FLANN_RANDOM_H
#include <algorithm>
#include <cstdlib>
#include <cstddef>
#include <vector>
#include "rtflann/general.h"
namespace rtflann
{
/**
* Seeds the random number generator
* @param seed Random seed
*/
inline void seed_random(unsigned int seed)
{
srand(seed);
}
/*
* Generates a random double value.
*/
/**
* Generates a random double value.
* @param high Upper limit
* @param low Lower limit
* @return Random double value
*/
inline double rand_double(double high = 1.0, double low = 0)
{
return low + ((high-low) * (std::rand() / (RAND_MAX + 1.0)));
}
/**
* Generates a random integer value.
* @param high Upper limit
* @param low Lower limit
* @return Random integer value
*/
inline int rand_int(int high = RAND_MAX, int low = 0)
{
return low + (int) ( double(high-low) * (std::rand() / (RAND_MAX + 1.0)));
}
class RandomGenerator
{
public:
ptrdiff_t operator() (ptrdiff_t i) { return rand_int(i); }
};
/**
* Random number generator that returns a distinct number from
* the [0,n) interval each time.
*/
class UniqueRandom
{
std::vector<int> vals_;
int size_;
int counter_;
public:
/**
* Constructor.
* @param n Size of the interval from which to generate
* @return
*/
UniqueRandom(int n)
{
init(n);
}
/**
* Initializes the number generator.
* @param n the size of the interval from which to generate random numbers.
*/
void init(int n)
{
static RandomGenerator generator;
// create and initialize an array of size n
vals_.resize(n);
size_ = n;
for (int i = 0; i < size_; ++i) vals_[i] = i;
// shuffle the elements in the array
std::random_shuffle(vals_.begin(), vals_.end(), generator);
counter_ = 0;
}
/**
* Return a distinct random integer in greater or equal to 0 and less
* than 'n' on each call. It should be called maximum 'n' times.
* Returns: a random integer
*/
int next()
{
if (counter_ == size_) {
return -1;
}
else {
return vals_[counter_++];
}
}
};
}
#endif //FLANN_RANDOM_H

934
corelib/src/rtflann/util/result_set.h Normal file
View File

@@ -0,0 +1,934 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_RESULTSET_H
#define RTABMAP_FLANN_RESULTSET_H
#include <algorithm>
#include <cstring>
#include <iostream>
#include <limits>
#include <set>
#include <vector>
namespace rtflann
{
/* This record represents a branch point when finding neighbors in
the tree. It contains a record of the minimum distance to the query
point, as well as the node at which the search resumes.
*/
template <typename T, typename DistanceType>
struct BranchStruct
{
T node; /* Tree node at which search resumes */
DistanceType mindist; /* Minimum distance to query for all nodes below. */
BranchStruct() {}
BranchStruct(const T& aNode, DistanceType dist) : node(aNode), mindist(dist) {}
bool operator<(const BranchStruct<T, DistanceType>& rhs) const
{
return mindist<rhs.mindist;
}
};
template <typename DistanceType>
struct DistanceIndex
{
DistanceIndex(DistanceType dist, size_t index) :
dist_(dist), index_(index)
{
}
bool operator<(const DistanceIndex& dist_index) const
{
return (dist_ < dist_index.dist_) || ((dist_ == dist_index.dist_) && index_ < dist_index.index_);
}
DistanceType dist_;
size_t index_;
};
template <typename DistanceType>
class ResultSet
{
public:
virtual ~ResultSet() {}
virtual bool full() const = 0;
virtual void addPoint(DistanceType dist, size_t index) = 0;
virtual DistanceType worstDist() const = 0;
};
/**
* KNNSimpleResultSet does not ensure that the element it holds are unique.
* Is used in those cases where the nearest neighbour algorithm used does not
* attempt to insert the same element multiple times.
*/
template <typename DistanceType>
class KNNSimpleResultSet : public ResultSet<DistanceType>
{
public:
typedef DistanceIndex<DistanceType> DistIndex;
KNNSimpleResultSet(size_t capacity_) :
capacity_(capacity_)
{
// reserving capacity to prevent memory re-allocations
dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),-1));
clear();
}
~KNNSimpleResultSet()
{
}
/**
* Clears the result set
*/
void clear()
{
worst_distance_ = std::numeric_limits<DistanceType>::max();
dist_index_[capacity_-1].dist_ = worst_distance_;
count_ = 0;
}
/**
*
* @return Number of elements in the result set
*/
size_t size() const
{
return count_;
}
/**
* Radius search result set always reports full
* @return
*/
bool full() const
{
return count_==capacity_;
}
/**
* Add a point to result set
* @param dist distance to point
* @param index index of point
*/
void addPoint(DistanceType dist, size_t index)
{
if (dist>=worst_distance_) return;
if (count_ < capacity_) ++count_;
size_t i;
for (i=count_-1; i>0; --i) {
#ifdef FLANN_FIRST_MATCH
if ( (dist_index_[i-1].dist_>dist) || ((dist==dist_index_[i-1].dist_)&&(dist_index_[i-1].index_>index)) )
#else
if (dist_index_[i-1].dist_>dist)
#endif
{
dist_index_[i] = dist_index_[i-1];
}
else break;
}
dist_index_[i].dist_ = dist;
dist_index_[i].index_ = index;
worst_distance_ = dist_index_[capacity_-1].dist_;
}
/**
* Copy indices and distances to output buffers
* @param indices
* @param dists
* @param num_elements Number of elements to copy
* @param sorted Indicates if results should be sorted
*/
void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
{
size_t n = std::min(count_, num_elements);
for (size_t i=0; i<n; ++i) {
*indices++ = dist_index_[i].index_;
*dists++ = dist_index_[i].dist_;
}
}
DistanceType worstDist() const
{
return worst_distance_;
}
private:
size_t capacity_;
size_t count_;
DistanceType worst_distance_;
std::vector<DistIndex> dist_index_;
};
/**
* K-Nearest neighbour result set. Ensures that the elements inserted are unique
*/
template <typename DistanceType>
class KNNResultSet : public ResultSet<DistanceType>
{
public:
typedef DistanceIndex<DistanceType> DistIndex;
KNNResultSet(int capacity) : capacity_(capacity)
{
// reserving capacity to prevent memory re-allocations
dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),-1));
clear();
}
~KNNResultSet()
{
}
/**
* Clears the result set
*/
void clear()
{
worst_distance_ = std::numeric_limits<DistanceType>::max();
dist_index_[capacity_-1].dist_ = worst_distance_;
count_ = 0;
}
size_t size() const
{
return count_;
}
bool full() const
{
return count_ == capacity_;
}
void addPoint(DistanceType dist, size_t index)
{
if (dist >= worst_distance_) return;
size_t i;
for (i = count_; i > 0; --i) {
#ifdef FLANN_FIRST_MATCH
if ( (dist_index_[i-1].dist_<=dist) && ((dist!=dist_index_[i-1].dist_)||(dist_index_[i-1].index_<=index)) )
#else
if (dist_index_[i-1].dist_<=dist)
#endif
{
// Check for duplicate indices
for (size_t j = i - 1; dist_index_[j].dist_ == dist && j--;) {
if (dist_index_[j].index_ == index) {
return;
}
}
break;
}
}
if (count_ < capacity_) ++count_;
for (size_t j = count_-1; j > i; --j) {
dist_index_[j] = dist_index_[j-1];
}
dist_index_[i].dist_ = dist;
dist_index_[i].index_ = index;
worst_distance_ = dist_index_[capacity_-1].dist_;
}
/**
* Copy indices and distances to output buffers
* @param indices
* @param dists
* @param num_elements Number of elements to copy
* @param sorted Indicates if results should be sorted
*/
void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
{
size_t n = std::min(count_, num_elements);
for (size_t i=0; i<n; ++i) {
*indices++ = dist_index_[i].index_;
*dists++ = dist_index_[i].dist_;
}
}
DistanceType worstDist() const
{
return worst_distance_;
}
private:
size_t capacity_;
size_t count_;
DistanceType worst_distance_;
std::vector<DistIndex> dist_index_;
};
template <typename DistanceType>
class KNNResultSet2 : public ResultSet<DistanceType>
{
public:
typedef DistanceIndex<DistanceType> DistIndex;
KNNResultSet2(size_t capacity_) :
capacity_(capacity_)
{
// reserving capacity to prevent memory re-allocations
dist_index_.reserve(capacity_);
clear();
}
~KNNResultSet2()
{
}
/**
* Clears the result set
*/
void clear()
{
dist_index_.clear();
worst_dist_ = std::numeric_limits<DistanceType>::max();
is_full_ = false;
}
/**
*
* @return Number of elements in the result set
*/
size_t size() const
{
return dist_index_.size();
}
/**
* Radius search result set always reports full
* @return
*/
bool full() const
{
return is_full_;
}
/**
* Add another point to result set
* @param dist distance to point
* @param index index of point
* Pre-conditions: capacity_>0
*/
void addPoint(DistanceType dist, size_t index)
{
if (dist>=worst_dist_) return;
if (dist_index_.size()==capacity_) {
// if result set if filled to capacity, remove farthest element
std::pop_heap(dist_index_.begin(), dist_index_.end());
dist_index_.pop_back();
}
// add new element
dist_index_.push_back(DistIndex(dist,index));
if (is_full_) { // when is_full_==true, we have a heap
std::push_heap(dist_index_.begin(), dist_index_.end());
}
if (dist_index_.size()==capacity_) {
if (!is_full_) {
std::make_heap(dist_index_.begin(), dist_index_.end());
is_full_ = true;
}
// we replaced the farthest element, update worst distance
worst_dist_ = dist_index_[0].dist_;
}
}
/**
* Copy indices and distances to output buffers
* @param indices
* @param dists
* @param num_elements Number of elements to copy
* @param sorted Indicates if results should be sorted
*/
void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
{
if (sorted) {
// std::sort_heap(dist_index_.begin(), dist_index_.end());
// sort seems faster here, even though dist_index_ is a heap
std::sort(dist_index_.begin(), dist_index_.end());
}
else {
if (num_elements<size()) {
std::nth_element(dist_index_.begin(), dist_index_.begin()+num_elements, dist_index_.end());
}
}
size_t n = std::min(dist_index_.size(), num_elements);
for (size_t i=0; i<n; ++i) {
*indices++ = dist_index_[i].index_;
*dists++ = dist_index_[i].dist_;
}
}
DistanceType worstDist() const
{
return worst_dist_;
}
private:
size_t capacity_;
DistanceType worst_dist_;
std::vector<DistIndex> dist_index_;
bool is_full_;
};
/**
* Unbounded radius result set. It will hold as many elements as
* are added to it.
*/
template <typename DistanceType>
class RadiusResultSet : public ResultSet<DistanceType>
{
public:
typedef DistanceIndex<DistanceType> DistIndex;
RadiusResultSet(DistanceType radius_) :
radius_(radius_)
{
// reserving some memory to limit number of re-allocations
dist_index_.reserve(1024);
clear();
}
~RadiusResultSet()
{
}
/**
* Clears the result set
*/
void clear()
{
dist_index_.clear();
}
/**
*
* @return Number of elements in the result set
*/
size_t size() const
{
return dist_index_.size();
}
/**
* Radius search result set always reports full
* @return
*/
bool full() const
{
return true;
}
/**
* Add another point to result set
* @param dist distance to point
* @param index index of point
* Pre-conditions: capacity_>0
*/
void addPoint(DistanceType dist, size_t index)
{
if (dist<radius_) {
// add new element
dist_index_.push_back(DistIndex(dist,index));
}
}
/**
* Copy indices and distances to output buffers
* @param indices
* @param dists
* @param num_elements Number of elements to copy
* @param sorted Indicates if results should be sorted
*/
void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
{
if (sorted) {
// std::sort_heap(dist_index_.begin(), dist_index_.end());
// sort seems faster here, even though dist_index_ is a heap
std::sort(dist_index_.begin(), dist_index_.end());
}
else {
if (num_elements<size()) {
std::nth_element(dist_index_.begin(), dist_index_.begin()+num_elements, dist_index_.end());
}
}
size_t n = std::min(dist_index_.size(), num_elements);
for (size_t i=0; i<n; ++i) {
*indices++ = dist_index_[i].index_;
*dists++ = dist_index_[i].dist_;
}
}
DistanceType worstDist() const
{
return radius_;
}
private:
DistanceType radius_;
std::vector<DistIndex> dist_index_;
};
/**
* Bounded radius result set. It limits the number of elements
* it can hold to a preset capacity.
*/
template <typename DistanceType>
class KNNRadiusResultSet : public ResultSet<DistanceType>
{
public:
typedef DistanceIndex<DistanceType> DistIndex;
KNNRadiusResultSet(DistanceType radius_, size_t capacity_) :
radius_(radius_), capacity_(capacity_)
{
// reserving capacity to prevent memory re-allocations
dist_index_.reserve(capacity_);
clear();
}
~KNNRadiusResultSet()
{
}
/**
* Clears the result set
*/
void clear()
{
dist_index_.clear();
worst_dist_ = radius_;
is_heap_ = false;
}
/**
*
* @return Number of elements in the result set
*/
size_t size() const
{
return dist_index_.size();
}
/**
* Radius search result set always reports full
* @return
*/
bool full() const
{
return true;
}
/**
* Add another point to result set
* @param dist distance to point
* @param index index of point
* Pre-conditions: capacity_>0
*/
void addPoint(DistanceType dist, size_t index)
{
if (dist>=worst_dist_) return;
if (dist_index_.size()==capacity_) {
// if result set is filled to capacity, remove farthest element
std::pop_heap(dist_index_.begin(), dist_index_.end());
dist_index_.pop_back();
}
// add new element
dist_index_.push_back(DistIndex(dist,index));
if (is_heap_) {
std::push_heap(dist_index_.begin(), dist_index_.end());
}
if (dist_index_.size()==capacity_) {
// when got to full capacity, make it a heap
if (!is_heap_) {
std::make_heap(dist_index_.begin(), dist_index_.end());
is_heap_ = true;
}
// we replaced the farthest element, update worst distance
worst_dist_ = dist_index_[0].dist_;
}
}
/**
* Copy indices and distances to output buffers
* @param indices
* @param dists
* @param num_elements Number of elements to copy
* @param sorted Indicates if results should be sorted
*/
void copy(size_t* indices, DistanceType* dists, size_t num_elements, bool sorted = true)
{
if (sorted) {
// std::sort_heap(dist_index_.begin(), dist_index_.end());
// sort seems faster here, even though dist_index_ is a heap
std::sort(dist_index_.begin(), dist_index_.end());
}
else {
if (num_elements<size()) {
std::nth_element(dist_index_.begin(), dist_index_.begin()+num_elements, dist_index_.end());
}
}
size_t n = std::min(dist_index_.size(), num_elements);
for (size_t i=0; i<n; ++i) {
*indices++ = dist_index_[i].index_;
*dists++ = dist_index_[i].dist_;
}
}
DistanceType worstDist() const
{
return worst_dist_;
}
private:
bool is_heap_;
DistanceType radius_;
size_t capacity_;
DistanceType worst_dist_;
std::vector<DistIndex> dist_index_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* This is a result set that only counts the neighbors within a radius.
*/
template <typename DistanceType>
class CountRadiusResultSet : public ResultSet<DistanceType>
{
DistanceType radius;
size_t count;
public:
CountRadiusResultSet(DistanceType radius_ ) :
radius(radius_)
{
clear();
}
~CountRadiusResultSet()
{
}
void clear()
{
count = 0;
}
size_t size() const
{
return count;
}
bool full() const
{
return true;
}
void addPoint(DistanceType dist, size_t index)
{
if (dist<radius) {
count++;
}
}
DistanceType worstDist() const
{
return radius;
}
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** Class that holds the k NN neighbors
*/
template<typename DistanceType>
class UniqueResultSet : public ResultSet<DistanceType>
{
public:
struct DistIndex
{
DistIndex(DistanceType dist, unsigned int index) :
dist_(dist), index_(index)
{
}
bool operator<(const DistIndex dist_index) const
{
return (dist_ < dist_index.dist_) || ((dist_ == dist_index.dist_) && index_ < dist_index.index_);
}
DistanceType dist_;
unsigned int index_;
};
/** Default cosntructor */
UniqueResultSet() :
worst_distance_(std::numeric_limits<DistanceType>::max())
{
}
/** Check the status of the set
* @return true if we have k NN
*/
inline bool full() const
{
return is_full_;
}
/** Copy the set to two C arrays
* @param indices pointer to a C array of indices
* @param dist pointer to a C array of distances
* @param n_neighbors the number of neighbors to copy
*/
void copy(size_t* indices, DistanceType* dist, int n_neighbors, bool sorted = true)
{
if (n_neighbors<0) n_neighbors = dist_indices_.size();
int i = 0;
typedef typename std::set<DistIndex>::const_iterator Iterator;
for (Iterator dist_index = dist_indices_.begin(), dist_index_end =
dist_indices_.end(); (dist_index != dist_index_end) && (i < n_neighbors); ++dist_index, ++indices, ++dist, ++i) {
*indices = dist_index->index_;
*dist = dist_index->dist_;
}
}
/** The number of neighbors in the set
* @return
*/
size_t size() const
{
return dist_indices_.size();
}
/** The distance of the furthest neighbor
* If we don't have enough neighbors, it returns the max possible value
* @return
*/
inline DistanceType worstDist() const
{
return worst_distance_;
}
protected:
/** Flag to say if the set is full */
bool is_full_;
/** The worst distance found so far */
DistanceType worst_distance_;
/** The best candidates so far */
std::set<DistIndex> dist_indices_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** Class that holds the k NN neighbors
* Faster than KNNResultSet as it uses a binary heap and does not maintain two arrays
*/
template<typename DistanceType>
class KNNUniqueResultSet : public UniqueResultSet<DistanceType>
{
public:
/** Constructor
* @param capacity the number of neighbors to store at max
*/
KNNUniqueResultSet(unsigned int capacity) : capacity_(capacity)
{
this->is_full_ = false;
this->clear();
}
/** Add a possible candidate to the best neighbors
* @param dist distance for that neighbor
* @param index index of that neighbor
*/
inline void addPoint(DistanceType dist, size_t index)
{
// Don't do anything if we are worse than the worst
if (dist >= worst_distance_) return;
dist_indices_.insert(DistIndex(dist, index));
if (is_full_) {
if (dist_indices_.size() > capacity_) {
dist_indices_.erase(*dist_indices_.rbegin());
worst_distance_ = dist_indices_.rbegin()->dist_;
}
}
else if (dist_indices_.size() == capacity_) {
is_full_ = true;
worst_distance_ = dist_indices_.rbegin()->dist_;
}
}
/** Remove all elements in the set
*/
void clear()
{
dist_indices_.clear();
worst_distance_ = std::numeric_limits<DistanceType>::max();
is_full_ = false;
}
protected:
typedef typename UniqueResultSet<DistanceType>::DistIndex DistIndex;
using UniqueResultSet<DistanceType>::is_full_;
using UniqueResultSet<DistanceType>::worst_distance_;
using UniqueResultSet<DistanceType>::dist_indices_;
/** The number of neighbors to keep */
unsigned int capacity_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** Class that holds the radius nearest neighbors
* It is more accurate than RadiusResult as it is not limited in the number of neighbors
*/
template<typename DistanceType>
class RadiusUniqueResultSet : public UniqueResultSet<DistanceType>
{
public:
/** Constructor
* @param capacity the number of neighbors to store at max
*/
RadiusUniqueResultSet(DistanceType radius) :
radius_(radius)
{
is_full_ = true;
}
/** Add a possible candidate to the best neighbors
* @param dist distance for that neighbor
* @param index index of that neighbor
*/
void addPoint(DistanceType dist, size_t index)
{
if (dist < radius_) dist_indices_.insert(DistIndex(dist, index));
}
/** Remove all elements in the set
*/
inline void clear()
{
dist_indices_.clear();
}
/** Check the status of the set
* @return alwys false
*/
inline bool full() const
{
return true;
}
/** The distance of the furthest neighbor
* If we don't have enough neighbors, it returns the max possible value
* @return
*/
inline DistanceType worstDist() const
{
return radius_;
}
private:
typedef typename UniqueResultSet<DistanceType>::DistIndex DistIndex;
using UniqueResultSet<DistanceType>::dist_indices_;
using UniqueResultSet<DistanceType>::is_full_;
/** The furthest distance a neighbor can be */
DistanceType radius_;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** Class that holds the k NN neighbors within a radius distance
*/
template<typename DistanceType>
class KNNRadiusUniqueResultSet : public KNNUniqueResultSet<DistanceType>
{
public:
/** Constructor
* @param capacity the number of neighbors to store at max
*/
KNNRadiusUniqueResultSet(DistanceType radius, size_t capacity) : KNNUniqueResultSet<DistanceType>(capacity)
{
this->radius_ = radius;
this->clear();
}
/** Remove all elements in the set
*/
void clear()
{
dist_indices_.clear();
worst_distance_ = radius_;
is_full_ = true;
}
private:
using KNNUniqueResultSet<DistanceType>::dist_indices_;
using KNNUniqueResultSet<DistanceType>::is_full_;
using KNNUniqueResultSet<DistanceType>::worst_distance_;
/** The maximum distance of a neighbor */
DistanceType radius_;
};
}
#endif //FLANN_RESULTSET_H

72
corelib/src/rtflann/util/sampling.h Normal file
View File

@@ -0,0 +1,72 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_SAMPLING_H_
#define RTABMAP_FLANN_SAMPLING_H_
#include "rtflann/util/matrix.h"
#include "rtflann/util/random.h"
namespace rtflann
{
template<typename T>
Matrix<T> random_sample(Matrix<T>& srcMatrix, size_t size, bool remove = false)
{
UniqueRandom rand_unique(srcMatrix.rows);
Matrix<T> newSet(new T[size * srcMatrix.cols], size,srcMatrix.cols);
T* src,* dest;
for (size_t i=0; i<size; ++i) {
size_t r;
if (remove) {
r = static_cast<size_t>(rand_int(srcMatrix.rows-i));
}
else {
r = static_cast<size_t>(rand_unique.next());
}
dest = newSet[i];
src = srcMatrix[r];
std::copy(src, src+srcMatrix.cols, dest);
if (remove) {
src = srcMatrix[srcMatrix.rows-i-1];
dest = srcMatrix[r];
std::copy(src, src+srcMatrix.cols, dest);
}
}
if (remove) {
srcMatrix.rows -= size;
}
return newSet;
}
} // namespace
#endif /* FLANN_SAMPLING_H_ */

135
corelib/src/rtflann/util/saving.h Normal file
View File

@@ -0,0 +1,135 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE NNIndexGOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_SAVING_H_
#define RTABMAP_FLANN_SAVING_H_
#include <cstring>
#include <vector>
#include <stdio.h>
#include "rtflann/general.h"
#include "rtflann/util/serialization.h"
#ifdef FLANN_SIGNATURE_
#undef FLANN_SIGNATURE_
#endif
#define FLANN_SIGNATURE_ "FLANN_INDEX_v1.1"
namespace rtflann
{
/**
* Structure representing the index header.
*/
struct IndexHeader
{
IndexHeaderStruct h;
IndexHeader()
{
memset(h.signature, 0, sizeof(h.signature));
strcpy(h.signature, FLANN_SIGNATURE_);
memset(h.version, 0, sizeof(h.version));
strcpy(h.version, FLANN_VERSION_);
h.compression = 0;
h.first_block_size = 0;
}
private:
template<typename Archive>
void serialize(Archive& ar)
{
ar & h.signature;
ar & h.version;
ar & h.data_type;
ar & h.index_type;
ar & h.rows;
ar & h.cols;
ar & h.compression;
ar & h.first_block_size;
}
friend struct serialization::access;
};
/**
* Saves index header to stream
*
* @param stream - Stream to save to
* @param index - The index to save
*/
template<typename Index>
void save_header(FILE* stream, const Index& index)
{
IndexHeader header;
header.h.data_type = flann_datatype_value<typename Index::ElementType>::value;
header.h.index_type = index.getType();
header.h.rows = index.size();
header.h.cols = index.veclen();
fwrite(&header, sizeof(header),1,stream);
}
/**
*
* @param stream - Stream to load from
* @return Index header
*/
inline IndexHeader load_header(FILE* stream)
{
IndexHeader header;
int read_size = fread(&header,sizeof(header),1,stream);
if (read_size != 1) {
throw FLANNException("Invalid index file, cannot read");
}
if (strncmp(header.h.signature,
FLANN_SIGNATURE_,
strlen(FLANN_SIGNATURE_) - strlen("v0.0")) != 0) {
throw FLANNException("Invalid index file, wrong signature");
}
return header;
}
namespace serialization
{
ENUM_SERIALIZER(flann_algorithm_t);
ENUM_SERIALIZER(flann_centers_init_t);
ENUM_SERIALIZER(flann_log_level_t);
ENUM_SERIALIZER(flann_datatype_t);
}
}
#endif /* FLANN_SAVING_H_ */

813
corelib/src/rtflann/util/serialization.h Normal file
View File

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

95
corelib/src/rtflann/util/timer.h Normal file
View File

@@ -0,0 +1,95 @@
/***********************************************************************
* Software License Agreement (BSD License)
*
* Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
* Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
*
* THE BSD LICENSE
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
* IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
* NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*************************************************************************/
#ifndef RTABMAP_FLANN_TIMER_H
#define RTABMAP_FLANN_TIMER_H
#include <time.h>
namespace rtflann
{
/**
* A start-stop timer class.
*
* Can be used to time portions of code.
*/
class StartStopTimer
{
clock_t startTime;
public:
/**
* Value of the timer.
*/
double value;
/**
* Constructor.
*/
StartStopTimer()
{
reset();
}
/**
* Starts the timer.
*/
void start()
{
startTime = clock();
}
/**
* Stops the timer and updates timer value.
*/
double stop()
{
clock_t stopTime = clock();
value += ( (double)stopTime - startTime) / CLOCKS_PER_SEC;
return value;
}
/**
* Resets the timer value to 0.
*/
void reset()
{
value = 0;
}
};
}
#endif // FLANN_TIMER_H