began putting everything together
This commit is contained in:
262
code/eval/EvalBase.h
Normal file
262
code/eval/EvalBase.h
Normal file
@@ -0,0 +1,262 @@
|
||||
#ifndef EVALBASE_H
|
||||
#define EVALBASE_H
|
||||
|
||||
#include "../Settings.h"
|
||||
#include "../Helper.h"
|
||||
#include "../Vis.h"
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilter.h>
|
||||
#include <KLib/math/statistics/Statistics.h>
|
||||
|
||||
#include "GroundTruthWay.h"
|
||||
|
||||
#include "../particles/P3.h"
|
||||
#include "../particles/MyState.h"
|
||||
#include "../particles/MyObservation.h"
|
||||
#include "../particles/MyEvaluation.h"
|
||||
#include "../particles/MyTransition.h"
|
||||
#include "../particles/MyInitializer.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"
|
||||
|
||||
class EvalBase {
|
||||
|
||||
protected:
|
||||
|
||||
Grid<MyGridNode> grid;
|
||||
Helper::FHWSFloors floors;
|
||||
Vis vis;
|
||||
|
||||
K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
|
||||
|
||||
SensorReader* sr;
|
||||
SensorReaderTurn* srt;
|
||||
SensorReaderStep* srs;
|
||||
|
||||
GroundTruthWay gtw;
|
||||
|
||||
std::string runName;
|
||||
|
||||
public:
|
||||
|
||||
EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) {
|
||||
|
||||
// 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);
|
||||
|
||||
}
|
||||
|
||||
void run() {
|
||||
|
||||
// read CSV input
|
||||
// const int s_wifi = 0;
|
||||
//SensorReader sr("/apps/workspaces/ipin2015/measurements/2/1427362412784.csv");
|
||||
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5;
|
||||
const int s_linearAcceleration = 2;
|
||||
|
||||
std::list<TurnObservation> turn_observations;
|
||||
std::list<StepObservation> 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;
|
||||
|
||||
|
||||
std::vector<Point3> pathEst;
|
||||
|
||||
uint64_t lastTransitionTS = 0;
|
||||
bool firstReading = true;
|
||||
int64_t start_time = -1;
|
||||
|
||||
K::Statistics<double> stats;
|
||||
|
||||
// process each 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;
|
||||
|
||||
// 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: {
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// scheduled transition every 500 ms
|
||||
if (lastTransitionTS == 0) {lastTransitionTS = se.ts;}
|
||||
for ( ; se.ts - lastTransitionTS > MiscSettings::timeSteps; lastTransitionTS += MiscSettings::timeSteps) {
|
||||
|
||||
//Steps are sorted in the list by timestamp.
|
||||
//If the current observation timestamp is bigger/equal
|
||||
//to the current step timestamp, use this step as observation
|
||||
//and remove it from the list.
|
||||
//The new first timestamp in the list will be then be the next one (timestamp-wise)
|
||||
StepObservation so;
|
||||
if(current_time >= step_observations.front().ts && !step_observations.empty()) {
|
||||
so.step = true;
|
||||
so.ts = current_time;
|
||||
|
||||
obs.step = &so;
|
||||
step_observations.pop_front();
|
||||
|
||||
}
|
||||
else {
|
||||
so.step = false;
|
||||
so.ts = current_time;
|
||||
|
||||
obs.step = &so;
|
||||
}
|
||||
|
||||
TurnObservation to;
|
||||
//same principal as for steps is applied for turns
|
||||
if(current_time >= turn_observations.front().ts && !turn_observations.empty()) {
|
||||
to = turn_observations.front();
|
||||
obs.turn = &to;
|
||||
|
||||
turn_observations.pop_front();
|
||||
}
|
||||
else {
|
||||
to.delta_heading = 0.0;
|
||||
to.delta_motion = 0.0;
|
||||
|
||||
obs.turn = &to;
|
||||
}
|
||||
|
||||
|
||||
// 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);
|
||||
//}
|
||||
|
||||
// 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);
|
||||
|
||||
// // 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;
|
||||
// //}
|
||||
|
||||
vis.clearStates();
|
||||
for (const K::Particle<MyState> p : pf->getParticles()) {vis.addState(p.state.walkState);}
|
||||
|
||||
vis.show();;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
{
|
||||
// vis.setShowParticles(false);
|
||||
// vis.setShowTime(false);
|
||||
// vis.setShowCurPos(false);
|
||||
// vis.debugProcess(0, pathEst, gtw, pf, layers);
|
||||
// std::ofstream out("/tmp/" + runName + ".data");
|
||||
// out << vis.getDataset();
|
||||
// out.close();
|
||||
}
|
||||
|
||||
sleep(1000);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // EVALBASE_H
|
||||
Reference in New Issue
Block a user