From ba3b75874424d653aaa50061a4807356e240e086 Mon Sep 17 00:00:00 2001 From: toni Date: Thu, 12 Oct 2017 13:20:39 +0200 Subject: [PATCH] first commit init project --- CMakeLists.txt | 94 ++++++++ Plotti.h | 352 ++++++++++++++++++++++++++++++ Settings.h | 88 ++++++++ filter/KLB.h | 180 +++++++++++++++ filter/Logic.h | 554 +++++++++++++++++++++++++++++++++++++++++++++++ filter/Structs.h | 132 +++++++++++ main.cpp | 424 ++++++++++++++++++++++++++++++++++++ 7 files changed, 1824 insertions(+) create mode 100755 CMakeLists.txt create mode 100644 Plotti.h create mode 100644 Settings.h create mode 100644 filter/KLB.h create mode 100644 filter/Logic.h create mode 100644 filter/Structs.h create mode 100755 main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..d6073f4 --- /dev/null +++ b/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(Museum) + +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/Plotti.h b/Plotti.h new file mode 100644 index 0000000..64129a1 --- /dev/null +++ b/Plotti.h @@ -0,0 +1,352 @@ +#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::GnuplotSplotElementPoints pNormal1; + K::GnuplotSplotElementPoints pNormal2; + K::GnuplotSplotElementColorPoints pDistributation1; + K::GnuplotSplotElementColorPoints pDistributation2; + 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.getColor().setHexStr("#888888"); + splot.add(&pAPs); pAPs.setPointSize(0.7); + splot.add(&pColorPoints); pColorPoints.setPointSize(0.6); + splot.add(&pDistributation1); pDistributation1.setPointSize(0.6); + splot.add(&pDistributation2); pDistributation2.setPointSize(0.6); + splot.add(&pParticles); pParticles.getColor().setHexStr("#0000ff"); pParticles.setPointSize(0.4f); + splot.add(&pNormal1); pNormal1.getColor().setHexStr("#ff00ff"); pNormal1.setPointSize(0.4f); + splot.add(&pNormal2); pNormal2.getColor().setHexStr("#00aaff"); pNormal2.setPointSize(0.4f); + splot.add(&pFloor); + splot.add(&pOutline); pOutline.getStroke().getColor().setHexStr("#999999"); + splot.add(&pStairs); pStairs.getStroke().getColor().setHexStr("#000000"); + splot.add(&pInterest); pInterest.setPointSize(2); pInterest.getColor().setHexStr("#ff0000"); + splot.add(>Path); gtPath.getStroke().setWidth(2); gtPath.getStroke().getColor().setHexStr("#000000"); + splot.add(&estPath); estPath.getStroke().setWidth(2); estPath.getStroke().getColor().setHexStr("#00ff00"); + splot.add(&estPathSmoothed); estPathSmoothed.getStroke().setWidth(2); estPathSmoothed.getStroke().getColor().setHexStr("#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 "<> samples){ + + float min = +9999; + float max = -9999; + + pDistributation1.clear(); + for (int i = 0; i < samples.size(); ++i) { + //if (i % 10 != 0) {continue;} + + double prob = samples[i].weight; + if (prob < min) {min = prob;} + if (prob > max) {max = prob;} + + K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f); + pDistributation1.add(pos, prob); + } + + if (min == max) {min -= 1;} + gp << "set cbrange [" << min << ":" << max << "]\n"; + } + + void debugDistribution2(std::vector> samples){ + + float min = +9999; + float max = -9999; + + pDistributation2.clear(); + for (int i = 0; i < samples.size(); ++i) { + if (i % 25 != 0) {continue;} + + double prob = samples[i].weight; + if (prob < min) {min = prob;} + if (prob > max) {max = prob;} + + K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f); + pDistributation2.add(pos, prob); + } + + if (min == max) {min -= 1;} + gp << "set cbrange [" << min << ":" << max << "]\n"; + } + + void drawNormalN1(Distribution::NormalDistributionN normParticle){ + + pNormal1.clear(); + for (int i = 0; i < 100000; ++i) { + if (++i % 25 != 0) {continue;} + Eigen::VectorXd vec = normParticle.draw(); + K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z()); + pNormal1.add(pos); + } + + } + + void drawNormalN2(Distribution::NormalDistributionN normParticle){ + + pNormal2.clear(); + for (int i = 0; i < 100000; ++i) { + if (++i % 25 != 0) {continue;} + Eigen::VectorXd vec = normParticle.draw(); + K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z()); + pNormal2.add(pos); + } + } + + void debugWiFi(WiFiModelLogDistCeiling& model, const WiFiMeasurements& scan, const Timestamp curTS, const float z) { + + WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model); + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(scan); + + float min = +9999; + float max = -9999; + + const float step = 2.0f; + for (float x = 0; x < 80; x += step) { + for (float y = 0; y < 55; y += step) { + Point3 pt(x,y,z); + double prob = wiFiProbability.getProbability(pt + Point3(0,0,1.3), curTS, wifiObs); + + if (prob < min) {min = prob;} + if (prob > 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/Settings.h b/Settings.h new file mode 100644 index 0000000..2d519f5 --- /dev/null +++ b/Settings.h @@ -0,0 +1,88 @@ +#ifndef SETTINGS_H +#define SETTINGS_H + +#include +#include +#include + +namespace Settings { + + bool useKLB = false; + + const int numParticles = 6000; + const int numBSParticles = 50; + + 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; + } + + namespace Smoothing { + const bool activated = false; + const double stepLength = 0.7; + const double stepSigma = 0.2; + const double headingSigma = 25.0; + const double zChange = 0.0; // mu change in height between two time steps + const double zSigma = 0.1; + const int lag = 5; + } + + + //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 = -40; + constexpr float EXP = 2.3; + constexpr float WAF = 0.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::MAXIMUM, 1); + const VAPGrouper vg_eval = VAPGrouper(VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, VAPGrouper::Aggregation::MAXIMUM, 1); + } + + 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 = 1000; + 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 + } + + namespace Path_DongleTest { + const std::vector path1 = {0, 1, 2, 3, 4, 5, 6}; + const std::vector path2 = {6, 5, 4, 7, 8, 9, 8, 10}; + const std::vector path3 = {10, 8, 7, 4, 11, 12, 13, 14, 6}; + } + +} + +#endif // SETTINGS_H diff --git a/filter/KLB.h b/filter/KLB.h new file mode 100644 index 0000000..555b7f5 --- /dev/null +++ b/filter/KLB.h @@ -0,0 +1,180 @@ +#ifndef KLB_H +#define KLB_H + +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +//#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "Structs.h" + +#include "../Plotti.h" +#include "Logic.h" +#include "../Settings.h" + +static double getKernelDensityProbability(std::vector>& particles, MyState state, std::vector>& samplesWifi){ + + Distribution::KernelDensity parzen([&](MyState state){ + int size = particles.size(); + double prob = 0; + + #pragma omp parallel for reduction(+:prob) num_threads(6) + for(int i = 0; i < size; ++i){ + double distance = particles[i].state.position.getDistanceInCM(state.position); + prob += Distribution::Normal::getProbability(0, 100, distance) * particles[i].weight; + } + + return prob; + ;}); + + std::vector probsWifiV; + std::vector probsParticleV; + + //just for plottingstuff + std::vector> samplesParticles; + + const int step = 4; + int i = 0; + for(K::Particle particle : samplesWifi){ + if(++i % step != 0){continue;} + MyState state(GridPoint(particle.state.position.x_cm, particle.state.position.y_cm, particle.state.position.z_cm)); + + double probiParticle = parzen.getProbability(state); + probsParticleV.push_back(probiParticle); + + double probiwifi = particle.weight; + probsWifiV.push_back(probiwifi); + + //samplesParticles.push_back(K::Particle(state, probiParticle)); + } + + //make vectors + Eigen::Map probsWifi(&probsWifiV[0], probsWifiV.size()); + Eigen::Map probsParticle(&probsParticleV[0], probsParticleV.size()); + + //get divergence + double kld = Divergence::KullbackLeibler::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS); + //double kld = Divergence::JensenShannon::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS); + + //plotti + //plot.debugDistribution1(samplesWifi); + //plot.debugDistribution1(samplesParticles); + + + //estimate the mean +// K::ParticleFilterEstimationOrderedWeightedAverage estimateWifi(0.95); +// const MyState estWifi = estimateWifi.estimate(samplesWifi); +// plot.addEstimationNodeSmoothed(estWifi.position.inMeter()); + + return kld; +} + + +static double kldFromMultivariatNormal(std::vector>& particles, MyState state, std::vector>& particleWifi){ + //kld: particle die resampling hatten nehmen und nv daraus schätzen. vergleiche mit wi-fi + //todo put this in depletionhelper.h + + Point3 estPos = state.position.inMeter(); + + //this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and + //no inverse matrix is possible + std::mt19937_64 rng; + // initialize the random number generator with time-dependent seed + uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count(); + std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)}; + rng.seed(ss); + // initialize a uniform distribution between -0.0001 and 0.0001 + std::uniform_real_distribution unif(-0.0001, 0.0001); + + //create a gauss dist for the current particle approx. + Eigen::MatrixXd m(particles.size(), 3); + for(int i = 0; i < particles.size(); ++i){ + m(i,0) = (particles[i].state.position.x_cm / 100.0) + unif(rng); + m(i,1) = (particles[i].state.position.y_cm / 100.0) + unif(rng); + m(i,2) = (particles[i].state.position.z_cm / 100.0) + unif(rng); + } + + Eigen::VectorXd mean(3); + mean << estPos.x, estPos.y, estPos.z; + + Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(m, mean); + + //create a gauss dist for wifi + Eigen::MatrixXd covWifi(3,3); + covWifi << Settings::WiFiModel::sigma, 0, 0, + 0, Settings::WiFiModel::sigma, 0, + 0, 0, 0.01; + + //estimate the mean + K::ParticleFilterEstimationOrderedWeightedAverage estimateWifi(0.95); + const MyState estWifi = estimateWifi.estimate(particleWifi); + + Eigen::VectorXd meanWifi(3); + meanWifi << estWifi.position.x_cm / 100.0, estWifi.position.y_cm / 100.0, estWifi.position.z_cm / 100.0; + Distribution::NormalDistributionN normWifi(meanWifi, covWifi); + + //get the kld distance + double kld = Divergence::KullbackLeibler::getMultivariateGauss(normParticle, normWifi); + + //plot.debugDistribution1(particleWifi); + + //plot.drawNormalN1(normParticle); + //plot.drawNormalN2(normWifi); + + //plot.addEstimationNodeSmoothed(estWifi.position.inMeter()); + + return kld; +} + + + +#endif // KLB_H diff --git a/filter/Logic.h b/filter/Logic.h new file mode 100644 index 0000000..5830568 --- /dev/null +++ b/filter/Logic.h @@ -0,0 +1,554 @@ +#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 +#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 + p.weight = 1.0 / particles.size(); // equal weight + + } + } + +}; + +/** 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 + p.weight = 1.0 / particles.size(); // equal weight + } + } + +}; + +/** very simple transition model, just scatter normal distributed */ +struct PFTransSimple : public K::ParticleFilterTransition{ + + Grid& grid; + + // define the noise + Distribution::Normal noise_cm = Distribution::Normal(0.0, Settings::IMU::stepLength * 2.0 * 100.0); + Distribution::Normal height_m = Distribution::Normal(0.0, 6.0); + + // draw randomly from a vector + //random_selector<> rand; + + // draw from 0 - 1 + Distribution::Uniform uniRand = Distribution::Uniform(0,1); + + /** ctor */ + PFTransSimple(Grid& grid) : grid(grid) {} + + virtual void transition(std::vector>& particles, const MyControl* control) override { + + //int noNewPositionCounter = 0; + + #pragma omp parallel for num_threads(6) + for (int i = 0; i < particles.size(); ++i) { + K::Particle& p = particles[i]; + + // update the baromter + float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z; + p.state.relativePressure += deltaZ_cm * 0.105f; + + double diffHeight = p.state.position.inMeter().z + height_m.draw(); + double newHeight_cm = p.state.position.z_cm; + if(diffHeight > 9.1){ + newHeight_cm = 10.8 * 100.0; + } else if (diffHeight < 9.1 && diffHeight > 5.7){ + newHeight_cm = 7.4 * 100.0; + } else if (diffHeight < 5.7 && diffHeight > 2.0) { + newHeight_cm = 4.0 * 100.0; + } else { + newHeight_cm = 0.0; + } + + GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), 0.0); + GridPoint newPosition = p.state.position + noisePt; + newPosition.z_cm = newHeight_cm; + +// p.state.position = grid.getNearestNode(newPosition); + + if(grid.hasNodeFor(newPosition)){ + p.state.position = newPosition; + }else{ + //no new position! +// #pragma omp atomic +// noNewPositionCounter++; + } + + } + +// std::cout << noNewPositionCounter << std::endl; + } +}; + +/** 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; + + //WalkModuleActivityControl modActivity; + + 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(&modActivity); + //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) { + + //this is just for the smoothing transition... quick and dirty + p.state.headingChangeMeasured_rad = control->turnSinceLastTransition_rad; + + // 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; + + } + } + +}; + +/** +* particle-filter transition +* Adapting the Sample Size in Particle Filters Through KLD-Sampling - D. Fox +*/ +struct PFTransKLDSampling : public K::ParticleFilterTransition { + + Grid& grid; + + GridWalker walker; + + WalkModuleHeading modHeadUgly; // stupid + WalkModuleHeadingControl modHead; + WalkModuleHeadingVonMises modHeadMises; + WalkModuleNodeImportance modImportance; + WalkModuleSpread modSpread; + WalkModuleFavorZ modFavorZ; + //WalkModulePreventVisited modPreventVisited; + + //WalkModuleActivityControl modActivity; + + std::minstd_rand gen; + + /** upper bound epsilon of the kld distance - the particle size is not allowed to exceed epsilon*/ + double epsilon; + + /** the upper 1 - delta quantil of the normal distribution. something like 0.01 */ + double delta; + + /** the bins */ + Binning bins; + + /** max particle size */ + uint32_t N_max; + + + PFTransKLDSampling(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(&modActivity); + //walker.addModule(&modHeadUgly); + walker.addModule(&modImportance); + //walker.addModule(&modFavorZ); + //walker.addModule(&modButterAct); + //walker.addModule(&modWifi); + //walker.addModule(&modPreventVisited); + + + epsilon = 0.15; + delta = 0.01; + N_max = 5000; + bins.setBinSizes({0.01, 0.01, 0.2, 0.3}); + bins.setRanges({BinningRange(-1,100), BinningRange(-10,60), BinningRange(-1,15), BinningRange(0, 2 * M_PI)}); + } + + virtual void transition(std::vector>& particles, const MyControl* control) override { + + std::normal_distribution noise(0, Settings::IMU::stepSigma); + Distribution::Uniform getParticle(0, particles.size()-1); + + //init stuff + uint32_t n = 0; + uint32_t k = 1; + double N = 0; + + //clear the bins + bins.clearUsed(); + + //create new particle set + std::vector> particlesNew; + + do{ + + //draw equally from the particle set + int particleIdx = getParticle.draw(); + K::Particle& p = particles[particleIdx]; + + //sample new particles based on the transition step + // 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; + + //if it falls into an empty bin then draw another particle + //is bin free? + if(bins.isFree(p.state)){ + k++; + bins.markUsed(p.state); + + //calculate the new N + double z_delta = K::NormalDistributionCDF::getProbit(1 - delta); + double front = (k - 1) / (2 * epsilon); + double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta ); + + N = front * std::pow(back, 3.0); + } + ++n; + + //add particle to new particleset + particlesNew.push_back(p); + + + } while (n < N && n < N_max); + + + //write new particleset + particles.clear(); + particles.swap(particlesNew); + } + +}; + + +struct BFTrans : public K::BackwardFilterTransition{ + + +public: + + /** + * ctor + * @param choice the choice to use for randomly drawing nodes + * @param fp the underlying floorplan + */ + BFTrans() + { + //nothin + } + + uint64_t ts = 0; + uint64_t deltaMS = 0; + + /** set the current time in millisconds */ + void setCurrentTime(const uint64_t ts) { + if (this->ts == 0) { + this->ts = ts; + deltaMS = 0; + } else { + deltaMS = this->ts - ts; + this->ts = ts; + } + } + + + /** + * smoothing transition starting at T with t, t-1,...0 + * @param particles_new p_t (Forward Filter) p2 + * @param particles_old p_t+1 (Smoothed Particles from Step before) p1 + * q(p1 | p2) is calculated + */ + std::vector> transition(std::vector>const& particles_new, + std::vector>const& particles_old ) override { + + + // calculate alpha(m,n) = p(q_t+1(m) | q_t(n)) + // this means, predict all possible states q_t+1 with all passible states q_t + // e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and + // p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1)) + std::vector> predictionProbabilities; + + omp_set_dynamic(0); // Explicitly disable dynamic teams + omp_set_num_threads(7); + #pragma omp parallel for shared(predictionProbabilities) + for (int i = 0; i < particles_old.size(); ++i) { + std::vector innerVector; + auto p1 = &particles_old[i]; + + for(int j = 0; j < particles_new.size(); ++j){ + + auto p2 = &particles_new[j]; + + const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0; + + //TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental + + const double distProb = K::NormalDistribution::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m); + + // TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement? + double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD()); + double diffDeg = Angle::radToDeg(diffRad); + double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad); + double measurementDeg = Angle::radToDeg(measurementRad); + const double headingProb = K::NormalDistribution::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg); + + // does the angle between two particles positions is similiar to the measurement + //double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y); + + //check how near we are to the measurement + double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0; + const double floorProb = K::NormalDistribution::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ); + + //combine the probabilities + double prob = distProb;// * floorProb * headingProb; + innerVector.push_back(prob); + + } + #pragma omp critical + predictionProbabilities.push_back(innerVector); + } + + return predictionProbabilities; + + } +}; + + +struct PFEval : public K::ParticleFilterEvaluation { + + WiFiModel& wifiModel; + WiFiObserverFree wiFiProbability; // free-calculation + //WiFiObserverGrid wiFiProbability; // grid-calculation + WiFiQualityAnalyzer wqa; + + BeaconModelLogDistCeiling& beaconModel; + BeaconObserverFree beaconProbability; + + Grid& grid; + + PFEval(WiFiModel& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid& grid) : + wifiModel(wifiModel), + beaconModel(beaconModel), + grid(grid), + wiFiProbability(Settings::WiFiModel::sigma, wifiModel), + beaconProbability(Settings::BeaconModel::sigma, beaconModel){ + + } + + /** probability step-distance */ + //TODO: add number of recognized steps + inline double getStepDistanceProb(const Point3 particle1, const Point3 particle2){ + double distance = particle1.getDistance(particle2); + return Distribution::Normal::getProbability(Settings::IMU::stepLength, Settings::IMU::stepSigma + 0.4, distance); + } + + //TODO: combinied evaluation heading and distance + + /** 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)); + } + + double getStairProb(const K::Particle& p, const ActivityButterPressure::Activity act) { + + const float kappa = 0.75; + + const MyNode& gn = grid.getNodeFor(p.state.position); + switch (act) { + + case ActivityButterPressure::Activity::STAY: + if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;} + if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;} + {return 1-kappa;} + + case ActivityButterPressure::Activity::UP: + case ActivityButterPressure::Activity::DOWN: + if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;} + if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;} + {return 1-kappa;} + + } + + return 1.0; + + } + + virtual double evaluation(std::vector>& particles, const MyObs& observation) override { + + double sum = 0; + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); + + wqa.add(wifiObs); + float quality = wqa.getQuality(); + + #pragma omp parallel for num_threads(3) + for (int i = 0; i < particles.size(); ++i) { + K::Particle& p = particles[i]; + + Point3 pos_m = p.state.position.inMeter(); + Point3 posOld_m = p.state.positionOld.inMeter(); + + double pWifi = getWIFI(observation, wifiObs, p.state.position); + const double pBaroPressure = getStairProb(p, observation.activity); + const double pStepDistance = getStepDistanceProb(pos_m, posOld_m); + //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 bool volatile init = observation.currentTime.sec() < 25; + //double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5)); + //double pWiFiMod = (init) ? (std::pow(pWifi, 0.5)) : (std::pow(pWifi, 0.9)); + + // bad wifi? -> we have no idea where we are! + if (quality < 0.25 && !init) { + //pWifi = 1; + //p.weight = std::pow(p.weight, 0.5); + } + + const double prob = pWifi * pStepDistance;// * pBaroPressure; + + p.weight = prob; + + #pragma omp atomic + 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..831406a --- /dev/null +++ b/filter/Structs.h @@ -0,0 +1,132 @@ + +#ifndef FSTRUCTS_H +#define FSTRUCTS_H + +#include +#include +#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; + + float headingChangeMeasured_rad; + + 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; + } + + float getBinValue(const int dim) const { + switch (dim) { + case 0: return this->position.x_cm / 100.0; + case 1: return this->position.y_cm / 100.0; + case 2: return this->position.z_cm / 100.0; + case 3: return this->heading.direction.getRAD(); + } + throw "cant find this value within the bin"; + } +}; + +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; + motionDeltaAngle_rad = 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; + + /** detected activity */ + ActivityButterPressure::Activity activity = ActivityButterPressure::Activity::STAY; + + /** 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..feb3e4f --- /dev/null +++ b/main.cpp @@ -0,0 +1,424 @@ +#include + +#include "filter/Structs.h" +#include "filter/KLB.h" +#include "Plotti.h" +#include "filter/Logic.h" +#include "Settings.h" + +#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/IndoorMap/maps/"; +//const std::string dataDir = "/home/toni/Documents/programme/localization/DynLag/code/data/"; +const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/"; +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 SecondFloorOnly = { + + mapDir + "SHL_Stock_2_01.xml", + + { + dataDir + "Path1_1.csv", + dataDir + "Path2_1.csv", + dataDir + "Path3_1.csv", + }, + + mapDir + "wifi_fp_all.dat", + 40, + VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO, + mapDir + "grid_Stock_2_01.dat" + }; + +} data; + + +Floorplan::IndoorMap* MyState::map; + +K::Statistics run(DataSetup setup, int numFile, std::string folder, std::vector gtPath) { + + std::vector kld_data; + + // 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"); + + //Wi-Fi model new +// WiFiModelFactory factory(map); +// WiFiModel* wifimodel= factory.loadXML("/home/toni/Documents/programme/localization/data/wifi/model/eachOptParPos_multimodel.xml"); +// 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); + + // add node-importance + Importance::addImportance(grid); + + grid.write(onp); + } else { + grid.read(inp); + } + + // stamp WiFi signal-strengths onto the grid + WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround); + + // reading file + Offline::FileReader fr(setup.training[numFile]); + + //interpolator for ground truth + Interpolator gtInterpolator = fr.getGroundTruthPath(map, gtPath); + + //gnuplot plot + 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; + + //History of all estimated particles. Used for smoothing + std::vector>> pfHistory; + std::vector tsHistory; + + //filter init + K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInit(grid))); + //K::ParticleFilterHistory pf(Settings::numParticles, std::unique_ptr(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f))); + + pf.setTransition(std::unique_ptr(new PFTrans(grid, &ctrl))); + //pf.setTransition(std::unique_ptr(new PFTransKLDSampling(grid, &ctrl))); + //pf.setTransition(std::unique_ptr(new PFTransSimple(grid))); + pf.setEvaluation(std::unique_ptr(new PFEval(WiFiModel, beaconModel, grid))); + + //resampling + if(Settings::useKLB){ + pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingDivergence())); + } else { + pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingSimple())); + //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingPercent(0.4))); + //pf.setResampling(std::unique_ptr>(new NodeResampling(*grid));); + //pf.setResampling(std::unique_ptr>(new K::ParticleFilterResamplingKLD())); + } + + 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.5))); + //pf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationKernelDensity())); + + + /** Smoothing Init */ + K::BackwardSimulation bf(Settings::numBSParticles); + if(Settings::Smoothing::activated){ + + //create the backward smoothing filter + bf.setSampler( std::unique_ptr>(new K::CumulativeSampler())); + + bool smoothing_resample = false; + //bf->setNEffThreshold(1.0); + if(smoothing_resample) + bf.setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); + bf.setTransition(std::unique_ptr( new BFTrans) ); + + //Smoothing estimation + bf.setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + //bf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); + //bf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationOrderedWeightedAverage(0.50f))); + } + + + + Timestamp lastTimestamp = Timestamp::fromMS(0); + + StepDetection sd; + TurnDetection td; + MotionDetection md; + ActivityButterPressure act; + + RelativePressure relBaro; + relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); + + K::Statistics errorStats; + K::Statistics errorStatsSmoothing; + + //file writing for error data + const long int t = static_cast(time(NULL)); + const std::string evalDir = errorDir + folder + std::to_string(t); + if(mkdir(evalDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1){ + Assert::doThrow("Eval folder couldn't be created!"); + } + + std::ofstream errorFile; + errorFile.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv"); + + std::ofstream errorFileSmoothing; + errorFileSmoothing.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_Smoothing.csv"); + + // parse each sensor-value within the offline data + for (const Offline::Entry& e : fr.getEntries()) { + + const Timestamp ts = Timestamp::fromMS(e.ts); + + if (e.type == Offline::Sensor::WIFI) { + obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data; + + } else if (e.type == Offline::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 == Offline::Sensor::ACC) { + if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { + ++ctrl.numStepsSinceLastTransition; + } + const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; + td.addAccelerometer(ts, _acc.data); + + } else if (e.type == Offline::Sensor::GYRO) { + const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; + const float delta_gyro = td.addGyroscope(ts, _gyr.data); + + ctrl.turnSinceLastTransition_rad += delta_gyro; + + } else if (e.type == Offline::Sensor::BARO) { + relBaro.add(ts, fr.getBarometer()[e.idx].data); + obs.relativePressure = relBaro.getPressureRealtiveToStart(); + obs.sigmaPressure = relBaro.getSigma(); + + //activity recognition + obs.activity = act.add(ts, fr.getBarometer()[e.idx].data); + //activity for transition + + } else if (e.type == Offline::Sensor::LIN_ACC) { + md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data); + + } else if (e.type == Offline::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) { + + + /** filtering stuff */ + obs.currentTime = ts; + MyState est = pf.update(&ctrl, obs); + + Point3 estPos = est.position.inMeter(); + Point3 gtPos = gtInterpolator.get(static_cast(ts.ms())); + + /** plotting stuff */ + plot.pInterest.clear(); + + plot.setEst(estPos); + plot.setGT(gtPos); + plot.addEstimationNode(estPos); + plot.addParticles(pf.getParticles()); + + /** error calculation stuff */ + float err_m = gtPos.getDistance(estPos); + errorStats.add(err_m); + errorFile << err_m << "\n"; + + + /** smoothing stuff */ + if(Settings::Smoothing::activated){ + //save the current estimation for later smoothing. + pfHistory.push_back(pf.getNonResamplingParticles()); + tsHistory.push_back(ts.ms()); + + //backward filtering + MyState estBF = est; + if(pfHistory.size() > Settings::Smoothing::lag){ + + bf.reset(); + + //use every n-th (std = 1) particle set of the history within a given lag (std = 5) + for(int i = 0; i <= Settings::Smoothing::lag; ++i){ + + ((BFTrans*)bf.getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]); + estBF = bf.update(pfHistory[(pfHistory.size() - 1) - i]); + } + } + + Point3 estPosSmoothing = estBF.position.inMeter(); + Point3 gtPosSmoothed = gtInterpolator.get(static_cast(tsHistory[(tsHistory.size() - 1) - Settings::Smoothing::lag])); + + //plot + plot.addEstimationNodeSmoothed(estPosSmoothing); + + // error between GT and smoothing + float errSmoothing_m = gtPosSmoothed.getDistance(estPosSmoothing); + errorStatsSmoothing.add(errSmoothing_m); + errorFileSmoothing << errSmoothing_m << "\n"; + } + + //plot misc + plot.setTimeInMinute(static_cast(ts.sec()) / 60, static_cast(static_cast(ts.sec())%60)); + + if(Settings::useKLB){ + plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n"; + } + plot.gp << "set label 1002 at screen 0.98, 0.98 'act:" << obs.activity << "'\n"; + + /** Draw everything */ + plot.show(); + usleep(10*10); + + lastTimestamp = ts; + + // reset control + ctrl.resetAfterTransition(); + } + + } + + errorFile.close(); + + std::cout << "Statistical Analysis Filtering: " << std::endl; + std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl; + + std::cout << "Statistical Analysis Smoothing: " << std::endl; + std::cout << "Median: " << errorStatsSmoothing.getMedian() << " Average: " << errorStatsSmoothing.getAvg() << " Std: " << errorStatsSmoothing.getStdDev() << std::endl; + + //Write the current plotti buffer into file + std::ofstream plotFile; + plotFile.open(evalDir + "/plot_" + 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(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i); + plot.show(); + usleep(10*10); + } + + plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90); + plot.show(); + + plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0); + plot.show(); + + plot.printOverview(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t)); + plot.show(); + + + /** Draw KLB */ + K::Gnuplot gp; + K::GnuplotPlot plotkld; + K::GnuplotPlotElementLines lines; + if(Settings::useKLB){ + + std::string path = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t); + gp << "set terminal png size 1280,720\n"; + gp << "set output '" << path << "_shennendistance.png'\n"; + + for(int i=0; i < kld_data.size()-1; ++i){ + + K::GnuplotPoint2 p1(i, kld_data[i]); + K::GnuplotPoint2 p2(i+1, kld_data[i+1]); + + lines.addSegment(p1, p2); + } + + plotkld.add(&lines); + gp.draw(plotkld); + gp.flush(); + } + + std::cout << "finished" << std::endl; + sleep(1); + + return errorStats; + +} + +int main(int argc, char** argv) { + + K::Statistics statsAVG; + K::Statistics statsMedian; + K::Statistics statsSTD; + K::Statistics statsQuantil; + K::Statistics tmp; + + for(int i = 0; i < 1; ++i){ + + tmp = run(data.SecondFloorOnly, 2, "Wifi-Dongle-Test", Settings::Path_DongleTest::path3); + statsMedian.add(tmp.getMedian()); + statsAVG.add(tmp.getAvg()); + statsSTD.add(tmp.getStdDev()); + statsQuantil.add(tmp.getQuantile(0.75)); + + std::cout << "Iteration " << i << " completed" << std::endl;; + } + + std::cout << "==========================================================" << std::endl; + std::cout << "Average of all statistical data: " << std::endl; + std::cout << "Median: " << statsMedian.getAvg() << std::endl; + std::cout << "Average: " << statsAVG.getAvg() << std::endl; + std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl; + std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl; + std::cout << "==========================================================" << std::endl; + + //EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS + std::ofstream finalStatisticFile; + finalStatisticFile.open (errorDir + "/tmp.csv"); + + finalStatisticFile << "Average of all statistical data: \n"; + finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n"; + finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n"; + finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n"; + finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n"; + + finalStatisticFile.close(); + //EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS + +}