FHWS/RussenJournal
Archived
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

init

Browse Source
This commit is contained in:
toni
2016-11-16 12:21:39 +01:00
commit 1adb74ec29
7 changed files with 1203 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

252
FileReader.h Normal file
View File

@@ -0,0 +1,252 @@
#ifndef FILEREADER_H
#define FILEREADER_H
#include <fstream>
#include <Indoor/Exception.h>
#include <vector>
#include <Indoor/math/Interpolator.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/pressure/BarometerData.h>
#include <Indoor/sensors/imu/AccelerometerData.h>
#include <Indoor/sensors/imu/GyroscopeData.h>
#include <Indoor/sensors/beacon/BeaconMeasurements.h>
#include <unordered_map>
#include <Indoor/geo/Point2.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/floorplan/v2/Floorplan.h>
class FileReader {
public:
template <typename T> struct TS {
const uint64_t ts;
T data;
TS(const uint64_t ts) : ts(ts) {;}
TS(const uint64_t ts, const T& data) : ts(ts), data(data) {;}
};
enum class Sensor {
ACC,
GYRO,
WIFI,
POS,
BARO,
BEACON,
};
/** entry for one sensor */
struct Entry {
Sensor type;
uint64_t ts;
int idx;
Entry(Sensor type, uint64_t ts, int idx) : type(type), ts(ts), idx(idx) {;}
};
std::vector<TS<int>> groundTruth;
std::vector<TS<WiFiMeasurements>> wifi;
std::vector<TS<BeaconMeasurement>> beacon;
std::vector<TS<AccelerometerData>> acc;
std::vector<TS<GyroscopeData>> gyro;
std::vector<TS<BarometerData>> barometer;
/** ALL entries */
std::vector<Entry> entries;
public:
FileReader(const std::string& file) {
parse(file);
}
const std::vector<Entry>& getEntries() const {return entries;}
const std::vector<TS<int>>& getGroundTruth() const {return groundTruth;}
const std::vector<TS<WiFiMeasurements>>& getWiFiGroupedByTime() const {return wifi;}
const std::vector<TS<BeaconMeasurement>>& getBeacons() const {return beacon;}
const std::vector<TS<AccelerometerData>>& getAccelerometer() const {return acc;}
const std::vector<TS<GyroscopeData>>& getGyroscope() const {return gyro;}
const std::vector<TS<BarometerData>>& getBarometer() const {return barometer;}
private:
void parse(const std::string& file) {
std::ifstream inp(file);
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
while(!inp.eof() && !inp.bad()) {
uint64_t ts;
char delim;
int idx = -1;
std::string data;
inp >> ts;
inp >> delim;
inp >> idx;
inp >> delim;
inp >> data;
if (idx == 8) {parseWiFi(ts, data);}
else if (idx == 9) {parseBeacons(ts, data);}
else if (idx == 99) {parseGroundTruth(ts, data);}
else if (idx == 0) {parseAccelerometer(ts, data);}
else if (idx == 3) {parseGyroscope(ts, data);}
else if (idx == 5) {parseBarometer(ts, data);}
// TODO: this is a hack...
// the loop is called one additional time after the last entry
// and keeps the entries of entry
}
inp.close();
}
void parseAccelerometer(const uint64_t ts, const std::string& data) {
const int pos1 = data.find(';');
const int pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1);
TS<AccelerometerData> elem(ts, AccelerometerData(std::stof(x), std::stof(y), std::stof(z)));
acc.push_back(elem);
entries.push_back(Entry(Sensor::ACC, ts, acc.size()-1));
}
void parseGyroscope(const uint64_t ts, const std::string& data) {
const int pos1 = data.find(';');
const int pos2 = data.find(';', pos1+1);
const std::string x = data.substr(0, pos1);
const std::string y = data.substr(pos1+1, pos2-pos1-1);
const std::string z = data.substr(pos2+1);
TS<GyroscopeData> elem(ts, GyroscopeData(std::stof(x), std::stof(y), std::stof(z)));
gyro.push_back(elem);
entries.push_back(Entry(Sensor::GYRO, ts, gyro.size()-1));
}
void parseWiFi(const uint64_t ts, const std::string& data) {
std::string tmp = data;
// add new wifi reading
wifi.push_back(TS<WiFiMeasurements>(ts, WiFiMeasurements()));
entries.push_back(Entry(Sensor::WIFI, ts, wifi.size()-1));
// process all APs
while(!tmp.empty()) {
std::string mac = tmp.substr(0, 17);
tmp = tmp.substr(17);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
std::string freq = tmp.substr(0, 4);
tmp = tmp.substr(4);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
int pos = tmp.find(';');
std::string rssi = tmp.substr(0, pos);
tmp = tmp.substr(pos);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
// append AP to current scan-entry
WiFiMeasurement e(AccessPoint(mac), std::stoi(rssi), Timestamp::fromMS(ts));
wifi.back().data.entries.push_back(e);
}
}
void parseBeacons(const uint64_t ts, const std::string& data) {
std::string tmp = data;
std::string mac = tmp.substr(0, 17);
tmp = tmp.substr(17);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
std::string rssi = tmp;
//yes the timestamp is redundant here, but in case of multiusage...
TS<BeaconMeasurement> e(ts, BeaconMeasurement(Timestamp::fromMS(ts), Beacon(mac), std::stoi(rssi)));
beacon.push_back(e);
entries.push_back(Entry(Sensor::BEACON, ts, beacon.size()-1));
}
void parseGroundTruth(const uint64_t ts, const std::string& data) {
const int pos1 = data.find(';');
std::string gtIndex = data.substr(0, pos1);
TS<int> elem(ts, std::stoi(gtIndex));
groundTruth.push_back(elem);
}
void parseBarometer(const uint64_t ts, const std::string& data) {
const int pos1 = data.find(';');
const std::string hPa = data.substr(0, pos1);
TS<BarometerData> elem(ts, BarometerData(std::stof(hPa)));
barometer.push_back(elem);
entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1));
}
const Interpolator<uint64_t, Point2> getGroundTruthPath(Floorplan::IndoorMap& map, std::vector<int> gtPath) const {
// finde alle positionen der waypoints im gtPath aus map
std::unordered_map<int, Point2> waypointsMap;
for(Floorplan::Floor* f : map.floors){
for (Floorplan::POI* poi : f->pois){
waypointsMap.insert({std::stoi(poi->name), poi->pos});
}
}
// bringe aufgenommene gt punkte der app in unordered_map
std::unordered_map<int, const uint64_t> waypointsApp;
for(TS<int> val : groundTruth){
waypointsApp.insert({val.data, val.ts});
}
// bringe diese in richtige reihenfolge und füge timestamp hinzu
Interpolator<uint64_t, Point2> interpol;
for(int id : gtPath){
auto itMap = waypointsMap.find(id);
if(itMap == waypointsMap.end()) {throw "not found";}
auto itApp = waypointsApp.find(id);
if(itApp == waypointsApp.end()) {throw "not found";}
interpol.add(itApp->second, itMap->second);
}
return interpol;
}
};
#endif // FILEREADER_H

