460 lines
17 KiB
C++
460 lines
17 KiB
C++
#ifndef FIXEDLAGEVALBASE_H
|
|
#define FIXEDLAGEVALBASE_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 "../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<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:
|
|
|
|
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<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_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;
|
|
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;
|
|
int cnt = 0;
|
|
|
|
//stats file
|
|
std::ofstream statsout("/tmp/unsmoothed_" + runName + ".stats");
|
|
|
|
//heading
|
|
double currentHeadingGivenByLukas = 0.0;
|
|
|
|
// 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 * 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);
|
|
|
|
currentHeadingGivenByLukas = 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;
|
|
|
|
//EXPERIMENTAL: Set all Particle Angles to the estimated angle of the particle set
|
|
// if(cnt % 30 == 0){
|
|
// pf->setAngle(est.avgAngle * 3.14159265359 / 180);
|
|
// }
|
|
|
|
// 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 > 35) {
|
|
statsFiltering.add(err);
|
|
std::cout << "Filtering: " << se.ts << " " << statsFiltering.asString() << std::endl;
|
|
}
|
|
|
|
if(cnt > 35){
|
|
//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[(tsHistory.size() - 1) - 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 curGTSmoothed = gtw.getPosAtTime(tsHistory[(tsHistory.size() - 1) - MiscSettings::lag]);
|
|
const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
|
|
|
|
const float errSmoothed = diffSmoothed.length();
|
|
statsSmoothing.add(errSmoothed);
|
|
std::cout << "Smoothing: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.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(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";
|
|
|
|
//double avgAngleRad = estBF.avgAngle * 180/3.14159265359;
|
|
|
|
//std::cout << "Measurement: "<< std::fmod((-(obs.orientation.values[0] * 180/3.14159265359) + 720 + 30), 360) << std::endl;
|
|
|
|
|
|
//std::cout << "Measurement: "<< std::fmod(((obs.orientation.values[0] * 180/3.14159265359) + 720 + 60), 360) << std::endl;
|
|
|
|
// std::cout << "MeasurementLukas: " << currentHeadingGivenByLukas << std::endl;
|
|
// std::cout << "EstimationS: " << estBF.avgAngle << std::endl;
|
|
// std::cout << "EstimationF: " << est.avgAngle << std::endl;
|
|
//vis.gp << "set label 113 ' EstAngle:" << avgAngleRad << "' 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);
|
|
}
|
|
}
|
|
|
|
}
|
|
statsout.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();
|
|
|
|
// 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();
|
|
|
|
// // plot-data
|
|
// std::ofstream oPath("/tmp/path_" + runName + ".dat"); vis.groundTruth.addDataTo(oPath); oPath.close();
|
|
// std::ofstream oEst("/tmp/est_" + runName + ".dat"); vis.estPath.addDataTo(oEst); oEst.close();
|
|
// 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_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
|
|
// oPlot << "'est_bergwerk_path2_nexus_shortest.dat' skip 21 notitle with lines lw 2.5 lc rgb '#000099' ";
|
|
|
|
// oPlot.close();
|
|
}
|
|
|
|
};
|
|
|
|
#endif // EVALBASE_H
|