From 94fb34e6f9a9f296f71cc5288a9a8c71c563f3fd Mon Sep 17 00:00:00 2001 From: toni Date: Tue, 22 Mar 2016 18:01:18 +0100 Subject: [PATCH] added fixelag smoothing --- code/CMakeLists.txt.user | 2 +- code/Settings.h | 4 + code/eval/FixedLagEvalBase.h | 349 ++++++++++++++++++ code/eval/PaperVisImportance.h | 10 +- code/eval/SmoothingEval1.h | 5 +- .../smoothing/MySmoothingTransition.h | 145 ++++++++ .../smoothing/MySmoothingTransitionSimple.h | 125 +++++++ 7 files changed, 632 insertions(+), 8 deletions(-) create mode 100644 code/eval/FixedLagEvalBase.h create mode 100644 code/particles/smoothing/MySmoothingTransition.h create mode 100644 code/particles/smoothing/MySmoothingTransitionSimple.h diff --git a/code/CMakeLists.txt.user b/code/CMakeLists.txt.user index d916038..c6fe5c2 100644 --- a/code/CMakeLists.txt.user +++ b/code/CMakeLists.txt.user @@ -1,6 +1,6 @@ - + EnvironmentId diff --git a/code/Settings.h b/code/Settings.h index 79fdaf5..08a3586 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -19,6 +19,10 @@ namespace MiscSettings { const int numParticles = 7500; + const int lag = 5; + + const int fixedLagGap = 1; + } diff --git a/code/eval/FixedLagEvalBase.h b/code/eval/FixedLagEvalBase.h new file mode 100644 index 0000000..80e1ae5 --- /dev/null +++ b/code/eval/FixedLagEvalBase.h @@ -0,0 +1,349 @@ +#ifndef FIXEDLAGEVALBASE_H +#define FIXEDLAGEVALBASE_H + +#include "../Settings.h" +#include "../Helper.h" +#include "../Vis.h" + +#include +#include +#include +#include + +#include "GroundTruthWay.h" + +#include "../particles/MyState.h" +#include "../particles/MyObservation.h" +#include "../particles/MyEvaluation.h" +#include "../particles/MyTransition.h" +#include "../particles/MyInitializer.h" +#include "../particles/smoothing/MySmoothingTransition.h" +#include "../particles/smoothing/MySmoothingTransitionSimple.h" + +#include "../reader/SensorReader.h" +#include "../reader/SensorReaderStep.h" +#include "../reader/SensorReaderTurn.h" + +#include "../lukas/TurnObservation.h" +#include "../lukas/StepObservation.h" + +#include "../toni/BarometerSensorReader.h" + +#include "../frank/WiFiSensorReader.h" +#include "../frank/BeaconSensorReader.h" +#include "../frank/OrientationSensorReader.h" + +class FixedLagEvalBase { + +protected: + + Grid grid; + Helper::FHWSFloors floors; + Vis vis; + + K::ParticleFilterHistory* pf; + K::BackwardFilter* bf; + + SensorReader* sr; + SensorReaderTurn* srt; + SensorReaderStep* srs; + + + std::string runName; + + GroundTruthWay gtw; + + // OLD + //std::vector way0 = {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 way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0}; + //std::vector way2 = {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}; + + // NEW + std::vector path1 = {29, 28,27,26,255,25,24,23,22,21,20}; + std::vector path1dbl = {29, 29, 28,27,26,255,25,24,23,22,21,20}; + std::vector path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6}; + std::vector path2dbl = {19, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6}; + std::vector path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31}; + std::vector path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31}; + std::vector path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; + std::vector path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint! + +public: + + FixedLagEvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) { + + // build the grid + Helper::buildTheGrid(grid, floors); + + // setup the visualisation + vis.addFloor(floors.f0, floors.h0); + vis.addFloor(floors.f1, floors.h1); + vis.addFloor(floors.f2, floors.h2); + vis.addFloor(floors.f3, floors.h3); + + vis.floors.setColorHex("#666666"); + vis.groundTruth.setCustomAttr("dashtype 3"); + vis.groundTruth.setColorHex("#009900"); + vis.gp << "unset cbrange\n"; + + + } + + static GridPoint conv(const Point3& p) { + return GridPoint(p.x, p.y, p.z); + } + + GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map& waypoints, std::vector ids) { + + // construct the ground-truth-path by using all contained waypoint ids + std::vector path; + for (int id : ids) { + auto it = waypoints.find(id); + if(it == waypoints.end()) {throw "not found";} + path.push_back(it->second); + } + + // new created the timed path + GroundTruthWay gtw; + int i = 0; + while (sr.hasNext()) { + const SensorEntry se = sr.getNext(); + if (se.data.empty()) {continue;} // why necessary?? + if (se.idx == 99) { + gtw.add(se.ts, path[i]); + ++i; + } + } + + // ensure the sensor-data contained usable timestamps for the ground-truth mapping + assert(i>0); + + sr.rewind(); + return gtw; + + } + + void run() { + + // sensor numbers + const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6; + //const int s_linearAcceleration = 2; + + std::list turn_observations; + std::list step_observations; + + //Create an BarometerSensorReader + BarometerSensorReader baroSensorReader; + + + //Read all turn Observations + while(srt->hasNext()) { + + SensorEntryTurn set = srt->getNext(); + TurnObservation to; + + to.ts = set.ts; + to.delta_heading = set.delta_heading; + to.delta_motion = set.delta_motion; + + turn_observations.push_back(to); + } + + + //Step Observations + while(srs->hasNext()) { + + SensorEntryStep ses = srs->getNext(); + StepObservation so; + + so.ts = ses.ts; + + step_observations.push_back(so); + } + + + + // the to-be-evaluated observation + MyObservation obs; + obs.step = new StepObservation(); obs.step->steps = 0; + obs.turn = new TurnObservation(); obs.turn->delta_heading = 0; obs.turn->delta_motion = 0; + + // control data + MyControl ctrl; + + //History of all estimated particles. Used for smoothing + std::vector>> pfHistory; + std::vector smoothedEst; + std::vector tsHistory; + + std::vector pathEst; + + uint64_t lastTransitionTS = 0; + int64_t start_time = -1; + + K::Statistics statsFiltering; + K::Statistics statsSmoothing; + int cnt = 0; + + //stats file + std::ofstream statsout("/tmp/unsmoothed_" + runName + ".stats"); + + // process each single sensor reading + while(sr->hasNext()) { + + // get the next sensor reading from the CSV + const SensorEntry se = sr->getNext(); + + //start_time needed for time calculation of steps and turns + obs.latestSensorDataTS = se.ts; + if (start_time == -1) {start_time = se.ts;} + int64_t current_time = se.ts - start_time; + + switch(se.idx) { + + case s_wifi: { + obs.wifi = WiFiSensorReader::readWifi(se); + break; + } + + case s_beacons: { + BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se); + if (!boe.mac.empty()) { + obs.beacons.entries.push_back(boe); + } // add the observed beacon + obs.beacons.removeOld(obs.latestSensorDataTS); + break; + } + + case s_barometer: { + obs.barometer = baroSensorReader.readBarometer(se); + break; + } + +// case s_linearAcceleration:{ +// baroSensorReader.readVerticalAcceleration(se); +// break; +// } + + case s_orientation: { + obs.orientation = OrientationSensorReader::read(se); + break; + } + + } + + // process all occurred turns + while (!step_observations.empty() && current_time > step_observations.front().ts) { + const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so; + obs.step->steps++; + ctrl.walked_m = obs.step->steps * 0.71; + } + + // process all occurred steps + while (!turn_observations.empty() && current_time > turn_observations.front().ts) { + const TurnObservation _to = turn_observations.front(); turn_observations.pop_front(); + obs.turn->delta_heading += _to.delta_heading; + obs.turn->delta_motion += _to.delta_motion; + ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading); + + } + + + // time for a transition? + if (se.ts - lastTransitionTS > MiscSettings::timeSteps) { + + lastTransitionTS = se.ts; + + // timed updates + ((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS); + + + // update the particle filter (transition + eval), estimate a new current position and add it to the estimated path + const MyState est = pf->update(&ctrl, obs); + const Point3 curEst = est.pCur; + + // error calculation. compare ground-truth to estimation + const int offset = 0; + const Point3 curGT = gtw.getPosAtTime(se.ts - offset); + const Point3 diff = curEst - curGT; + + + pathEst.push_back(curEst); + const float err = diff.length(); + + // skip the first 24 scans due to uniform distribution start + if (++cnt > 24) { + statsFiltering.add(err); + std::cout << "Filtering: " << statsFiltering.asString() << std::endl; + + //save the current estimation for later smoothing. + pfHistory.push_back(pf->getNonResamplingParticles()); + tsHistory.push_back(se.ts); + } + + + //fixed-lag smoothing + MyState estBF; + if(pfHistory.size() >= MiscSettings::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 < MiscSettings::lag; i += MiscSettings::fixedLagGap){ + + ((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]); + estBF = bf->update(pfHistory[(pfHistory.size() - 1) - i]); + + } + + //use and visualize the smoothed particle set + const Point3 curSmoothedEst = estBF.pCur; + + smoothedEst.push_back(curSmoothedEst); + + // error calculation. compare ground-truth to estimation + const Point3 curGTSmoothed = gtw.getPosAtTime(se.ts - (MiscSettings::lag * MiscSettings::timeSteps)); + const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed; + + const float errSmoothed = diffSmoothed.length(); + statsSmoothing.add(errSmoothed); + std::cout << "Smoothing: " << statsSmoothing.asString() << std::endl; + + + // plot + vis.clearStates(); + for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) { + const K::Particle& p = bf->getbackwardParticles().back()[j]; + vis.addState(p.state.walkState); + } + vis.setTimestamp(se.ts); + vis.addGroundTruth(gtw); + vis.addEstPath(smoothedEst); + vis.setEstAndShould(curSmoothedEst, curGTSmoothed); + + if (obs.barometer != nullptr) { + vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n"; + } + vis.gp << "set label 111 '" < dijkstra; @@ -124,7 +124,7 @@ public: DijkstraMapperNormal accNormal(grid); // path without importance - dijkstra.build(gnStart, gnStart, accNormal); + dijkstra.build(&gnStart, accNormal); DijkstraPath pathNormal(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart)); // stamp importance information onto the grid-nodes @@ -132,7 +132,7 @@ public: gridImp.addImportance(grid, h0.cm()); // path WITH importance - dijkstra.build(gnStart, gnStart, accImp); + dijkstra.build(&gnStart, accImp); DijkstraPath pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart)); // build plot @@ -240,7 +240,7 @@ public: // path WITH importance DijkstraMapper accImp(grid); - dijkstra.build(gnStart, gnStart, accImp); + dijkstra.build(&gnStart, accImp); DijkstraPath pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart)); // stamp distance information onto the grid diff --git a/code/eval/SmoothingEval1.h b/code/eval/SmoothingEval1.h index 591c987..e945b0a 100644 --- a/code/eval/SmoothingEval1.h +++ b/code/eval/SmoothingEval1.h @@ -2,6 +2,7 @@ #define SMOOTHINGEVAL1_H #include "SmoothingEvalBase.h" +#include "FixedLagEvalBase.h" #include "../DijkstraMapper.h" #include #include @@ -20,7 +21,7 @@ #include #include -class SmoothingEval1 : public SmoothingEvalBase { +class SmoothingEval1 : public FixedLagEvalBase { public: @@ -75,7 +76,7 @@ public: bf->setEstimation(std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); if(smoothing_resample) bf->setResampling( std::unique_ptr>(new K::ParticleFilterResamplingSimple()) ); - bf->setTransition(std::unique_ptr( new MySmoothingTransition(&grid)) ); + bf->setTransition(std::unique_ptr( new MySmoothingTransitionSimple()) ); } diff --git a/code/particles/smoothing/MySmoothingTransition.h b/code/particles/smoothing/MySmoothingTransition.h new file mode 100644 index 0000000..928b80a --- /dev/null +++ b/code/particles/smoothing/MySmoothingTransition.h @@ -0,0 +1,145 @@ +#ifndef MYSMOOTHINGTRANSITION_H +#define MYSMOOTHINGTRANSITION_H + +#include +#include +#include +#include + +#include +#include + +#include "../MyState.h" +#include "../MyControl.h" + +#include "../../DijkstraMapper.h" + +#include "../../toni/barometric.h" +#include + +static double smoothing_walk_mu = 0.7; +static double smoothing_walk_sigma = 0.5; +static double smoothing_heading_sigma = 15.0; +static double smoothing_baro_sigma = 0.2; +static bool smoothing_baro_use_hPa = false; +static double smoothing_baro_with_measurement = false; + +class MySmoothingTransition : public K::BackwardFilterTransition { + +private: + + /** the created grid to draw transitions from */ + Grid* grid; + + /** a simple normal distribution */ + K::NormalDistribution distWalk; + + +public: + + /** + * ctor + * @param choice the choice to use for randomly drawing nodes + * @param fp the underlying floorplan + */ + MySmoothingTransition(Grid* grid) : + grid(grid), distWalk(smoothing_walk_mu, smoothing_walk_sigma) { + distWalk.setSeed(4321); + } + +public: + + 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) + * @param particles_old p_t+1 (Smoothed Particles from Step before) + */ + 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; + + auto p1 = particles_old.begin(); + auto p2 = particles_new.begin(); + + #pragma omp parallel for private(p2) shared(predictionProbabilities) + for (p1 = particles_old.begin(); p1 < particles_old.end(); ++p1) { + std::vector innerVector; + for(p2 = particles_new.begin(); p2 < particles_new.end(); ++p2){ + + // find the node (square) the particle is within + // just to be safe, we round z to the nearest floor + + //TODO:: Nullptr check! sometimes src/dst can be nullptr + //const Node3* dst = graph->getNearestNode(p1->state.x_cm, p1->state.y_cm, std::round(p1->state.z_nr)); + //const Node3* src = graph->getNearestNode(p2->state.x_cm, p2->state.y_cm, std::round(p2->state.z_nr)); + + const MyGridNode* dst = grid->getNodePtrFor(GridPoint(p1->state.pCur.x, p1->state.pCur.y, p1->state.pCur.z)); + const MyGridNode* src = grid->getNodePtrFor(GridPoint(p2->state.pCur.x, p2->state.pCur.y, p2->state.pCur.z)); + + Dijkstra dijkstra; + dijkstra.build(src, dst, DijkstraMapper(*grid)); + + double distDijkstra_m = dijkstra.getNode(*src)->cumWeight; + + const double distProb = distWalk.getProbability(distDijkstra_m); + + //getProb using the angle(heading) between src and dst + double angle = 0.0; + if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){ + angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y); + } + const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle); + + //assert(headingProb != 0.0); + //assert(distProb != 0.0); + + //check how near we are to the measurement + double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa); + + //combine the probabilities + double prob = distProb * floorProb * headingProb; + innerVector.push_back(prob); + + //if(distance_m != distance_m) {throw "detected NaN";} + //if(distProb != distProb) {throw "detected NaN";} + if(angle != angle) {throw "detected NaN";} + if(headingProb != headingProb) {throw "detected NaN";} + if(floorProb != floorProb) {throw "detected NaN";} + if(floorProb == 0) {throw "detected NaN";} + if(prob != prob) {throw "detected NaN";} + + //assert(prob != 0.0); + + + } + #pragma omp critical + predictionProbabilities.push_back(innerVector); + } + return predictionProbabilities; + + } + + +}; + +#endif // MYTRANSITION_H diff --git a/code/particles/smoothing/MySmoothingTransitionSimple.h b/code/particles/smoothing/MySmoothingTransitionSimple.h new file mode 100644 index 0000000..0f53077 --- /dev/null +++ b/code/particles/smoothing/MySmoothingTransitionSimple.h @@ -0,0 +1,125 @@ +#ifndef MYSMOOTHINGTRANSITIONSIMPLE_H +#define MYSMOOTHINGTRANSITIONSIMPLE_H + +#include +#include +#include +#include + +#include + +#include "../MyState.h" +#include "../MyControl.h" +#include "../../Helper.h" +#include "../../toni/barometric.h" + +class MySmoothingTransitionSimple : public K::BackwardFilterTransition { + +private: + + /** a simple normal distribution */ + K::NormalDistribution distWalk; + + +public: + + /** + * ctor + * @param choice the choice to use for randomly drawing nodes + * @param fp the underlying floorplan + */ + MySmoothingTransitionSimple() : + distWalk(smoothing_walk_mu, smoothing_walk_sigma) { + distWalk.setSeed(4321); + } + +public: + + 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) + * @param particles_old p_t+1 (Smoothed Particles from Step before) + */ + 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; + + auto p1 = particles_old.begin(); //smoothed / backward filter p_t+1 + auto p2 = particles_new.begin(); //forward filter p_t + + #pragma omp parallel for private(p2) shared(predictionProbabilities) + for (p1 = particles_old.begin(); p1 < particles_old.end(); ++p1) { + std::vector innerVector; + for(p2 = particles_new.begin(); p2 < particles_new.end(); ++p2){ + + //!!!distance kann hier zu groß werden!!! + const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0; + + //get distance walked and getProb using the walking model + //double distDijkstra_m = ((GRID_DISTANCE_CM / 100.0) * (8 - 1)); + const double distProb = distWalk.getProbability(distance_m); + + + //getProb using the angle(heading) between src and dst + double angle = 0.0; + if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){ + angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y); + } + + const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle); + + //assert(headingProb != 0.0); + //assert(distProb != 0.0); + + + //check how near we are to the measurement + double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa); + + + //combine the probabilities + double prob = distProb * headingProb * floorProb; + innerVector.push_back(prob); + + if(distance_m != distance_m) {throw "detected NaN";} + if(distProb != distProb) {throw "detected NaN";} + if(angle != angle) {throw "detected NaN";} + if(headingProb != headingProb) {throw "detected NaN";} + if(floorProb != floorProb) {throw "detected NaN";} + if(floorProb == 0) {throw "detected NaN";} + if(prob != prob) {throw "detected NaN";} + + //assert(prob != 0.0); + + + } + #pragma omp critical + predictionProbabilities.push_back(innerVector); + } + return predictionProbabilities; + + } + + +}; + +#endif // MYTRANSITION_H