250
Plotti.h Normal file
View File

@@ -0,0 +1,250 @@
#ifndef PLOTTI_H
#define PLOTTI_H
#include "filter/Structs.h"
#include <functional>
#include <Indoor/geo/Point2.h>
#include <Indoor/geo/Point3.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
struct Plotti {
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementPoints pGrid;
K::GnuplotSplotElementLines pFloor;
K::GnuplotSplotElementLines pOutline;
K::GnuplotSplotElementLines pStairs;
K::GnuplotSplotElementPoints pAPs;
K::GnuplotSplotElementPoints pInterest;
K::GnuplotSplotElementPoints pParticles;
K::GnuplotSplotElementColorPoints pColorPoints;
K::GnuplotSplotElementLines gtPath;
K::GnuplotSplotElementLines estPath;
K::GnuplotSplotElementLines estPathSmoothed;
Plotti() {
gp << "set xrange[0-50:70+50]\nset yrange[0-50:50+50]\nset ticslevel 0\n";
splot.add(&pGrid); pGrid.setPointSize(0.25); pGrid.setColorHex("#888888");
splot.add(&pAPs); pAPs.setPointSize(0.7);
splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
splot.add(&pParticles); pParticles.setColorHex("#0000ff"); pParticles.setPointSize(0.4f);
splot.add(&pFloor);
splot.add(&pOutline); pOutline.setColorHex("#999999");
splot.add(&pStairs); pStairs.setColorHex("#000000");
splot.add(&pInterest); pInterest.setPointSize(2); pInterest.setColorHex("#ff0000");
splot.add(&gtPath); gtPath.setLineWidth(2); gtPath.setColorHex("#000000");
splot.add(&estPath); estPath.setLineWidth(2); estPath.setColorHex("#00ff00");
splot.add(&estPathSmoothed); estPathSmoothed.setLineWidth(2); estPathSmoothed.setColorHex("#0000ff");
}
void addLabel(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "'\n";
}
void addLabelV(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "' rotate by 90\n";
}
void showAngle(const int idx, const float rad) {
Point2 cen(0.9, 0.9);
Point2 rot(0, 1);
Point2 pos = cen + rot.rotated(rad) * 0.05;
gp << "set arrow "<<idx<<" from screen " << cen.x << "," << cen.y << " to screen " << pos.x << "," << pos.y << "\n";
}
void setEst(const Point3 pos) {
gp << "set arrow 991 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+1 << " nohead lw 1 front \n";
}
void setGT(const Point3 pos) {
gp << "set arrow 995 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+0.3 << " nohead lw 3 front \n";
gp << "set arrow 996 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x+0.3 << "," << pos.y << "," << pos.z << " nohead lw 3 front \n";
}
void addGroundTruthNode(const Point3 pos) {
K::GnuplotPoint3 gp(pos.x, pos.y, pos.z);
gtPath.add(gp);
}
// estimated path
void addEstimationNode(const Point3 pos){
K::GnuplotPoint3 est(pos.x, pos.y, pos.z);
estPath.add(est);
}
// estimated path
void addEstimationNodeSmoothed(const Point3 pos){
K::GnuplotPoint3 est(pos.x, pos.y, pos.z);
estPathSmoothed.add(est);
}
template <typename Node> void debugProb(Grid<Node>& grid, std::function<double(const MyObs&, const Point3& pos)> func, const MyObs& obs) {
pColorPoints.clear();
// const float step = 2.0;
// float z = 0;
// for (float x = -20; x < 90; x += step) {
// for (float y = -10; y < 60; y += step) {
// const Point3 pos_m(x,y,z);
// const double prob = func(obs, pos_m);
// pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
// }
// }
std::minstd_rand gen;
std::uniform_int_distribution<int> dist(0, grid.getNumNodes()-1);
float min = +9999;
float max = -9999;
for (int i = 0; i < 10000; ++i) {
int idx = dist(gen);
Node& n = grid[idx];
const Point3 pos_cm(n.x_cm, n.y_cm, n.z_cm);
const Point3 pos_m = pos_cm / 100.0f;
const double prob = func(obs, pos_m);
if (prob < min) {min = prob;}
if (prob > max) {max = prob;}
pColorPoints.add(K::GnuplotPoint3(pos_m.x, pos_m.y, pos_m.z), prob);
}
if (min == max) {min -= 1;}
gp << "set cbrange [" << min << ":" << max << "]\n";
}
void addStairs(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::Stair* stair : f->stairs) {
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), f);
for (const Floorplan::Quad3& quad : quads) {
for (int i = 0; i < 4; ++i) {
int idx1 = i;
int idx2 = (i+1) % 4;
pStairs.addSegment(
K::GnuplotPoint3(quad[idx1].x,quad[idx1].y, quad[idx1].z),
K::GnuplotPoint3(quad[idx2].x,quad[idx2].y, quad[idx2].z)
);
}
}
}
}
}
void addFloors(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::FloorObstacle* obs : f->obstacles) {
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line) {
K::GnuplotPoint3 p1(line->from.x, line->from.y, f->atHeight);
K::GnuplotPoint3 p2(line->to.x, line->to.y, f->atHeight);
pFloor.addSegment(p1, p2);
}
}
}
}
void addOutline(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::FloorOutlinePolygon* poly : f->outline) {
const int cnt = poly->poly.points.size();
for (int i = 0; i < cnt; ++i) {
Point2 p1 = poly->poly.points[(i+0)];
Point2 p2 = poly->poly.points[(i+1)%cnt];
K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
pOutline.addSegment(gp1, gp2);
}
}
}
}
template <typename Node> void addGrid(Grid<Node>& grid) {
pGrid.clear();
for (const Node& n : grid) {
K::GnuplotPoint3 p(n.x_cm, n.y_cm, n.z_cm);
pGrid.add(p/100.0f);
}
}
template <typename State> void addParticles(const std::vector<K::Particle<State>>& particles) {
pParticles.clear();
int i = 0;
for (const K::Particle<State>& p : particles) {
if (++i % 25 != 0) {continue;}
K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
pParticles.add(pos / 100.0f);
}
}
void show() {
gp.draw(splot);
gp.flush();
}
void saveToFile(std::ofstream& stream){
gp.draw(splot);
stream << "set terminal x11 size 2000,1500\n";
stream << gp.getBuffer();
stream << "pause -1\n";
gp.flush();
}
void printSingleFloor(const std::string& path, const int floorNum) {
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_" << floorNum <<".png'\n";
gp << "set view 0,0\n";
gp << "set zrange [" << (floorNum * 4) - 2 << " : " << (floorNum * 4) + 2 << "]\n";
gp << "set autoscale xy\n";
}
void printSideView(const std::string& path, const int degree) {
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_deg" << degree <<".png'\n";
gp << "set view 90,"<< degree << "\n";
gp << "set autoscale xy\n";
gp << "set autoscale z\n";
}
void printOverview(const std::string& path) {
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_overview" << ".png'\n";
gp << "set view 75,60\n";
gp << "set autoscale xy\n";
gp << "set autoscale z\n";
}
};
#endif // PLOTTI_H

