From 48566b79fcd9049cd20c7a1070134fcfe16e6596 Mon Sep 17 00:00:00 2001 From: toni Date: Fri, 10 Mar 2017 14:25:48 +0100 Subject: [PATCH] added code --- code/CMakeLists.txt | 94 +++ code/Plotti.h | 275 ++++++ code/Settings.h | 69 ++ code/filter/Logic.h | 216 +++++ code/filter/Structs.h | 116 +++ code/main.cpp | 357 ++++++++ code/map/SHL/SHL25.xml | 1332 ++++++++++++++++++++++++++++++ code/map/SHL/SHL_IPIN2015_gt.xml | 1309 +++++++++++++++++++++++++++++ 8 files changed, 3768 insertions(+) create mode 100755 code/CMakeLists.txt create mode 100644 code/Plotti.h create mode 100644 code/Settings.h create mode 100644 code/filter/Logic.h create mode 100644 code/filter/Structs.h create mode 100755 code/main.cpp create mode 100644 code/map/SHL/SHL25.xml create mode 100644 code/map/SHL/SHL_IPIN2015_gt.xml diff --git a/code/CMakeLists.txt b/code/CMakeLists.txt new file mode 100755 index 0000000..3d69b24 --- /dev/null +++ b/code/CMakeLists.txt @@ -0,0 +1,94 @@ +# Usage: +# Create build folder, like RC-build next to RobotControl and WifiScan folder +# CD into build folder and execute 'cmake -DCMAKE_BUILD_TYPE=Debug ../RobotControl' +# make + +CMAKE_MINIMUM_REQUIRED(VERSION 2.8) + +# select build type +SET( CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" ) + +PROJECT(IPIN2017) + +IF(NOT CMAKE_BUILD_TYPE) + MESSAGE(STATUS "No build type selected. Default to Debug") + SET(CMAKE_BUILD_TYPE "Debug") +ENDIF() + + + +INCLUDE_DIRECTORIES( + ../ + ../../ + ../../../ + ../../../../ +) + + +FILE(GLOB HEADERS + ./notes.txt + ./*.h + ./*/*.h + ./*/*/*.h + ./*/*/*/*.h + ./*/*/*/*/*.h + ./*/*/*/*/*/*.h +) + + +FILE(GLOB SOURCES + ./*.cpp + ./*/*.cpp + ./*/*/*.cpp + ./*/*/*/*.cpp + ../../Indoor/lib/tinyxml/tinyxml2.cpp +) + + +# system specific compiler flags +ADD_DEFINITIONS( + + -std=gnu++11 + + -Wall + -Werror=return-type + -Wextra + -Wpedantic + + -fstack-protector-all + + -g3 + #-O2 + -march=native + + -DWITH_TESTS + -DWITH_ASSERTIONS + -DWITH_DEBUG_LOG + + +) + +# allow OMP +find_package(OpenMP) +if (OPENMP_FOUND) + set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + + +# build a binary file +ADD_EXECUTABLE( + ${PROJECT_NAME} + ${HEADERS} + ${SOURCES} +) + +# needed external libraries +TARGET_LINK_LIBRARIES( + ${PROJECT_NAME} + gtest + pthread +) + +SET(CMAKE_C_COMPILER ${CMAKE_CXX_COMPILER}) + diff --git a/code/Plotti.h b/code/Plotti.h new file mode 100644 index 0000000..a295350 --- /dev/null +++ b/code/Plotti.h @@ -0,0 +1,275 @@ +#ifndef PLOTTI_H +#define PLOTTI_H + +#include "filter/Structs.h" +#include "Settings.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, const Point2 cen, const std::string& str) { + + Point2 rot(0, 1); + Point2 pos = cen + rot.rotated(rad) * 0.05; + + gp << "set label "< max) {max = prob;} + pColorPoints.add(K::GnuplotPoint3(x,y,z), prob); + } + } + + if (min == max) {min -= 1;} + gp << "set cbrange [" << min << ":" << max << "]\n"; + + } + + template 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/code/Settings.h b/code/Settings.h new file mode 100644 index 0000000..8a270fb --- /dev/null +++ b/code/Settings.h @@ -0,0 +1,69 @@ +#ifndef SETTINGS_H +#define SETTINGS_H + +#include +#include +#include + +namespace Settings { + + const int numParticles = 10000; + + namespace IMU { + const float turnSigma = 2.5; // 3.5 + const float stepLength = 1.00; + const float stepSigma = 0.15; //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 = 8.0; + /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ + constexpr float TXP = -46; + constexpr float EXP = 2.7; + 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 = 8.0; + constexpr float TXP = -71; + constexpr float EXP = 1.5; + constexpr float WAF = -20.0; //-5 //20?? + } + + 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/code/filter/Logic.h b/code/filter/Logic.h new file mode 100644 index 0000000..5e686ed --- /dev/null +++ b/code/filter/Logic.h @@ -0,0 +1,216 @@ +#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 + +#include "Structs.h" +#include +#include "../Settings.h" + + +/** particle-filter init randomly distributed within the building*/ +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 init with fixed position*/ +struct PFInitFixed : public K::ParticleFilterInitializer { + + Grid& grid; + GridPoint startPos; + float headingDeg; + + PFInitFixed(Grid& grid, GridPoint startPos, float headingDeg) : + grid(grid), startPos(startPos), headingDeg(headingDeg) {;} + + virtual void initialize(std::vector>& particles) override { + + Distribution::Normal norm(0.0f, 1.5f); + + for (K::Particle& p : particles) { + + GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f); + + GridPoint startPos = grid.getNodeFor(pos); + p.state.position = startPos; // scatter arround the start position + p.state.heading.direction = headingDeg / 180.0 * M_PI; // fixed 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; + WalkModuleHeadingVonMises modHeadMises; + WalkModuleNodeImportance modImportance; + WalkModuleSpread modSpread; + WalkModuleFavorZ modFavorZ; + //WalkModulePreventVisited modPreventVisited; + + std::minstd_rand gen; + + + PFTrans(Grid& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) { + + walker.addModule(&modHead); + //walker.addModule(&modHeadMises); + //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, wifiModel), + 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){ + + //consider adding the persons height + Point3 p = point.inMeter() + Point3(0,0,1.3); + return beaconProbability.getProbability(p, 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); + const double pBeacon = getBEACON(observation, p.state.position); + + //small checks + _assertNotNAN(pWifi, "Wifi prob is nan"); + _assertNot0(pBaroPressure,"pBaroPressure is null"); + + const double prob = pBaroPressure * pBeacon * pWifi; + + p.weight = prob; + sum += (prob); + + } + + if(sum == 0.0){ + return 1.0; + } + + return sum; + + } + +}; + +#endif // FLOGIC_H diff --git a/code/filter/Structs.h b/code/filter/Structs.h new file mode 100644 index 0000000..b632edc --- /dev/null +++ b/code/filter/Structs.h @@ -0,0 +1,116 @@ + +#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.0f; + + 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; + + /** current motion delta angle*/ + float motionDeltaAngle_rad = 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/code/main.cpp b/code/main.cpp new file mode 100755 index 0000000..da4bc70 --- /dev/null +++ b/code/main.cpp @@ -0,0 +1,357 @@ +#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 +#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; + std::string grid; +}; + +/** all configured datasets */ +struct Data { + + DataSetup BERKWERK = { + + 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, + mapDir + "SHL/grid25.dat" + }; + + DataSetup IPIN2015 = { + + mapDir + "SHL/SHL_IPIN2015_gt.xml", + + { + dataDir + "ipin2015/galaxy/Path0/1433581471902.csv", + dataDir + "ipin2015/nexus/Path0/1433606195078.csv", + dataDir + "ipin2015/galaxy/Path1/1433587749492.csv", + dataDir + "ipin2015/nexus/Path1/1433606670723.csv", + dataDir + "ipin2015/galaxy/Path2/1433581471902.csv", + dataDir + "ipin2015/nexus/Path2/1433607251262.csv", + dataDir + "eiszeit/path2/1479986737368.csv" + }, + + dataDir + "bergwerk/wifiParams.txt", + 40, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, + mapDir + "SHL/grid_IPIN2015_gt.dat" + }; + +} 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); + beaconModel.loadBeaconsFromMap(map, Settings::BeaconModel::TXP, Settings::BeaconModel::EXP, Settings::BeaconModel::WAF); + Assert::isFalse(beaconModel.getAllBeacons().empty(), "no Beacons stored within the map.xml"); + + + // build the grid + std::ifstream inp(setup.grid, std::ifstream::binary); + Grid grid(20); + + // grid.dat empty? -> build one and save it + if (!inp.good() || (inp.peek()&&0) || inp.eof()) { + std::ofstream onp; + onp.open(setup.grid); + GridFactory factory(grid); + factory.build(map); + grid.write(onp); + } else { + grid.read(inp); + } + + + // 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]); + + // doing ground truth stuff + std::vector path_0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0}; + std::vector path_1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0}; + std::vector path_2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29}; + Interpolator gtInterpolator = fr.getGroundTruthPath(map, path_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; + + //random start position + //std::unique_ptr> init(new PFInit(grid)); std::move(init); + + //filter init + //std::unique_ptr init = + //K::ParticleFilterHistory pf(numParticles, std::unique_ptr(new PFInit(grid))); + K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 1080.0f), 90.0f))); + pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); + pf.setEvaluation(std::unique_ptr(new PFEval(WiFiModel, beaconModel, grid))); + + //resampling + pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); + //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.4))); + pf.setNEffThreshold(0.95); + + //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; + MotionDetection md; + + RelativePressure relBaro; relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); + + K::Statistics errorStats; + + //file writing for error data + 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_gyro = td.addGyroscope(ts, _gyr.data); + + ctrl.turnSinceLastTransition_rad += delta_gyro; + + } else if (e.type == FileReader::Sensor::BARO) { + relBaro.add(ts, fr.getBarometer()[e.idx].data); + obs.relativePressure = relBaro.getPressureRealtiveToStart(); + obs.sigmaPressure = relBaro.getSigma(); + + } else if (e.type == FileReader::Sensor::LIN_ACC) { + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + + } else if (e.type == FileReader::Sensor::GRAVITY) { + md.addGravity(ts, fr.getGravity()[e.idx].data); + Eigen::Vector2f curVec = md.getCurrentMotionAxis(); + ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad(); + } + + if (ts.ms() - lastTimestamp.ms() > 500) { + + obs.currentTime = ts; + + MyState est = pf.update(&ctrl, obs); + Point3 estPos = est.position.inMeter(); + + //current ground truth position + Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())); + + + /** plotting stuff */ + + plot.pInterest.clear(); + + //turn angle plot + static float angleSumTurn = 0; angleSumTurn += ctrl.turnSinceLastTransition_rad; + plot.showAngle(1, angleSumTurn + M_PI, Point2(0.9, 0.9), "Turn: "); + + //motion angle plot + static float angleSumMotion = 0; angleSumMotion += ctrl.motionDeltaAngle_rad; + plot.showAngle(2, angleSumMotion + M_PI, Point2(0.9, 0.8), "Motion: "); + + //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(gtPos); + plot.addEstimationNode(estPos); + plot.addParticles(pf.getParticles()); + + plot.pColorPoints.clear(); + plot.debugWiFi(WiFiModel, obs.wifi, ts, 10.8f); //3 floor + plot.debugWiFi(WiFiModel, obs.wifi, ts, 7.4f); //2 floor + plot.debugWiFi(WiFiModel, obs.wifi, ts, 4.0f); //1 floor + plot.debugWiFi(WiFiModel, obs.wifi, ts, 0.0f); //0 floor + + //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 = gtPos.getDistance(estPos); + errorStats.add(err_m); + errorFile << err_m << "\n"; + + plot.show(); + usleep(10*10); + + + lastTimestamp = ts; + + // reset control + ctrl.resetAfterTransition(); + } + + } + + errorFile.close(); + + std::cout << "Statistical Analysis: " << std::endl; + std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << std::endl; + + //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.BERKWERK, 6, "EVALBERGWERK"); // Nexus vor + + //for(int i = 0; i < 5; ++i){ + run(data.IPIN2015, 3, "EVALIPIN2015"); // Nexus Path2 + //} + +} diff --git a/code/map/SHL/SHL25.xml b/code/map/SHL/SHL25.xml new file mode 100644 index 0000000..c1894d8 --- /dev/null +++ b/code/map/SHL/SHL25.xml @@ -0,0 +1,1332 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/code/map/SHL/SHL_IPIN2015_gt.xml b/code/map/SHL/SHL_IPIN2015_gt.xml new file mode 100644 index 0000000..01546fb --- /dev/null +++ b/code/map/SHL/SHL_IPIN2015_gt.xml @@ -0,0 +1,1309 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +