toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added Evalbase.h again

Browse Source
This commit is contained in:
toni
2016-05-05 10:35:59 +02:00
parent bbc8f5aa19 a6790c3db3
commit 2fbc10fbd3
14 changed files with 2259 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
code/eval/FixedLagEvalBase.h
View File

@@ -101,6 +101,8 @@ public:
return GridPoint(p.x, p.y, p.z);
}
GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
// construct the ground-truth-path by using all contained waypoint ids
@@ -206,12 +208,16 @@ public:
std::vector<float> errorsDistNorm;
std::vector<float> errorsDistSmooth;
// acticity output
std::vector<int> activity;
// number of updates to skip before starting the smoothing process
const int skip = 35;
//stats file
std::ofstream statsout("/tmp/unsmoothed_" + runName + ".stats");
std::ofstream statsoutFiltering("/tmp/unsmoothed_" + runName + ".stats");
std::ofstream statsoutSmoothing("/tmp/smoothed_" + runName + ".stats");
//heading
double currentHeadingGivenByLukas = 0.0;
@@ -321,6 +327,7 @@ public:
const float errDist = gtw.getMinDist(curEst); // minimum distance between estimation and ground-truth
activity.push_back((int)actDet.getCurrentActivity());
// count number of transitions
++cnt;
@@ -332,6 +339,8 @@ public:
errorsDistNorm.push_back(errDist);
//std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl;
//std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl;
statsoutFiltering << "\n\t"; statsFiltering.appendTo(statsoutFiltering); statsoutFiltering << "\n\n";
}
if(cnt > skip){
@@ -355,6 +364,9 @@ public:
}
//use and visualize the smoothed particle set
const Point3 curSmoothedEst = estBF.pCur;
@@ -377,9 +389,13 @@ public:
errorsSmooth.push_back(errSmoothed);
errorsDistSmooth.push_back(errDistSmoothed);
//std::cout << "SmoothingTime: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl;
//std::cout << "SmoothingDist: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsDistSmoothing.asString() << std::endl;
//save to file
statsoutSmoothing << "\n\t"; statsSmoothing.appendTo(statsoutFiltering); statsoutFiltering << "\n\n";
//plot
vis.clearStates();
for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
@@ -455,6 +471,12 @@ public:
oError.close();
}
{ // detailled activity over time
std::ofstream oAct("/tmp/act_" + runName + ".dat");
for (int i : activity) {oAct << i << "\n";}
oAct.close();
}
// plot
//vis.clearStates();
vis.gp.setTerminal("png", K::GnuplotSize(2560 * 2.54, 1440 * 2.54) );

92
code/eval/SmoothingEval1.h
View File

@@ -2,6 +2,7 @@
#define SMOOTHINGEVAL1_H
#include "SmoothingEvalBase.h"
#include "SmoothingEvalBaseLOG.h"
#include "FixedLagEvalBase.h"
#include "../DijkstraMapper.h"
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
@@ -21,7 +22,7 @@
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverageWithAngle.h>
class SmoothingEval1 : public SmoothingEvalBase {
class SmoothingEval1 : public FixedLagEvalBase {
public:
@@ -45,23 +46,15 @@ public:
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
// state estimation step
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
//create the backward smoothing filter
//bf = new K::BackwardSimulation<MyState>(MiscSettings::numBSParticles);
bf = new K::CondensationBackwardFilter<MyState>;
//bf->setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
bool smoothing_resample = false;
bf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
//bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
bf = new K::BackwardSimulation<MyState>(MiscSettings::numBSParticles);
//bf = new K::CondensationBackwardFilter<MyState>;
bf->setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));
}
@@ -88,7 +81,7 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
bool smoothing_resample = true;
//Smoothing using Simple Trans
@@ -123,7 +116,6 @@ public:
//Smoothing using Simple Trans
//bf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>()));
if(smoothing_resample)
@@ -155,7 +147,7 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = false;
bool smoothing_resample = true;
//Smoothing using Dijkstra
@@ -184,6 +176,14 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = true;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
void bergwerk_path1_nexus_simple() {
@@ -244,6 +244,14 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = true;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
void bergwerk_path2_nexus_simple() {
@@ -305,6 +313,14 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = true;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
void bergwerk_path3_nexus_simple() {
@@ -365,6 +381,14 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.05;
bool smoothing_resample = true;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
void bergwerk_path4_nexus_simple() {
@@ -426,6 +450,14 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.15;
bool smoothing_resample = true;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
void bergwerk_path1_galaxy_simple() {
@@ -469,6 +501,14 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.15;
bool smoothing_resample = true;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
void bergwerk_path2_galaxy_simple() {
@@ -513,6 +553,14 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.15;
bool smoothing_resample = true;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
void bergwerk_path3_galaxy_simple() {
@@ -558,6 +606,14 @@ public:
smoothing_heading_sigma = 5.0;
smoothing_baro_sigma = 0.15;
bool smoothing_resample = true;
//Smoothing using Simple Trans
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental) );
}
void bergwerk_path4_galaxy_simple() {

2
code/eval/SmoothingEvalBase.h
View File

@@ -457,7 +457,7 @@ public:
// plot
//vis.clearStates();
vis.gp.setTerminal("png", K::GnuplotSize(1280 * 2.54, 720 * 2.54) );
vis.gp.setTerminal("png", K::GnuplotSize(2560 * 2.54, 1440 * 2.54) );
vis.gp.setOutput("/tmp/" + runName + ".png");
vis.gp << "set view 60," << 40 << "\n"; //For fixed position

652
code/eval/SmoothingEvalBaseLOG.h Normal file
View File

@@ -0,0 +1,652 @@
#ifndef SMOOTHINGEVALBASELOG_H
#define SMOOTHINGEVALBASELOG_H
#include "../Settings.h"
#include "../Helper.h"
#include "../Vis.h"
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterHistory.h>
#include <KLib/math/filter/smoothing/BackwardSimulation.h>
#include <KLib/math/filter/smoothing/CondensationBackwardFilter.h>
#include <KLib/math/filter/smoothing/sampling/ParticleTrajectorieSampler.h>
#include <KLib/math/filter/smoothing/sampling/CumulativeSampler.h>
#include <KLib/math/statistics/Statistics.h>
#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 "../particles/smoothing/MySmoothingTransitionExperimental.h"
#include "../reader/SensorReader.h"
#include "../reader/SensorReaderStep.h"
#include "../reader/SensorReaderTurn.h"
#include "../reader/SensorReaderAccel.h"
#include "../lukas/TurnObservation.h"
#include "../lukas/StepObservation.h"
#include "../lukas/ActivityDetection.h"
#include "../toni/BarometerSensorReader.h"
#include "../frank/WiFiSensorReader.h"
#include "../frank/BeaconSensorReader.h"
#include "../frank/OrientationSensorReader.h"
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
class SmoothingEvalBaseLOG {
protected:
Grid<MyGridNode> grid;
Helper::FHWSFloors floors;
Vis vis;
K::ParticleFilterHistory<MyState, MyControl, MyObservation>* pf;
K::BackwardFilter<MyState>* bf;
SensorReader* sr;
SensorReaderTurn* srt;
SensorReaderStep* srs;
std::string runName;
GroundTruthWay gtw;
// OLD
//std::vector<int> 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<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> 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<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path1dbl = {29, 29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path2dbl = {19, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
std::vector<int> 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!
float stepSize = 0.71;
public:
void dumpParticles(const std::string& mode, const int idx, const std::vector<K::Particle<MyState>>& particles, const Point3 curEst) {
std::string a = "/tmp/" + runName;
std::string b = a + "/particles";
std::string c = b + "/" + mode + "_" + std::to_string(idx) + ".dat";
std::string d = b + "/" + "plot" + "_" + std::to_string(idx) + ".gp";
mkdir(a.c_str(), 0777);
mkdir(b.c_str(), 0777);
std::string file = c;
// std::unordered_map<uint64_t, float> map;
// for (const K::Particle<MyState>& p : particles) {
// const float dz = std::abs(p.state.pCur.z - curEst.z);
// //if (dz > 20) {continue;} // skip particles on other levels
// const uint64_t idx = (uint64_t) p.state.pCur.z * 0xFFFFFFFFUL + (int) p.state.pCur.y * 0xFFFF + (int) p.state.pCur.x;
// //const int x = idx & 0xFFFF; if (x - p.state.pCur.x > 10) {throw 1;} // sanity check
// //const int y = idx / 0xFFFF; if (y - p.state.pCur.y > 10) {throw 1;} // sanity check
// map[idx] += p.weight;
// }
// how to plot
std::ofstream plot(d);
plot << "splot\\\n";
plot << "'filtered_" << (idx) << ".dat' every ::2 using 1:2:3:4 with points palette pt 3 notitle,\\\n";
plot << "'filtered_" << (idx) << ".dat' every ::0::0 using 1:2:3:4 with points palette pt 3 ps 5 title 'avg',\\\n";
plot << "'filtered_" << (idx) << ".dat' every ::1::1 using 1:2:3:4 with points palette pt 3 ps 3 title 'avg50',\\\n";
plot << "'smoothed_" << (idx) << ".dat' every ::2 using ($$1+1000):2:3:4 with points palette pt 7 notitle,\\\n";
plot << "'smoothed_" << (idx) << ".dat' every ::0::0 using ($$1+1000):2:3:4 with points palette pt 6 ps 5 title 'avg',\\\n";
plot << "'smoothed_" << (idx) << ".dat' every ::1::1 using ($$1+1000):2:3:4 with points palette pt 6 ps 3 title 'avg50',\\\n";
std::ofstream out(file);
// first two entries are different : avg and avg 50%
K::ParticleFilterEstimationWeightedAverage<MyState> avg;
K::ParticleFilterEstimationOrderedWeightedAverage<MyState> avg50(0.5);
const MyState sAvg = avg.estimate((std::vector<K::Particle<MyState>>&) particles);
const MyState sAvg50 = avg50.estimate((std::vector<K::Particle<MyState>>&) particles);
out << sAvg.pCur.x << " " << sAvg.pCur.y << " " << sAvg.pCur.z << " 0\n";
out << sAvg50.pCur.x << " " << sAvg50.pCur.y << " " << sAvg50.pCur.z << " 0\n";
for (const K::Particle<MyState>& p : particles) {
const float dz = std::abs(p.state.pCur.z - curEst.z);
if (dz > 20) {continue;} // skip particles on other levels
out << p.state.pCur.x << " " << p.state.pCur.y << " " << p.state.pCur.z << " " << p.weight << "\n";
}
// for (auto it : map) {
// const uint64_t idx = it.first;
// const int x = idx % 0xFFFF;
// const int y = idx / 0xFFFF % 0xFFFF;
// const int z = idx / 0xFFFF / 0xFFFF;
// out << x << " " << y << " " << z << " " << it.second << "\n";
// }
out.close();
}
void addPlotDump(const int idx, const std::string txt) {
std::string c = "/tmp/" + runName + "/" + "particles" + "/plot_" + std::to_string(idx) + ".gp";
std::ofstream out(c, std::fstream::app);
out << txt;
out.close();
}
SmoothingEvalBaseLOG() : 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<int, Point3>& waypoints, std::vector<int> ids) {
// construct the ground-truth-path by using all contained waypoint ids
std::vector<Point3> 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_accel = 0;
std::list<TurnObservation> turn_observations;
std::list<StepObservation> step_observations;
//Create an BarometerSensorReader
BarometerSensorReader baroSensorReader;
// activity detection
ActivityDetection actDet;
//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<std::vector<K::Particle<MyState>>> pfHistory;
std::vector<Point3> smoothedEst;
std::vector<uint64_t> tsHistory;
std::vector<Point3> pathEst;
uint64_t lastTransitionTS = 0;
int64_t start_time = -1;
K::Statistics<double> statsFiltering;
K::Statistics<double> statsSmoothing;
K::Statistics<float> statsDistFiltering;
K::Statistics<float> statsDistSmoothing;
int cnt = 0;
// error per update-step
std::vector<float> errorsNorm;
std::vector<float> errorsSmooth;
std::vector<float> errorsDistNorm;
std::vector<float> errorsDistSmooth;
//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);
actDet.addBaro(baroSensorReader.getHPA(se));
break;
}
case s_accel: {
float acc[3];
SensorReaderAccel sre; sre.read(se, acc);
actDet.addAccel(acc);
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 * stepSize;
}
// 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);
}
// currently detected activity
// TODO: feed sensor values!
ctrl.currentActivitiy = actDet.getCurrentActivity();
// this is just for testing purposes
obs.currentActivity = actDet.getCurrentActivity();
// 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();
const float errDist = gtw.getMinDist(curEst); // minimum distance between estimation and ground-truth
++cnt;
// skip the first 24 scans due to uniform distribution start
if (cnt > 35) {
statsFiltering.add(err);
statsDistFiltering.add(errDist);
errorsNorm.push_back(err);
errorsDistNorm.push_back(errDist);
//std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl;
//std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl;
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
dumpParticles("filtered", cnt-36, pf->getNonResamplingParticles(), curEst);
}
// plot
vis.clearStates();
for (int i = 0; i < (int) pf->getParticles().size(); i+=15) {
const K::Particle<MyState>& p = pf->getParticles()[i];
vis.addState(p.state.walkState);
}
vis.setTimestamp(se.ts);
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.setEstAndShould(curEst, curGT);
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 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
//vis.show();
// prevent gnuplot errors
//usleep(1000*33);
}
}
statsout.close();
// Smoothing Part starts here
// ========================= //
//File
//std::ofstream statsout2("/tmp/smoothed_" + runName + ".stats");
//stats.reset();
bf->reset();
vis.resetTimestamp();
MyState estBF;
//iterate thru all particle sets from T to t (currentParticleSetToSmoothAtTimeT)
for(int i = pfHistory.size() - 1; i >= 0 ; i -= MiscSettings::fixedLagGap){
//Set time
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
estBF = bf->update(pfHistory[i]);
smoothedEst.push_back(estBF.pCur);
}
for(int t = 0; t < smoothedEst.size(); ++t){
int idx = smoothedEst.size() - 1 - t;
const Point3 curSmoothedEst = smoothedEst[idx];
dumpParticles("smoothed", t, bf->getbackwardParticles()[idx], curSmoothedEst);
}
std::reverse(smoothedEst.begin(), smoothedEst.end());
for(int t = 0; t < smoothedEst.size(); ++t){
const Point3 curSmoothedEst = smoothedEst[t];
// error calculation. compare ground-truth to estimation
const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[t]);
const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
const float errSmoothed = diffSmoothed.length();
const float errDistSmoothed = gtw.getMinDist(curSmoothedEst); // minimum distance between smoothed-estimation and ground-truth
statsSmoothing.add(errSmoothed);
statsDistSmoothing.add(errDistSmoothed);
errorsSmooth.push_back(errSmoothed);
errorsDistSmooth.push_back(errDistSmoothed);
//std::cout << "SmoothingTime: " << tsHistory[t] << " " << statsSmoothing.asString() << std::endl;
//std::cout << "SmoothingDist: " << tsHistory[t] << " " << statsDistSmoothing.asString() << std::endl;
// plot
vis.clearStates();
for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
vis.setTimestamp(tsHistory[t]);
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 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
vis.gp << "set label 112 '" << "Act: " << actDet.toString() << "' at screen 0.1,0.15\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
//vis.show();
// prevent gnuplot errors
//usleep(1000*33);
}
//statsout2.close();
{ // detailled error-description (normally filtered)
std::ofstream oError("/tmp/err_norm_" + runName + ".dat");
for (float f : errorsNorm) {oError << f << "\n";}
oError.close();
}
{ // detailled error-description (smothed)
std::ofstream oError("/tmp/err_smooth_" + runName + ".dat");
for (float f : errorsSmooth) {oError << f << "\n";}
oError.close();
}
{ // detailled distance-error-description (normally filtered)
std::ofstream oError("/tmp/err_dist_norm_" + runName + ".dat");
for (float f : errorsDistNorm) {oError << f << "\n";}
oError.close();
}
{ // detailled distance-error-description (smothed)
std::ofstream oError("/tmp/err_dist_smooth_" + runName + ".dat");
for (float f : errorsDistSmooth) {oError << f << "\n";}
oError.close();
}
// plot
//vis.clearStates();
vis.gp.setTerminal("png", K::GnuplotSize(1280 * 2.54, 720 * 2.54) );
vis.gp.setOutput("/tmp/" + runName + ".png");
vis.gp << "set view 60," << 40 << "\n"; //For fixed position
for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
//vis.setTimestamp(se.ts);
vis.setFilteringMedian(statsFiltering.getMedian());
vis.setSmoothingMedian(statsSmoothing.getMedian());
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.addSmoothPath(smoothedEst);
//vis.setEstAndShould(curEst, curGT);
//vis.setEstAndShould2(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 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();
vis.show();
vis.show();
vis.show();
// prevent gnuplot errors
usleep(1000*33);
// append error for each run to a file
std::ofstream oTError("/tmp/errorsFilter.txt", std::ios_base::app);
oTError << runName << "\n\t"; statsFiltering.appendTo(oTError); oTError << "\n\n";
oTError.close();
// append error for each run to a file
std::ofstream oSError("/tmp/errorsSmoothing.txt", std::ios_base::app);
oSError << runName << "\n\t"; statsSmoothing.appendTo(oSError); oSError << "\n\n";
oSError.close();
// append error for each run to a file
std::ofstream oTDError("/tmp/errorsDistFiltering.txt", std::ios_base::app);
oTDError << runName << "\n\t"; statsDistFiltering.appendTo(oTDError); oTDError << "\n\n";
oTDError.close();
// append error for each run to a file
std::ofstream oSDError("/tmp/errorsDistSmoothing.txt", std::ios_base::app);
oSDError << runName << "\n\t"; statsDistSmoothing.appendTo(oSDError); oSDError << "\n\n";
oSDError.close();
// plot-data
std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close(); // ground truth
std::ofstream oEstN("/tmp/est_norm_" + runName + ".dat"); vis.estPath.addDataTo(oEstN); oEstN.close(); // estimation via filter itself
std::ofstream oEstS("/tmp/est_smooth_" + runName + ".dat"); vis.smoothPath.addDataTo(oEstS); oEstS.close(); // estimation via smoothing
std::ofstream oFloor("/tmp/floors.dat"); vis.floors.addDataTo(oFloor); oFloor.close();
std::ofstream oPlot("/tmp/plot_" + runName + ".gp");
oPlot << "set terminal eps size 3.4,2\n";
oPlot << "set output '" << runName << ".eps'\n";
oPlot << "set termoption dashlength 0.5\n";
oPlot << "set ticslevel 0\n";
oPlot << "set view equal xy\n";
oPlot << "set zrange [0:2200]\n";
oPlot << "set multiplot layout 1,1 scale 2.7,2.7 offset 0,0.23\n";
oPlot << "set view 72,33\n";
oPlot << "unset border\n";
oPlot << "unset xtics\n";
oPlot << "unset ytics\n";
oPlot << "unset ztics\n";
oPlot << "splot \\\n";
oPlot << "'floors.dat' skip 21 notitle with lines lc rgb '#777777', \\\n";
oPlot << "'path_" << runName << ".dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
oPlot << "'est_norm_" << runName << ".dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099', ";
oPlot << "'est_smooth_" << runName << ".dat' skip 21 notitle with lines lw 2.5 lc rgb '#000000' ";
oPlot.close();
{
std::ofstream oErrPlot("/tmp/plot_err_" + runName + ".gp");
oErrPlot << "plot \\\n";
oErrPlot << "'err_norm_" << runName << ".dat' with lines, \\\n";
oErrPlot << "'err_smooth_" << runName << ".dat' with lines";
oErrPlot.close();
}
{
std::ofstream oErrPlot("/tmp/plot_dist_err_" + runName + ".gp");
oErrPlot << "plot \\\n";
oErrPlot << "'err_dist_norm_" << runName << ".dat' with lines, \\\n";
oErrPlot << "'err_dist_smooth_" << runName << ".dat' with lines";
oErrPlot.close();
}
}
};
#endif // EVALBASE_H