69
Settings.h Normal file
View File

@@ -0,0 +1,69 @@
#ifndef SETTINGS_H
#define SETTINGS_H
#include <Indoor/grid/GridPoint.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/VAPGrouper.h>
namespace Settings {
const int numParticles = 5000;
namespace IMU {
const float turnSigma = 1.5; // 3.5
const float stepLength = 0.80;
const float stepSigma = 0.40; //toni changed
}
const float smartphoneAboveGround = 1.3;
const float offlineSensorSpeedup = 2;
namespace Grid {
constexpr int gridSize_cm = 20;
}
//const GridPoint destination = GridPoint(70*100, 35*100, 0*100); // use destination
const GridPoint destination = GridPoint(0,0,0); // do not use destination
namespace SensorDebug {
const Timestamp updateEvery = Timestamp::fromMS(200);
}
namespace WiFiModel {
constexpr float sigma = 13.0;
/** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */
constexpr float TXP = -48;
constexpr float EXP = 2.5;
constexpr float WAF = -5.0;
// how to perform VAP grouping. see
// - calibration in Controller.cpp
// - eval in Filter.h
// NOTE: maybe the UAH does not allow valid VAP grouping? delete the grid and rebuild without!
const VAPGrouper vg_calib = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::AVERAGE);
}
namespace BeaconModel {
constexpr float sigma = 13.0;
constexpr float TXP = -48;
constexpr float EXP = 1.5;
constexpr float WAF = -8.0; //-5
}
namespace MapView3D {
const int maxColorPoints = 10000;
constexpr int fps = 15;
const Timestamp msPerFrame = Timestamp::fromMS(1000/fps);
}
namespace Filter {
const Timestamp updateEvery = Timestamp::fromMS(500);
constexpr bool useMainThread = false; // perform filtering in the main thread
}
}
#endif // SETTINGS_H

