From 1adb74ec299340ec066ea8beab2f0b218add7135 Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 16 Nov 2016 12:21:39 +0100 Subject: [PATCH] init --- FileReader.h | 252 +++++++++++++++++++++++++++++++++++++++ Plotti.h | 250 +++++++++++++++++++++++++++++++++++++++ Settings.h | 69 +++++++++++ filter/Logic.h | 181 ++++++++++++++++++++++++++++ filter/Structs.h | 111 +++++++++++++++++ main.cpp | 301 +++++++++++++++++++++++++++++++++++++++++++++++ notes.txt | 39 ++++++ 7 files changed, 1203 insertions(+) create mode 100644 FileReader.h create mode 100644 Plotti.h create mode 100644 Settings.h create mode 100644 filter/Logic.h create mode 100644 filter/Structs.h create mode 100755 main.cpp create mode 100644 notes.txt diff --git a/FileReader.h b/FileReader.h new file mode 100644 index 0000000..6b10c89 --- /dev/null +++ b/FileReader.h @@ -0,0 +1,252 @@ +#ifndef FILEREADER_H +#define FILEREADER_H + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +class FileReader { + +public: + + template 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> groundTruth; + std::vector> wifi; + std::vector> beacon; + std::vector> acc; + std::vector> gyro; + std::vector> barometer; + + /** ALL entries */ + std::vector entries; + +public: + + FileReader(const std::string& file) { + parse(file); + } + + const std::vector& getEntries() const {return entries;} + + + const std::vector>& getGroundTruth() const {return groundTruth;} + + const std::vector>& getWiFiGroupedByTime() const {return wifi;} + + const std::vector>& getBeacons() const {return beacon;} + + const std::vector>& getAccelerometer() const {return acc;} + + const std::vector>& getGyroscope() const {return gyro;} + + const std::vector>& 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 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 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(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 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 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 elem(ts, BarometerData(std::stof(hPa))); + barometer.push_back(elem); + entries.push_back(Entry(Sensor::BARO, ts, barometer.size()-1)); + + } + + const Interpolator getGroundTruthPath(Floorplan::IndoorMap& map, std::vector gtPath) const { + + // finde alle positionen der waypoints im gtPath aus map + std::unordered_map 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 waypointsApp; + for(TS val : groundTruth){ + waypointsApp.insert({val.data, val.ts}); + } + + // bringe diese in richtige reihenfolge und füge timestamp hinzu + Interpolator 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 diff --git a/Plotti.h b/Plotti.h new file mode 100644 index 0000000..0f86ab0 --- /dev/null +++ b/Plotti.h @@ -0,0 +1,250 @@ +#ifndef PLOTTI_H +#define PLOTTI_H + +#include "filter/Structs.h" + + +#include + +#include +#include + +#include + +#include +#include +#include + + +#include +#include +#include +#include +#include + +#include + +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(>Path); 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 "< void debugProb(Grid& grid, std::function 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 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 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(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 void addGrid(Grid& 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 void addParticles(const std::vector>& particles) { + pParticles.clear(); + int i = 0; + for (const K::Particle& 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 diff --git a/Settings.h b/Settings.h new file mode 100644 index 0000000..85b4bc7 --- /dev/null +++ b/Settings.h @@ -0,0 +1,69 @@ +#ifndef SETTINGS_H +#define SETTINGS_H + +#include +#include +#include + +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 diff --git a/filter/Logic.h b/filter/Logic.h new file mode 100644 index 0000000..2873668 --- /dev/null +++ b/filter/Logic.h @@ -0,0 +1,181 @@ +#ifndef FLOGIC_H +#define FLOGIC_H + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "Structs.h" +#include +#include "../Settings.h" + + +/** particle-filter init */ +struct PFInit : public K::ParticleFilterInitializer { + + Grid& grid; + + PFInit(Grid& grid) : grid(grid) {;} + + virtual void initialize(std::vector>& particles) override { + for (K::Particle& 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 { + + Grid& grid; + + GridWalker walker; + + WalkModuleHeading modHeadUgly; // stupid + WalkModuleHeadingControl modHead; + WalkModuleNodeImportance modImportance; + WalkModuleSpread modSpread; + WalkModuleFavorZ modFavorZ; + //WalkModulePreventVisited modPreventVisited; + + std::minstd_rand gen; + + + PFTrans(Grid& 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>& particles, const MyControl* control) override { + + std::normal_distribution noise(0, Settings::IMU::stepSigma); + + for (K::Particle& 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 { + + WiFiModelLogDistCeiling& wifiModel; + //WiFiObserverFree wiFiProbability; // free-calculation + WiFiObserverGrid wiFiProbability; // grid-calculation + + BeaconModelLogDistCeiling& beaconModel; + BeaconObserverFree beaconProbability; + + Grid& grid; + + PFEval(WiFiModelLogDistCeiling& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid& 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::getProbability(static_cast(hPa), 0.10, static_cast(observation.relativePressure)); + } + + virtual double evaluation(std::vector>& particles, const MyObs& observation) override { + + double sum = 0; + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); + + for (K::Particle& 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 diff --git a/filter/Structs.h b/filter/Structs.h new file mode 100644 index 0000000..3b3282a --- /dev/null +++ b/filter/Structs.h @@ -0,0 +1,111 @@ + +#ifndef FSTRUCTS_H +#define FSTRUCTS_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +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 diff --git a/main.cpp b/main.cpp new file mode 100755 index 0000000..27bd53d --- /dev/null +++ b/main.cpp @@ -0,0 +1,301 @@ +#include "FileReader.h" +#include + +#include "filter/Structs.h" +#include "Plotti.h" + +#include + +#include "filter/Logic.h" + +#include +#include + +#include +#include + +#include + + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "Settings.h" + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + + +#include + +//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 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 grid(20); + GridFactory 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 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 init = + K::ParticleFilterHistory pf(numParticles, std::unique_ptr(new PFInit(grid))); + pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); + pf.setEvaluation(std::unique_ptr(eval)); + + //resampling + //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); + pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.04))); + pf.setNEffThreshold(0.85); + + //estimation + //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); + pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.95))); + //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationKernelDensity())); + + + Timestamp lastTimestamp = Timestamp::fromMS(0); + + StepDetection sd; + TurnDetection td; + RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); + + K::Statistics errorStats; + + //file writing for offline competition + long int t = static_cast(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& _acc = fr.getAccelerometer()[e.idx]; + td.addAccelerometer(ts, _acc.data); + + } else if (e.type == FileReader::Sensor::GYRO) { + const FileReader::TS& _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(ts.sec()) / 60; + plot.gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << static_cast(static_cast(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 + + +} diff --git a/notes.txt b/notes.txt new file mode 100644 index 0000000..e63e6c5 --- /dev/null +++ b/notes.txt @@ -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) +