FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

updates the visualisation

Browse Source 
removed obsolte parts
fixed baromter stuff
worked on eval
added ground-truth
This commit is contained in:
FrankE
2016-01-30 19:50:58 +01:00
parent b2ea3145f4
commit cc899d1c46
18 changed files with 388 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
code/eval/Eval.h
View File

@@ -9,6 +9,8 @@
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.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 Eval : public EvalBase {
@@ -26,15 +28,16 @@ public:
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
//GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
//GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv");
srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt");
srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt");
//gtw = getGroundTruthWay(*sr, gtwp, way2);
gtw = getGroundTruthWay(*sr, floors.gtwp, way2);
}
@@ -47,7 +50,7 @@ public:
// the particle filter's evaluation method
std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
eval.get()->setUsage(true, false, false, false, false);
eval.get()->setUsage(true, false, false, true, true);
pf->setEvaluation( std::move(eval) );
// resampling step?
@@ -56,6 +59,7 @@ public:
// state estimation step
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
}

88
code/eval/EvalBase.h
View File

@@ -10,7 +10,6 @@
#include "GroundTruthWay.h"
#include "../particles/P3.h"
#include "../particles/MyState.h"
#include "../particles/MyObservation.h"
#include "../particles/MyEvaluation.h"
@@ -43,10 +42,15 @@ protected:
SensorReaderTurn* srt;
SensorReaderStep* srs;
GroundTruthWay gtw;
std::string runName;
GroundTruthWay gtw;
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};
public:
EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) {
@@ -62,11 +66,39 @@ public:
}
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);
assert(it != waypoints.end());
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() {
// read CSV input
// const int s_wifi = 0;
//SensorReader sr("/apps/workspaces/ipin2015/measurements/2/1427362412784.csv");
// sensor numbers
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5;
const int s_linearAcceleration = 2;
@@ -111,10 +143,10 @@ public:
std::vector<Point3> pathEst;
uint64_t lastTransitionTS = 0;
bool firstReading = true;
int64_t start_time = -1;
K::Statistics<double> stats;
int cnt = 0;
// process each sensor reading
while(sr->hasNext()) {
@@ -127,12 +159,6 @@ public:
if (start_time == -1) {start_time = se.ts;}
int64_t current_time = se.ts - start_time;
// ensure the graph timestamp starts with the first reading
if (firstReading) {
//vis.debugProcess(se.ts, pathEst, gtw, pf, layers);
firstReading = false;
}
switch(se.idx) {
case s_wifi: {
@@ -203,37 +229,33 @@ public:
}
// let the transition know the current timestamp to determine the time since the last transition
//if (!useSimpleTrans) {
((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
//} else {
// ((MyTransitionSimple*)pf->getTransition())->setCurrentTime(lastTransitionTS);
//}
// 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(nullptr, obs);
const Point3 curEst = est.pCur;
pathEst.push_back(curEst);
// debug print current particle set.
//vis.debugProcess(se.ts, pathEst, gtw, pf, layers);
// error calculation. compare ground-truth to estimation
// const Point3 curGT = gtw.getPosAtTime(se.ts - 750);
const Point3 curGT = gtw.getPosAtTime(se.ts - 750);
const Point3 diff = curEst - curGT;
// // TODO
// const Point3 diff = curEst - curGT;
// //if (std::abs(diff.z) < 0.1) {
// const float err = diff.length();
// std::cout << err << std::endl;
// stats.add(err);
// std::cout << stats.asString() << std::endl;
// //}
// skip the first 8 scans due to uniform distribution start
if (++cnt > 8) {
pathEst.push_back(curEst);
const float err = diff.length();
stats.add(err);
std::cout << stats.asString() << std::endl;
}
// plot
vis.clearStates();
for (const K::Particle<MyState> p : pf->getParticles()) {vis.addState(p.state.walkState);}
vis.setTimestamp(se.ts);
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.setEstAndShould(curEst, curGT);
vis.show();;
}

2
code/eval/GroundTruthWay.h
View File

@@ -16,7 +16,7 @@ public:
}
/** get the ground truth way */
const std::vector<Entry>& getWay() const {return entries;}
const std::vector<InterpolatorEntry>& getWay() const {return entries;}
};