181
filter/Logic.h Normal file
View File

@@ -0,0 +1,181 @@
#ifndef FLOGIC_H
#define FLOGIC_H
#include <Indoor/grid/Grid.h>
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/GridWalkerMulti.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include "Structs.h"
#include <omp.h>
#include "../Settings.h"
/** particle-filter init */
struct PFInit : public K::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
PFInit(Grid<MyNode>& grid) : grid(grid) {;}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
for (K::Particle<MyState>& p : particles) {
int idx = rand() % grid.getNumNodes();
p.state.position = grid[idx]; // random position
p.state.heading.direction = (rand() % 360) / 180.0 * M_PI; // random heading
p.state.heading.error = 0;
p.state.relativePressure = 0; // start with a relative pressure of 0
}
}
};
/** particle-filter transition */
struct PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
Grid<MyNode>& grid;
GridWalker<MyNode, MyState> walker;
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
WalkModuleNodeImportance<MyNode, MyState> modImportance;
WalkModuleSpread<MyNode, MyState> modSpread;
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
//WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
std::minstd_rand gen;
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
walker.addModule(&modHead);
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modHeadUgly);
//walker.addModule(&modImportance);
//walker.addModule(&modFavorZ);
//walker.addModule(&modButterAct);
//walker.addModule(&modWifi);
//walker.addModule(&modPreventVisited);
}
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
for (K::Particle<MyState>& p : particles) {
// save old position
p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
// update steps
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
// update the particle in-place
p.state = walker.getDestination(grid, p.state, dist_m);
// update the baromter
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
p.state.relativePressure += deltaZ_cm * 0.105f;
}
}
};
struct PFEval : public K::ParticleFilterEvaluation<MyState, MyObs> {
WiFiModelLogDistCeiling& wifiModel;
//WiFiObserverFree wiFiProbability; // free-calculation
WiFiObserverGrid<MyNode> wiFiProbability; // grid-calculation
BeaconModelLogDistCeiling& beaconModel;
BeaconObserverFree beaconProbability;
Grid<MyNode>& grid;
PFEval(WiFiModelLogDistCeiling& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid<MyNode>& grid) :
wifiModel(wifiModel),
beaconModel(beaconModel),
grid(grid),
wiFiProbability(Settings::WiFiModel::sigma),
beaconProbability(Settings::BeaconModel::sigma, beaconModel){
}
/** probability for WIFI */
inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const {
const MyNode& node = grid.getNodeFor(point);
return wiFiProbability.getProbability(node, observation.currentTime, vapWifi);
}
/** probability for BEACONS */
inline double getBEACON(const MyObs& observation, const GridPoint& point){
return beaconProbability.getProbability(point.inMeter(), observation.currentTime, observation.beacons);
}
/** probability for Barometer */
inline double getBaroPressure(const MyObs& observation, const float hPa) const{
return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
}
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObs& observation) override {
double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
for (K::Particle<MyState>& p : particles) {
// Point3 pos_m = p.state.position.inMeter();
// Point3 posOld_m = p.state.positionOld.inMeter();
const double pWifi = getWIFI(observation, wifiObs, p.state.position);
const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
//small checks
_assertNotNAN(pWifi, "pups");
_assertNot0(pBaroPressure,"pBaroPressure is null");
const double prob = pWifi*pBaroPressure;
p.weight = prob;
sum += (prob);
}
if(sum == 0.0){
return 1.0;
}
return sum;
}
};
#endif // FLOGIC_H

