319 lines
6.9 KiB
C++
319 lines
6.9 KiB
C++
#ifndef DATAMAP2_H
|
|
#define DATAMAP2_H
|
|
|
|
#include "../../../geo/BBox2.h"
|
|
#include <vector>
|
|
#include <functional>
|
|
|
|
template <typename T> class DataMap {
|
|
|
|
private:
|
|
|
|
float sx_m;
|
|
float sy_m;
|
|
|
|
float x_m;
|
|
float y_m;
|
|
int gridSize_cm;
|
|
|
|
BBox2 bbox;
|
|
|
|
int nx;
|
|
int ny;
|
|
|
|
T* data = nullptr;
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
DataMap() {
|
|
;
|
|
}
|
|
|
|
~DataMap() {
|
|
|
|
// cleanup
|
|
cleanup();
|
|
|
|
}
|
|
|
|
DataMap(const DataMap&) = delete;
|
|
DataMap* operator = (const DataMap& o) = delete;
|
|
|
|
T& operator [] (const int idx) {
|
|
return data[idx];
|
|
}
|
|
|
|
const T& operator [] (const int idx) const {
|
|
return data[idx];
|
|
}
|
|
|
|
|
|
/** does the map contain the given indices? */
|
|
bool containsGrid(const int x, const int y) const {
|
|
return (x >= 0) && (y >= 0) && (x < nx) && (y < ny);
|
|
}
|
|
|
|
/** does the map contain the given coordinate? */
|
|
bool contain(const float x_m, const float y_m) const {
|
|
return bbox.contains(Point2(x_m, y_m));
|
|
}
|
|
|
|
void resize(const DataMap& other) {
|
|
resize(other.bbox, other.gridSize_cm);
|
|
}
|
|
|
|
void resize(const BBox2 bbox, const int gridSize_cm) {
|
|
|
|
// cleanup
|
|
cleanup();
|
|
|
|
this->bbox = bbox;
|
|
|
|
// slightly increase to pervent out-of-bounds due to rounding
|
|
float buffer_m = 1;
|
|
|
|
// start-offset
|
|
sx_m = bbox.getMin().x - buffer_m;
|
|
sy_m = bbox.getMin().y - buffer_m;
|
|
|
|
// size in meter
|
|
x_m = (bbox.getMax().x - bbox.getMin().x) + 2*buffer_m;
|
|
y_m = (bbox.getMax().y - bbox.getMin().y) + 2*buffer_m;
|
|
|
|
// number of elements in the grid
|
|
this->gridSize_cm = gridSize_cm;
|
|
nx = (x_m*100) / gridSize_cm;
|
|
ny = (y_m*100) / gridSize_cm;
|
|
|
|
// allocate and reset all to 0.0
|
|
data = new T[nx*ny];
|
|
//std::fill(&data[0], &data[nx*ny], 0);
|
|
|
|
}
|
|
|
|
/** get the used grid-size (in cm) */
|
|
int getGridSize_cm() const {return gridSize_cm;}
|
|
|
|
void set(const float x_m, const float y_m, const T val) {
|
|
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm);
|
|
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm);
|
|
setGrid(ix, iy, val);
|
|
}
|
|
|
|
T& getRef(const int idx) const {
|
|
return data[idx];
|
|
}
|
|
|
|
T get(const float x_m, const float y_m) const {
|
|
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
|
|
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
|
|
return getGrid(ix, iy);
|
|
}
|
|
|
|
T& getRef(const float x_m, const float y_m) {
|
|
const int ix = std::round( ((x_m-sx_m)) * 100 / gridSize_cm );
|
|
const int iy = std::round( ((y_m-sy_m)) * 100 / gridSize_cm );
|
|
return getGridRef(ix, iy);
|
|
}
|
|
|
|
T getGrid(const int ix, const int iy) const {
|
|
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
|
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
|
const int idx = ix + iy*nx;
|
|
return data[idx];
|
|
}
|
|
|
|
T& getGridRef(const int ix, const int iy) {
|
|
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
|
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
|
const int idx = ix + iy*nx;
|
|
return data[idx];
|
|
}
|
|
|
|
const T& getGridRef(const int ix, const int iy) const {
|
|
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
|
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
|
const int idx = ix + iy*nx;
|
|
return data[idx];
|
|
}
|
|
|
|
void setGrid(const int ix, const int iy, const T val) {
|
|
Assert::isBetween(ix, 0, nx-1, "x out of range");
|
|
Assert::isBetween(iy, 0, ny-1, "y out of range");
|
|
const int idx = ix + iy*nx;
|
|
data[idx] = val;
|
|
}
|
|
|
|
/** convert grid indices to point coordinates */
|
|
Point2 gridToPos(const int ix, const int iy) const {
|
|
return Point2(
|
|
(ix * gridSize_cm / 100.0f) + sx_m,
|
|
(iy * gridSize_cm / 100.0f) + sy_m
|
|
);
|
|
}
|
|
|
|
/** convert 1D array index to point coordinates */
|
|
Point2 idxToPos(const int idx) const {
|
|
const int ix = idx % nx;
|
|
const int iy = idx / nx;
|
|
return gridToPos(ix, iy);
|
|
}
|
|
|
|
/** convert 2D to 1D index */
|
|
int getIndex(const int ix, const int iy) const {
|
|
return ix + iy*nx;
|
|
}
|
|
|
|
void forEach(std::function<void(float,float,const T&)> func) const {
|
|
for (int iy = 0; iy < ny; ++iy) {
|
|
for (int ix = 0; ix < nx; ++ix) {
|
|
const float x = (ix * gridSize_cm / 100.0f) + sx_m;
|
|
const float y = (iy * gridSize_cm / 100.0f) + sy_m;
|
|
func(x,y,getGridRef(ix, iy));
|
|
}
|
|
}
|
|
}
|
|
|
|
void forEachGrid(std::function<void(int,int,T&)> func) {
|
|
for (int iy = 0; iy < ny; ++iy) {
|
|
for (int ix = 0; ix < nx; ++ix) {
|
|
func(ix,iy,getGridRef(ix, iy));
|
|
}
|
|
}
|
|
}
|
|
|
|
private:
|
|
|
|
void cleanup() {
|
|
delete[] data;
|
|
data = nullptr;
|
|
}
|
|
|
|
|
|
};
|
|
|
|
|
|
struct DataMapNeighbors {
|
|
|
|
/** reference to all neighbors */
|
|
std::vector<int> neighbors;
|
|
|
|
};
|
|
|
|
|
|
struct DataMapSignalEntry : public DataMapNeighbors {
|
|
|
|
struct Entry {
|
|
float rssi;
|
|
float distanceToAP;
|
|
Entry(float rssi, float distanceToAP) : rssi(rssi), distanceToAP(distanceToAP) {;}
|
|
};
|
|
|
|
std::vector<Entry> entries;
|
|
|
|
void add(const DataMapSignalEntry& o) {
|
|
for (const Entry& e : o.entries) {entries.push_back(e);}
|
|
}
|
|
|
|
void add(const float rssi, const float distanceToAP) {
|
|
Entry e(rssi, distanceToAP);
|
|
entries.push_back(e);
|
|
}
|
|
|
|
float getMaxRSSI() const {
|
|
auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;};
|
|
if (entries.empty()) {return -120;}
|
|
auto it = std::max_element(entries.begin(), entries.end(), comp);
|
|
return it->rssi;
|
|
}
|
|
|
|
float getFirstRSSI() const {
|
|
auto comp = [] (const Entry& e1, const Entry& e2) {return e1.distanceToAP < e2.distanceToAP;};
|
|
if (entries.empty()) {return -120;}
|
|
auto it = std::min_element(entries.begin(), entries.end(), comp);
|
|
return it->rssi;
|
|
}
|
|
|
|
float getAvgFirst() const {
|
|
if (entries.empty()) {return -120;}
|
|
if (entries.size()==1) {return entries.front().rssi;}
|
|
std::vector<Entry> copy = entries;
|
|
auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi > e2.rssi;};
|
|
std::sort(copy.begin(), copy.end(), comp);
|
|
|
|
float sum = 0;
|
|
int cnt = std::min((int)copy.size(), 15);
|
|
for (int i = 0; i < cnt; ++i) {
|
|
sum += copy[i].rssi;
|
|
}
|
|
return sum/cnt;
|
|
|
|
}
|
|
|
|
// float get2ndMaxRSSI() const {
|
|
// if (entries.empty()) {return -120;}
|
|
// if (entries.size()==1) {return entries.front().rssi;}
|
|
// std::vector<Entry> copy = entries;
|
|
// auto comp = [] (const Entry& e1, const Entry& e2) {return e1.rssi < e2.rssi;};
|
|
// std::sort(copy.begin(), copy.end(), comp);
|
|
// return copy[copy.size()-2].rssi;
|
|
// }
|
|
|
|
|
|
};
|
|
|
|
|
|
class DataMapSignal : public DataMap<DataMapSignalEntry> {
|
|
|
|
public:
|
|
|
|
/** update average */
|
|
void update(const float x_m, const float y_m, const float rssi, const float distanceToAP) {
|
|
|
|
DataMapSignalEntry& entry = getRef(x_m, y_m);
|
|
entry.add(rssi, distanceToAP);
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
class DataMap2Factory {
|
|
|
|
public:
|
|
|
|
|
|
/** combine neighboring nodes into one */
|
|
template <typename T> static void combine(const DataMap<T>& map, DataMap<T>& dst) {
|
|
|
|
auto forEach = [&] (const float, const float, const T& n) {
|
|
for (int idx : n.neighbors) {
|
|
dst[idx].add(n);
|
|
}
|
|
};
|
|
|
|
map.forEach(forEach);
|
|
|
|
}
|
|
|
|
/** fill empty fields with the values of their immediate neighbors */
|
|
template <typename T> static void fillGaps(const DataMap<T>& map, DataMap<T>& dst) {
|
|
|
|
auto forEach = [&] (const float, const float, const T& n) {
|
|
if (n.entries.empty()) {
|
|
for (int idx : n.neighbors) {
|
|
dst[idx].add(n);
|
|
}
|
|
}
|
|
};
|
|
|
|
map.forEach(forEach);
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // DATAMAP2_H
|