FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added several grid-walks

Browse Source 
added new helper methods/classes (e.g. for heading)
new test cases
optimize the dijkstra
cleanups/refactoring
added timed-benchmarks to the log
many more...
This commit is contained in:
FrankE
2016-01-24 18:59:06 +01:00
parent cdf97322f8
commit 9947dced15
30 changed files with 1406 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
misc/KNN.h
View File

@@ -2,6 +2,7 @@
#define KNN_H
#include "../lib/nanoflann/nanoflann.hpp"
#include "Debug.h"
/**
* helper class to extract k-nearest-neighbors
@@ -9,14 +10,16 @@
* uses nanoflann
*
* usage:
* KNN<float, Grid<20, T>, T, 3> knn(theGrid);
* float search[] = {0,0,0};
* std::vector<T> elems = knn.get(search, 3);
* Grid<30, T> theGrid;
* KNN<Grid<20, T>, 3, float> knn(theGrid);
* std::vector<T> elems = knn.get({0,0,0}, 10);
*/
template <typename Scalar, typename DataStructure, typename Element, int dim> class KNN {
template <typename DataStructure, int dim, typename Scalar = float> class KNN {
private:
static constexpr const char* name = "KNN";
/** type-definition for the nanoflann KD-Tree used for searching */
typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<Scalar, DataStructure>, DataStructure, dim> Tree;
@@ -33,11 +36,15 @@ public:
/** ctor */
KNN(DataStructure& data) : tree(dim, data, nanoflann::KDTreeSingleIndexAdaptorParams(maxLeafs)), data(data) {
Log::add(name, "building kd-tree for " + std::to_string(data.kdtree_get_point_count()) + " elements");
tree.buildIndex();
Log::add(name, "done");
}
/** get the k-nearest-neighbors for the given input point */
std::vector<Element> get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const {
template <typename Element> std::vector<Element> get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const {
// buffer for to-be-fetched neighbors
size_t indices[numNeighbors];
@@ -56,6 +63,11 @@ public:
}
/** get the k-nearest-neighbors for the given input point */
template <typename Element> std::vector<Element> get(std::initializer_list<Scalar> point, const int numNeighbors, const float maxDistSquared = 99999) const {
return get(point.begin(), numNeighbors, maxDistSquared);
}
/** get the nearest neighbor and its distance */
void getNearest(const Scalar* point, size_t& idx, float& distSquared) {
@@ -64,6 +76,30 @@ public:
}
/** get the index of the element nearest to the given point */
size_t getNearestIndex(const Scalar* point) {
size_t idx;
float distSquared;
tree.knnSearch(point, 1, &idx, &distSquared);
return idx;
}
/** get the index of the element nearest to the given point */
size_t getNearestIndex(const std::initializer_list<Scalar> lst) {
size_t idx;
float distSquared;
tree.knnSearch(lst.begin(), 1, &idx, &distSquared);
return idx;
}
/** get the distance to the element nearest to the given point */
float getNearestDistance(const std::initializer_list<Scalar> lst) {
size_t idx;
float distSquared;
tree.knnSearch(lst.begin(), 1, &idx, &distSquared);
return std::sqrt(distSquared);
}
void get(const Scalar* point, const int numNeighbors, size_t* indices, float* squaredDist) {
// find k-nearest-neighbors