111
filter/Structs.h Normal file
View File

@@ -0,0 +1,111 @@
#ifndef FSTRUCTS_H
#define FSTRUCTS_H
#include <Indoor/grid/Grid.h>
#include <Indoor/sensors/radio/WiFiGridNode.h>
#include <Indoor/math/Distributions.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/beacon/BeaconMeasurements.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
#include <Indoor/grid/factory/v2/GridNodeImportance.h>
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ {
static Floorplan::IndoorMap* map;
float relativePressure = 0;
GridPoint positionOld;
MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
MyState& operator += (const MyState& o) {
this->position += o.position;
return *this;
}
MyState& operator /= (const double d) {
this->position /= d;
return *this;
}
MyState operator * (const double d) const {
return MyState(this->position*d);
}
bool belongsToRegion(const MyState& o) const {
return position.inMeter().getDistance(o.position.inMeter()) < 3.0;
}
};
struct MyControl {
/** turn angle (in radians) since the last transition */
float turnSinceLastTransition_rad = 0;
/** number of steps since the last transition */
int numStepsSinceLastTransition = 0;
/** reset the control-data after each transition */
void resetAfterTransition() {
turnSinceLastTransition_rad = 0;
numStepsSinceLastTransition = 0;
}
};
struct MyObs {
/** relative pressure since t_0 */
float relativePressure = 0;
/** current estimated sigma for pressure sensor */
float sigmaPressure = 0.10f;
/** wifi measurements */
WiFiMeasurements wifi;
/** beacon measurements */
BeaconMeasurements beacons;
/** gps measurements */
//GPSData gps;
/** time of evaluation */
Timestamp currentTime;
};
struct MyNode : public GridPoint, public GridNode, public GridNodeImportance, public WiFiGridNode<20> {
float navImportance;
float getNavImportance() const { return navImportance; }
float walkImportance;
float getWalkImportance() const { return walkImportance; }
/** empty ctor */
MyNode() : GridPoint(-1, -1, -1) {;}
/** ctor */
MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
static void staticDeserialize(std::istream& inp) {
WiFiGridNode::staticDeserialize(inp);
}
static void staticSerialize(std::ostream& out) {
WiFiGridNode::staticSerialize(out);
}
};
#endif // FSTRUCTS_H

301
main.cpp Executable file
View File

@@ -0,0 +1,301 @@
#include "FileReader.h"
#include <iostream>
#include "filter/Structs.h"
#include "Plotti.h"
#include <chrono>
#include "filter/Logic.h"
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/geo/Point2.h>
#include <KLib/math/statistics/Statistics.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/FixedFrequencyInterpolator.h>
#include <Indoor/data/Timestamp.h>
#include "Settings.h"
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterHistory.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationKernelDensity.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/geo/Heading.h>
//frank
//const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
//const std::string dataDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/src/data/";
//toni
const std::string mapDir = "/home/toni/Documents/programme/localization/russenJournal/russen/map/";
const std::string dataDir = "/home/toni/Documents/programme/localization/russenJournal/russen/data/";
const std::string errorDir = dataDir + "results/";
/** describes one dataset (map, training, parameter-estimation, ...) */
struct DataSetup {
std::string map;
std::vector<std::string> training;
std::string wifiParams;
int minWifiOccurences;
VAPGrouper::Mode vapMode;
int buildingNum;
};
/** all configured datasets */
struct Data {
DataSetup SHL = {
mapDir + "SHL/SHL25.xml",
{
dataDir + "bergwerk/path1/nexus/vor/1454775984079.csv",
dataDir + "bergwerk/path1/galaxy/vor/1454776168794.csv",
dataDir + "bergwerk/path2/nexus/vor/1454779863041.csv",
dataDir + "bergwerk/path2/galaxy/vor/1454780113404.csv",
dataDir + "bergwerk/path3/nexus/vor/1454782562231.csv",
dataDir + "bergwerk/path3/galaxy/vor/1454782896548.csv",
dataDir + "bergwerk/path4/nexus/vor/1454776525797.csv",
dataDir + "bergwerk/path4/galaxy/vor/1454779020844.csv"
},
dataDir + "bergwerk/wifiParams.txt",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
10
};
} data;
Floorplan::IndoorMap* MyState::map;
void run(DataSetup setup, int numFile, std::string folder) {
// load the floorplan
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
MyState::map = map;
WiFiModelLogDistCeiling WiFiModel(map);
WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
BeaconModelLogDistCeiling beaconModel(map);
BeaconModelLogDistCeiling::APEntry beacon1(Point3(69.84f,45.26f,3.8f+3.4f+1.2f),-81,Settings::BeaconModel::EXP, Settings::BeaconModel::WAF);
beaconModel.addBeacon(MACAddress("48:EF:8D:77:66:DF"), beacon1);
BeaconModelLogDistCeiling::APEntry beacon2(Point3(69.84f,45.26f,3.8f+3.4f+1.2f),-81,Settings::BeaconModel::EXP, Settings::BeaconModel::WAF);
beaconModel.addBeacon(MACAddress("6F:5F:39:0C:51:E4"), beacon2);
BeaconModelLogDistCeiling::APEntry beacon3(Point3(69.84f,45.26f,3.8f+3.4f+1.2f),-81,Settings::BeaconModel::EXP, Settings::BeaconModel::WAF);
beaconModel.addBeacon(MACAddress("49:23:D8:7F:E8:D2"), beacon3);
//beaconModel.loadBeaconsFromMap(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(beaconModel.getAllBeacons().empty(), "no AccessPoints stored within the map.xml");
// build the grid
Grid<MyNode> grid(20);
GridFactory<MyNode> factory(grid);
factory.build(map);
// add node-importance
Importance::addImportance(grid);
// stamp WiFi signal-strengths onto the grid
WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround);
// reading file
FileReader fr(setup.training[numFile]);
//std::vector<int> gt;
// doing ground truth stuff
//fr.getGroundTruthPath(map, gt_1);
Plotti plot;
plot.addFloors(map);
plot.addOutline(map);
plot.addStairs(map);
plot.gp << "set autoscale xy\n";
//plot.addGrid(grid);
// init ctrl and observation
MyControl ctrl;
ctrl.resetAfterTransition();
MyObs obs;
int numParticles = 5000;
PFEval* eval = new PFEval(WiFiModel, beaconModel, grid);
//filter init
//std::unique_ptr<PFInit> init =
K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
pf.setEvaluation(std::unique_ptr<PFEval>(eval));
//resampling
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.04)));
pf.setNEffThreshold(0.85);
//estimation
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.95)));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationKernelDensity<MyState, 3>>(new K::ParticleFilterEstimationKernelDensity<MyState, 3>()));
Timestamp lastTimestamp = Timestamp::fromMS(0);
StepDetection sd;
TurnDetection td;
RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
K::Statistics<float> errorStats;
//file writing for offline competition
long int t = static_cast<long int>(time(NULL));
std::ofstream errorFile;
errorFile.open (errorDir + folder + "/error_" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");
// parse each sensor-value within the offline data
for (const FileReader::Entry& e : fr.getEntries()) {
const Timestamp ts = Timestamp::fromMS(e.ts);
if (e.type == FileReader::Sensor::WIFI) {
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == FileReader::Sensor::BEACON){
obs.beacons.entries.push_back(fr.getBeacons()[e.idx].data);
// remove to old beacon measurements
obs.beacons.removeOld(ts);
} else if (e.type == FileReader::Sensor::ACC) {
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
++ctrl.numStepsSinceLastTransition;
}
const FileReader::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
td.addAccelerometer(ts, _acc.data);
} else if (e.type == FileReader::Sensor::GYRO) {
const FileReader::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
const float delta = td.addGyroscope(ts, _gyr.data);
ctrl.turnSinceLastTransition_rad += delta;
} else if (e.type == FileReader::Sensor::BARO) {
relBaro.add(ts, fr.getBarometer()[e.idx].data);
obs.relativePressure = relBaro.getPressureRealtiveToStart();
obs.sigmaPressure = relBaro.getSigma();
}
if (ts.ms() - lastTimestamp.ms() > 500) {
obs.currentTime = ts;
MyState est = pf.update(&ctrl, obs);
Point3 estPos = est.position.inMeter();
plot.pInterest.clear();
// plotting stuff
static float angleSum = 0; angleSum += ctrl.turnSinceLastTransition_rad;
//plot.showAngle(1, ctrl.turnAngle);
plot.showAngle(2, angleSum + M_PI);
//plot.debugWiFi(eval->model, obs.wifis, obs.curTS);
//plot.debugProb(grid, std::bind(&PFEval::getGPS, eval, std::placeholders::_1, std::placeholders::_2), obs);
//plot.debugProb(grid, std::bind(&PFEval::getWIFI, eval, std::placeholders::_1, std::placeholders::_2), obs);
//plot.debugProb(grid, std::bind(&PFEval::getALL, eval, std::placeholders::_1, std::placeholders::_2), obs);
plot.setEst(estPos);
//plot.setGT(mapPos);
plot.addEstimationNode(estPos);
plot.addParticles(pf.getParticles());
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
//plot.gp << "set label 1001 at screen 0.02, 0.98 'base:" << relBaro.getBaseAvg() << " sigma:" << relBaro.getSigma() << " cur:" << relBaro.getPressureRealtiveToStart() << " hPa " << -relBaro.getPressureRealtiveToStart()/0.10/4.0f << " floor'\n";
int minutes = static_cast<int>(ts.sec()) / 60;
plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast<int>(static_cast<int>(ts.sec())%60) << "'\n";
//plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << ctrl.barometer.act << "'\n";
// error between GT and estimation
//float err_m = mapPos.getDistance(estPos);
// errorStats.add(err_m);
//errorFile << err_m << "\n";
plot.show();
usleep(100*10);
lastTimestamp = ts;
// reset control
ctrl.resetAfterTransition();
}
}
errorFile.close();
//Write the current plotti buffer into file
std::ofstream plotFile;
plotFile.open(errorDir + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
// for(int i = 0; i < map->floors.size(); ++i){
// plot.printSingleFloor("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i);
// plot.show();
// usleep(1000*10);
// }
// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90);
// plot.show();
// plot.printSideView("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0);
// plot.show();
// plot.printOverview("/home/toni/Documents/programme/localization/IPIN2016/competition/eval/"+ folder + "/image" + std::to_string(numFile) + "_" + std::to_string(t));
// plot.show();
sleep(1);
}
int main(int argc, char** argv) {
//Testing files
run(data.SHL, 6, "EVAL"); // Nexus vor
}

39
notes.txt Normal file
View File

@@ -0,0 +1,39 @@
some minor tests on one dataset
Optimizing WiFi Parameter
Remove Strong outliers during error-optimization:
Does NOT seem to help at all!
While wifi-opt is better, overwall result is worst
0% out: med(3.85227) avg(5.04607)
1% out: med(3.84351) avg(5.10242)
3% out: med(3.84224) avg(5.19669)
med(3.84321) avg(5.17647)
5% out: med(4.00438) avg(5.28421)
10% out: med(4.02074) avg(5.43715)
WiFiProbability
NormalDist: med(3.86891) avg(5.42092)
Region: med(3.90684) avg(5.16647)
Step Length:
70cm: med(4.0231) avg(5.52792)
80cm: med(4.01928) avg(5.30433)
85cm: med(3.85938) avg(5.10636)
90cm: med(3.87452) avg(5.06212)
Map/NonMap
NonMap: med(3.7643) avg(5.20786)
with Map: med(4.53366) avg(6.66799)