767 lines
23 KiB
C++
Executable File
767 lines
23 KiB
C++
Executable File
#ifndef EVALWALK_H
|
|
#define EVALWALK_H
|
|
|
|
#include <KLib/math/filter/particles/ParticleFilter.h>
|
|
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
|
|
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
|
|
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
|
|
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
|
|
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingNEff.h>
|
|
|
|
#include "../plots/PlotErrFunc.h"
|
|
|
|
#include <thread>
|
|
|
|
#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
|
|
#include "Indoor/sensors/radio/setup/WiFiFingerprint.h"
|
|
#include "Indoor/sensors/radio/setup/WiFiFingerprints.h"
|
|
|
|
#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
|
|
#include "Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h"
|
|
|
|
#include "Indoor/sensors/radio/VAPGrouper.h"
|
|
#include "Indoor/sensors/imu/StepDetection.h"
|
|
#include "Indoor/sensors/imu/TurnDetection.h"
|
|
#include "Indoor/sensors/activity/ActivityDetector.h"
|
|
|
|
#include "Indoor/floorplan/v2/Floorplan.h"
|
|
#include "Indoor/floorplan/v2/FloorplanReader.h"
|
|
#include "Indoor/floorplan/v2/FloorplanHelper.h"
|
|
#include "Indoor/floorplan/v2/FloorplanCeilings.h"
|
|
|
|
#include "Indoor/sensors/radio/model/WiFiModels.h"
|
|
|
|
#include "Indoor/sensors/offline/FileReader.h"
|
|
#include "../Helper.h"
|
|
#include "PF.h"
|
|
#include "../plots/PlotErrTime.h"
|
|
|
|
#include <Indoor/debug/PlotWifiMeasurements.h>
|
|
|
|
#include <Indoor/sensors/offline/FilePlayer.h>
|
|
#include <Indoor/sensors/offline/FileReader.h>
|
|
#include <Indoor/sensors/offline/Listener.h>
|
|
|
|
|
|
//#define PLOT_LIVE
|
|
//#define PLOT_WIFI
|
|
//#define PLOT_ERROR_TIME
|
|
//#define PLOT_ERROR_FUNC
|
|
|
|
#define PLOT_UPDATE_STEP 20
|
|
|
|
|
|
class EvalWalk : public Offline::Listener {
|
|
|
|
Grid<MyGridNode>* grid;
|
|
K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
|
|
|
|
std::string runName;
|
|
|
|
#ifdef PLOT_LIVE
|
|
Plotty plotty;
|
|
#endif
|
|
|
|
#ifdef PLOT_WIFI
|
|
PlotWifiMeasurements plotWifi;
|
|
#endif
|
|
|
|
#ifdef PLOT_ERROR_TIME
|
|
PlotErrTime pet;
|
|
#endif
|
|
|
|
#ifdef PLOT_ERROR_FUNC
|
|
PlotErrFunc pef;
|
|
#endif
|
|
|
|
Offline::FileReader reader;
|
|
Offline::FilePlayer player;
|
|
Offline::FileReader::GroundTruth groundTruthLive;
|
|
|
|
Timestamp lastTransition;
|
|
|
|
MyObservation curObs;
|
|
MyControl curCtrl;
|
|
MyState curEst;
|
|
|
|
StepDetection stepDetect;
|
|
TurnDetection turnDetect;
|
|
ActivityDetector actDetect;
|
|
|
|
std::vector<Point3> groundTruth;
|
|
|
|
Floorplan::IndoorMap* map;
|
|
|
|
EarthMapping em;
|
|
|
|
std::string walkName;
|
|
|
|
float absHead = 0;
|
|
|
|
public:
|
|
|
|
~EvalWalk() {
|
|
delete grid;
|
|
delete pf;
|
|
}
|
|
|
|
EvalWalk(Floorplan::IndoorMap* map) :
|
|
|
|
#ifdef PLOT_LIVE
|
|
plotty(map),
|
|
#endif
|
|
|
|
map(map), em(map)
|
|
|
|
#ifdef PLOT_ERROR_TIME
|
|
,pet("\\small{time (sec)}", "\\small{error (m)}", "\\small{APs visible}")
|
|
#endif
|
|
#ifdef PLOT_ERROR_FUNC
|
|
,pef("\\small{error (m)}", "\\small{updates (\\%)}")
|
|
#endif
|
|
{
|
|
|
|
const std::string saveFile = Settings::pathData + "/grid.dat";
|
|
grid = new Grid<MyGridNode>(Settings::Grid::gridSize_cm);
|
|
|
|
#ifdef PLOT_LIVE
|
|
// once
|
|
plotty.buildFloorplan();
|
|
#endif
|
|
|
|
// deserialize grid
|
|
std::ifstream inp(saveFile, std::ofstream::binary);
|
|
if (inp) {
|
|
grid->read(inp);
|
|
inp.close();
|
|
} else {
|
|
// build the grid
|
|
GridFactory<MyGridNode> gf(*grid);
|
|
gf.build(map);
|
|
Importance::addImportance(*grid);
|
|
std::ofstream out(saveFile, std::ofstream::binary);
|
|
grid->write(out);
|
|
out.close();
|
|
}
|
|
|
|
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)) );
|
|
|
|
|
|
// transition
|
|
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
|
|
|
|
// resampling step?
|
|
pf->setNEffThreshold(0.02);
|
|
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
|
|
|
|
//pf->setNEffThreshold(0.75);
|
|
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );
|
|
|
|
//pf->setNEffThreshold(0.75);
|
|
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.05)) );
|
|
|
|
// K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.75, 0.05);
|
|
// pf->setNEffThreshold(1.0);
|
|
// pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(res) );
|
|
|
|
// move during resampling. NOT ALLOWED!
|
|
// res->setDrawCallback([&] (K::Particle<MyState>& p) {
|
|
// static std::minstd_rand gen;
|
|
// static int cnt = 0; ++cnt;
|
|
// bool init = cnt < pf->getParticles().size() * 50;
|
|
|
|
// const MyGridNode* n = grid->getNodePtrFor(p.state.position);
|
|
// std::normal_distribution<float> distTurn(0, (init) ? (0.5) : (0.05));
|
|
// for (int j = 0; j < 2; ++j) {
|
|
// std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
|
|
// const int idx = distIdx(gen);
|
|
// n = &(grid->getNeighbor(*n, idx));
|
|
// }
|
|
// p.state.position = *n;
|
|
// p.state.heading.direction += distTurn(gen);
|
|
// });
|
|
|
|
// 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>()));
|
|
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.25f)));
|
|
|
|
}
|
|
|
|
struct LeWalk {
|
|
std::string name;
|
|
float absHead;
|
|
std::string walkFile;
|
|
std::vector<int> gtIndices;
|
|
LeWalk(const std::string& name, float absHead, std::string walkFile, std::vector<int> gtIndices) :
|
|
name(name), absHead(absHead), walkFile(walkFile), gtIndices(gtIndices) {
|
|
;
|
|
}
|
|
};
|
|
|
|
struct LeModel {
|
|
std::string name;
|
|
std::string plotKey;
|
|
WiFiModel* model;
|
|
LeModel(const std::string& name, const std::string& plotKey, WiFiModel* model) : name(name), plotKey(plotKey), model(model) {;}
|
|
};
|
|
|
|
static void serialize(const K::Statistics<float>& stats, const LeWalk& walk, const LeModel& model, const int idx) {
|
|
const std::string file = Settings::fPathGFX + "/walks/data/" + model.name + "/" + walk.name + "_" + model.name + "_" + std::to_string(idx) + ".txt";
|
|
std::ofstream out(file);
|
|
for (const float f : stats.getAll()) {
|
|
out << f << "\n";
|
|
}
|
|
out.close();
|
|
}
|
|
|
|
static K::Statistics<float>* deserialize(const LeWalk& walk, const LeModel& model, const int idx) {
|
|
const std::string file = Settings::fPathGFX + "/walks/data/" + model.name + "/" + walk.name + "_" + model.name + "_" + std::to_string(idx) + ".txt";
|
|
K::Statistics<float>* res = new K::Statistics<float>();
|
|
std::ifstream inp(file);
|
|
while(inp) {
|
|
float f;
|
|
inp >> f;
|
|
res->add(f);
|
|
}
|
|
return res;
|
|
}
|
|
|
|
static std::vector<LeWalk> allWalks() {
|
|
return {
|
|
LeWalk("path1a", M_PI/2, Settings::path1a, Settings::GroundTruth::path1),
|
|
LeWalk("path1b", M_PI/2, Settings::path1b, Settings::GroundTruth::path1),
|
|
LeWalk("toni-all-1a", M_PI/2, Settings::path_toni_all_1a, Settings::GroundTruth::path1),
|
|
LeWalk("toni-all-1b", M_PI/2, Settings::path_toni_all_1b, Settings::GroundTruth::path1),
|
|
LeWalk("path2a", 0, Settings::path2a, Settings::GroundTruth::path2),
|
|
LeWalk("path2b", 0, Settings::path2b, Settings::GroundTruth::path2),
|
|
LeWalk("toni-all-2a", M_PI/2, Settings::path_toni_all_2a, Settings::GroundTruth::path2),
|
|
LeWalk("toni-all-2b", M_PI/2, Settings::path_toni_all_2b, Settings::GroundTruth::path2),
|
|
/////////LeWalk("toni-inst-1a", M_PI/2, Settings::path_toni_inst_1a, Settings::GroundTruth::path_toni_inst_1),
|
|
LeWalk("toni-inst-1b", M_PI/2, Settings::path_toni_inst_1b, Settings::GroundTruth::path_toni_inst_1),
|
|
LeWalk("toni-inst-2a", M_PI/2, Settings::path_toni_inst_2a, Settings::GroundTruth::path_toni_inst_2),
|
|
LeWalk("toni-inst-2b", M_PI/2, Settings::path_toni_inst_2b, Settings::GroundTruth::path_toni_inst_2),
|
|
LeWalk("toni-inst-3a", M_PI/2, Settings::path_toni_inst_3a, Settings::GroundTruth::path_toni_inst_3),
|
|
LeWalk("toni-inst-3b", M_PI/2, Settings::path_toni_inst_3b, Settings::GroundTruth::path_toni_inst_3),
|
|
|
|
};
|
|
}
|
|
|
|
static std::vector<LeModel> allModels(Floorplan::IndoorMap* map) {
|
|
WiFiModelFactory fac(map);
|
|
return {
|
|
LeModel("empiric", "\\noOptEmpiric{}", fac.loadXML(Settings::wifiAllFixed)),
|
|
LeModel("opt 1", "\\optParamsAllAP{}", fac.loadXML(Settings::wifiAllOptPar)),
|
|
LeModel("opt 2", "\\optParamsEachAP{}", fac.loadXML(Settings::wifiEachOptPar)),
|
|
LeModel("opt 3", "\\optParamsPosEachAP{}", fac.loadXML(Settings::wifiEachOptParPos)),
|
|
LeModel("per floor", "\\optPerFloor{}", fac.loadXML(Settings::wifiEachOptParPos_multimodel)),
|
|
LeModel("per bbox", "\\optPerRegion{}", fac.loadXML(Settings::wifiEachOptParPos_perBBox)),
|
|
};
|
|
}
|
|
|
|
static constexpr int numRepeats = 25;
|
|
|
|
/** create GFX for all previously walked parts */
|
|
static void walkEverythingBuildStats(Floorplan::IndoorMap* map) {
|
|
|
|
const std::vector<LeWalk> walks = allWalks();
|
|
const std::vector<LeModel> models = allModels(map);
|
|
|
|
PlotErrFunc* pefAll = new PlotErrFunc();
|
|
pefAll->setYRange(0, 90, 5);
|
|
pefAll->getPlot().getAxisY().setRange(0, 90);
|
|
pefAll->getPlot().getAxisY().setTicsStep({0, 25, 50, 75, 90});
|
|
pefAll->getPlot().getAxisX().setRange(0, 15);
|
|
|
|
for (const LeModel& mdl : models) {
|
|
|
|
K::Statistics<float>* modelStatsAvg = new K::Statistics<float>();
|
|
K::Statistics<float>* modelStatsMed = new K::Statistics<float>();
|
|
|
|
K::Statistics<float>* modelStatsSingle = new K::Statistics<float>();
|
|
pefAll->add(mdl.plotKey, modelStatsSingle);
|
|
|
|
|
|
int numStuck = 0;
|
|
int numTotal = 0;
|
|
|
|
std::cout << "model " << mdl.name << std::endl;
|
|
|
|
|
|
// ... to perform every walk...
|
|
for (const LeWalk& walk : walks) {
|
|
|
|
int numStuckWalk = 0;
|
|
int numTotalWalk = 0;
|
|
|
|
// stats for all runs of one walk
|
|
K::Statistics<float> walkStatsAvg;
|
|
|
|
for (int i = 0; i < 75; ++i) {
|
|
|
|
++numTotal;
|
|
++numTotalWalk;
|
|
|
|
K::Statistics<float>* walkStats = deserialize(walk, mdl, i);
|
|
|
|
//if (walkStats->getMedian() < 15) {
|
|
if (walkStats->getQuantile(0.75) < 20) {
|
|
//modelStatsSingle->add(*walkStats);
|
|
} else {
|
|
++numStuck;
|
|
++numStuckWalk;
|
|
}
|
|
|
|
modelStatsSingle->add(*walkStats);
|
|
walkStatsAvg.add(*walkStats);
|
|
|
|
}
|
|
|
|
if (modelStatsSingle->getCount() > 0) {
|
|
pefAll->plot();
|
|
}
|
|
|
|
std::cout << "|" << walk.name << "(" << walkStatsAvg.getMedian() << "@" << (numStuckWalk*100/numTotalWalk) << "%)\t";
|
|
|
|
}
|
|
|
|
std::cout << " | OVERALL:" << "(" << modelStatsSingle->getMedian() << "@" << (numStuck*100/numTotal) << "%)" << std::endl;
|
|
sleep(1);
|
|
|
|
}
|
|
|
|
pefAll->getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 3.7));
|
|
pefAll->getGP().setOutput(Settings::fPathGFX + "/overall-system-error.tex");
|
|
pefAll->writePlotToFile(Settings::fPathGFX + "/overall-system-error.gp");
|
|
pefAll->getPlot().getMargin().set(4, 0.2, 0.1, 1.8);
|
|
pefAll->getPlot().setStringMod(new K::GnuplotStringModLaTeX());
|
|
pefAll->getPlot().getKey().setVisible(true);
|
|
pefAll->getPlot().getKey().setPosition(K::GnuplotKey::Hor::RIGHT, K::GnuplotKey::Ver::BOTTOM);
|
|
pefAll->getPlot().getKey().setWidthIncrement(7);
|
|
pefAll->getPlot().getAxisX().setTicsLabelFormat("%h m");
|
|
pefAll->getPlot().getAxisX().setRange(0, 22);
|
|
pefAll->getPlot().getAxisX().setLabel("");
|
|
pefAll->getPlot().getAxisY().setLabel("filter updates (%)");
|
|
pefAll->getPlot().getAxisY().setLabelOffset(3.0, 0);
|
|
pefAll->plot();
|
|
|
|
sleep(100);
|
|
|
|
|
|
}
|
|
|
|
/** perform all walks and write them to file */
|
|
static void walkEverything(Floorplan::IndoorMap* map) {
|
|
|
|
const std::vector<LeWalk> walks = allWalks();
|
|
const std::vector<LeModel> models = allModels(map);
|
|
|
|
PlotErrFunc* pefAll = new PlotErrFunc();
|
|
|
|
// use every model ...
|
|
for (const LeModel& mdl : models) {
|
|
|
|
K::Statistics<float>* modelStatsSingle = new K::Statistics<float>();
|
|
pefAll->add(mdl.name, modelStatsSingle);
|
|
|
|
// ... to perform every walk...
|
|
for (const LeWalk& walk : walks) {
|
|
|
|
//K::Statistics<float> curWalkRepeatStatsAvg;
|
|
//K::Statistics<float> curWalkRepeatStatsMed;
|
|
|
|
// ... several times
|
|
//#pragma omp parallel for num_threads(2)
|
|
for (int i = numRepeats*2; i < numRepeats*3; ++i) {
|
|
|
|
EvalWalk ew(map);
|
|
ew.walkName = walk.name + " - " + mdl.name + " - " + std::to_string(i);
|
|
|
|
// get ground-truth
|
|
ew.absHead = walk.absHead;
|
|
ew.groundTruth = FloorplanHelper::getGroundTruth(map, walk.gtIndices);
|
|
|
|
// eval
|
|
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(ew.grid, *mdl.model, ew.em) );
|
|
ew.pf->setEvaluation( std::move(eval) );
|
|
|
|
// data-file
|
|
ew.reader.open(walk.walkFile);
|
|
ew.groundTruthLive = ew.reader.getGroundTruth(map, walk.gtIndices);
|
|
ew.player.setReader(&ew.reader);
|
|
ew.player.setListener(&ew);
|
|
ew.player.start();
|
|
|
|
// wait for completion
|
|
ew.player.join();
|
|
|
|
// write plots
|
|
ew.writeToFile();
|
|
|
|
// add every single error for each timestamp to the overall model error
|
|
modelStatsSingle->reset();
|
|
modelStatsSingle->add(ew.statsErr);
|
|
|
|
// export this walks's statistics
|
|
serialize(ew.statsErr, walk, mdl, i);
|
|
|
|
pefAll->plot();
|
|
|
|
}
|
|
|
|
// all repeats done
|
|
const std::string walkname = walk.name + " - " + mdl.name;
|
|
std::cout << "walk result for " << walkname << std::endl;
|
|
//std::cout << "\t" << curWalkRepeatStatsAvg.asString() << std::endl;
|
|
//std::cout << "\t" << curWalkRepeatStatsMed.asString() << std::endl;
|
|
std::cout << std::endl;
|
|
//sleep(1);
|
|
|
|
}
|
|
|
|
// all walks for one model done
|
|
std::cout << "MODEL RESULTS for " << mdl.name << std::endl;
|
|
//std::cout << "\t" << modelStatsAvg->asString() << std::endl;
|
|
//std::cout << "\t" << modelStatsMed->asString() << std::endl;
|
|
std::cout << std::endl;
|
|
std::cout << std::endl;
|
|
std::cout << std::endl;
|
|
|
|
sleep(2);
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void walk1() {
|
|
|
|
// path1
|
|
// absHead = M_PI/2;
|
|
// const std::string path = Settings::path1b;
|
|
// const std::vector<int> pathPoints = Settings::GroundTruth::path1;
|
|
|
|
// path2
|
|
// absHead = 0;
|
|
// const std::string path = Settings::path2b;
|
|
// const std::vector<int> pathPoints = Settings::GroundTruth::path2;
|
|
|
|
// path_toni_inst_2
|
|
absHead = M_PI;
|
|
const std::string path = Settings::path_toni_inst_2b;
|
|
const std::vector<int> pathPoints = Settings::GroundTruth::path_toni_inst_2;
|
|
|
|
runName = "";
|
|
|
|
// get ground-truth
|
|
groundTruth = FloorplanHelper::getGroundTruth(map, pathPoints);
|
|
|
|
// wifi model
|
|
//WiFiModelLogDistCeiling wifiModel(map);
|
|
//wifiModel.loadXML(Settings::wifiAllFixed);
|
|
//wifiModel.loadXML(Settings::wifiEachOptParPos);
|
|
|
|
//WiFiModelPerFloor wifiModel(map);
|
|
//wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);
|
|
|
|
WiFiModelPerBBox wifiModel(map);
|
|
wifiModel.loadXML(Settings::wifiEachOptParPos_perBBox);
|
|
|
|
// eval
|
|
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel, em) );
|
|
pf->setEvaluation( std::move(eval) );
|
|
|
|
// data-file
|
|
reader.open(path);
|
|
groundTruthLive = reader.getGroundTruth(map, pathPoints);
|
|
player.setReader(&reader);
|
|
player.setListener(this);
|
|
player.start();
|
|
|
|
// wait for completion
|
|
player.join();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) override {
|
|
|
|
const float delta_rad = turnDetect.addGyroscope(ts, data);
|
|
curCtrl.turnSinceLastTransition_rad += delta_rad;
|
|
|
|
}
|
|
|
|
virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) override {
|
|
turnDetect.addAccelerometer(ts, data);
|
|
const bool step = stepDetect.add(ts, data);
|
|
if (step) {
|
|
++curCtrl.numStepsSinceLastTransition;
|
|
}
|
|
gotSensorData(ts);
|
|
actDetect.add(ts, data);
|
|
}
|
|
|
|
virtual void onGravity(const Timestamp ts, const GravityData data) override {
|
|
;
|
|
}
|
|
|
|
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) override {
|
|
curObs.wifi = data;
|
|
//curObs.wifi = WiFiMeasurements::mix(curObs.wifi, data);
|
|
#ifdef PLOT_WIFI
|
|
plotWifi.add(Settings::WiFiModel::vg_eval.group(data));
|
|
plotWifi.plot();
|
|
#endif
|
|
}
|
|
|
|
virtual void onBarometer(const Timestamp ts, const BarometerData data) override {
|
|
actDetect.add(ts, data);
|
|
curCtrl.activityNew = actDetect.get();
|
|
curObs.activityNew = actDetect.get();
|
|
}
|
|
|
|
virtual void onGPS(const Timestamp ts, const GPSData data) override {
|
|
curObs.gps = data;
|
|
}
|
|
|
|
virtual void onCompass(const Timestamp ts, const CompassData data) override {
|
|
const float newAzimuth =- data.azimuth + M_PI/2; // oriented towards north for our map
|
|
const float newAzimuth_safe = Angle::makeSafe_2PI(newAzimuth);
|
|
const float diff = Angle::getSignedDiffRAD_2PI(curCtrl.compassAzimuth_rad, newAzimuth_safe);
|
|
curCtrl.compassAzimuth_rad += diff * 0.01;
|
|
curCtrl.compassAzimuth_rad = Angle::makeSafe_2PI(curCtrl.compassAzimuth_rad);
|
|
curObs.compassAzimuth_rad = curCtrl.compassAzimuth_rad;
|
|
//curCtrl.compassAzimuth_rad = curCtrl.compassAzimuth_rad * 0.99 + newAzimuth * 0.01;
|
|
}
|
|
|
|
|
|
private:
|
|
|
|
/** called when any sensor has received new data */
|
|
void gotSensorData(const Timestamp ts) {
|
|
curObs.currentTime = ts;
|
|
filterUpdateIfNeeded();
|
|
}
|
|
|
|
/** check whether its time for a filter update, and if so, execute the update and return true */
|
|
bool filterUpdateIfNeeded() {
|
|
|
|
static float avgSum = 0;
|
|
static int avgCount = 0;
|
|
|
|
// fixed update rate based on incoming sensor data
|
|
// allows working with live data and faster for offline data
|
|
const Timestamp diff = curObs.currentTime - lastTransition;
|
|
if (diff >= Settings::Filter::updateEvery) {
|
|
|
|
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
|
|
const Timestamp err = diff - Settings::Filter::updateEvery;
|
|
lastTransition = curObs.currentTime - err;
|
|
|
|
const Timestamp ts1 = Timestamp::fromUnixTime();
|
|
filterUpdate();
|
|
const Timestamp ts2 = Timestamp::fromUnixTime();
|
|
const Timestamp tsDiff = ts2-ts1;
|
|
|
|
//const QString filterTime = QString::number(tsDiff.ms());
|
|
//avgSum += tsDiff.ms(); ++avgCount;
|
|
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
|
|
return true;
|
|
|
|
} else {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
K::Statistics<float> statsErr;
|
|
int updateCount = 0;
|
|
|
|
int getNumFHWSAPs(const WiFiMeasurements& mes) {
|
|
std::unordered_set<std::string> set;
|
|
for (const WiFiMeasurement& m : mes.entries) {
|
|
std::string mac = m.getAP().getMAC().asString();
|
|
mac.back() = '0';
|
|
set.insert(mac);
|
|
}
|
|
return set.size();
|
|
}
|
|
|
|
|
|
void writeToFile() {
|
|
|
|
std::string path = Settings::fPathGFX;
|
|
std::string base = path + "/walks/" + "walk-" + walkName;
|
|
|
|
#ifdef PLOT_LIVE
|
|
plotty.gp << "unset arrow 1\n";
|
|
plotty.gp << "unset arrow 2\n";
|
|
plotty.gp << "unset colorbox\n";
|
|
plotty.gp << "unset border\n";
|
|
plotty.gp << "set view equal xy\n";
|
|
plotty.splot.getView().setCamera(74,30);
|
|
plotty.splot.getView().setScaleAll(3.5);
|
|
plotty.splot.getAxisX().setTicsVisible(false);
|
|
plotty.splot.getAxisY().setTicsVisible(false);
|
|
plotty.splot.getAxisZ().setTicsVisible(false);
|
|
|
|
plotty.particles.clear();
|
|
plotty.gp.setTerminal("epslatex", K::GnuplotSize(8.6, 4.5));
|
|
plotty.gp.setOutput(base + "-map.tex");
|
|
plotty.writeCodeTo(base + "-map.gp");
|
|
plotty.plot();
|
|
#endif
|
|
|
|
#ifdef PLOT_ERROR_FUNC
|
|
pef.getPlot().getMargin().set(4, 0.2, 0.1, 2.0);
|
|
pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 3.0));
|
|
pef.getGP().setOutput(base + "-error-cum.tex");
|
|
pef.writePlotToFile(base + "-error-cum.gp");
|
|
pef.plot();
|
|
#endif
|
|
|
|
#ifdef PLOT_ERROR_TIME
|
|
pet.getPlot().getMargin().set(4, 0.2, 0.1, 2.0);
|
|
pet.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 3.0));
|
|
pet.getGP().setOutput(base + "-error-time.tex");
|
|
pet.writePlotToFile(base + "-error-time.gp");
|
|
pet.plot();
|
|
#endif
|
|
|
|
}
|
|
|
|
/** perform a filter-update (called from a background-loop) */
|
|
void filterUpdate() {
|
|
|
|
++updateCount;
|
|
|
|
MyControl ctrlCopy = curCtrl;
|
|
//static float absHead = relHeadingOffset;
|
|
absHead += ctrlCopy.turnSinceLastTransition_rad;
|
|
|
|
//lastEst = curEst;
|
|
curEst = pf->update(&curCtrl, curObs);
|
|
|
|
const Point3 curGT = groundTruthLive.get(lastTransition);
|
|
|
|
// start the error-over-time plot after some filter updates
|
|
if (updateCount > 12) {
|
|
|
|
// error between ground-truth and estimation
|
|
const float estRealErr = curEst.position.inMeter().getDistance(curGT);
|
|
statsErr.add(estRealErr);
|
|
|
|
#ifdef PLOT_ERROR_FUNC
|
|
pef.showMarkers(true, true);
|
|
pef.clear();
|
|
pef.add("", &statsErr);
|
|
#endif
|
|
|
|
#ifdef PLOT_ERROR_TIME
|
|
// timed error
|
|
pet.addErr(lastTransition, estRealErr);
|
|
pet.addB(lastTransition, getNumFHWSAPs(curObs.wifi));
|
|
#endif
|
|
|
|
#ifdef PLOT_LIVE
|
|
// update estimated path
|
|
const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm);
|
|
plotty.pathEst.add(p3/100);
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
// plot
|
|
static int cnt = 0;
|
|
if (++cnt % PLOT_UPDATE_STEP == 0) {
|
|
|
|
std::cout << statsErr.asString() << std::endl;
|
|
|
|
#ifdef PLOT_LIVE
|
|
// estimation and ground-truth
|
|
plotty.setCurEst(curEst.position.inMeter());
|
|
plotty.setGroundTruth(curGT);
|
|
|
|
// show particles
|
|
float maxWeight = 0;
|
|
float minWeight = 99;
|
|
plotty.particles.clear();
|
|
for (int i = 0; i < pf->getParticles().size(); i += 10) {
|
|
const auto p = pf->getParticles()[i];
|
|
const K::GnuplotPoint3 p3(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
|
|
plotty.particles.add(p3/100, p.weight);
|
|
if (p.weight > maxWeight) {maxWeight = p.weight;}
|
|
if (p.weight < minWeight) {minWeight = p.weight;}
|
|
}
|
|
plotty.gp << "set cbrange [" << minWeight << ":" << maxWeight << "] \n";
|
|
|
|
// show ground-truth
|
|
plotty.pathReal.clear();
|
|
for (const Point3 pt : groundTruth) {
|
|
plotty.pathReal.add(K::GnuplotPoint3(pt.x, pt.y, pt.z));
|
|
}
|
|
|
|
std::string title =
|
|
" time " + std::to_string(curObs.currentTime.sec()) +
|
|
" steps: " + std::to_string(ctrlCopy.numStepsSinceLastTransition) +
|
|
" turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad) +
|
|
" APs: " + std::to_string(curObs.wifi.entries.size()) +
|
|
" Act: " + std::to_string((int)curCtrl.activityNew) +
|
|
" " + walkName;
|
|
plotty.setTitle(title);
|
|
|
|
// relative heading and compass
|
|
{
|
|
Point2 cen(0.1, 0.9);
|
|
Point2 dir(std::cos(absHead), std::sin(absHead));
|
|
Point2 arr = cen + dir * 0.1;
|
|
plotty.gp << "set arrow 1 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
|
|
dir = Point2(std::cos(ctrlCopy.compassAzimuth_rad), std::sin(ctrlCopy.compassAzimuth_rad));
|
|
arr = cen + dir * 0.05;
|
|
plotty.gp << "set arrow 2 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
|
|
}
|
|
|
|
#endif
|
|
|
|
#ifdef PLOT_LIVE
|
|
plotty.plot();
|
|
#endif
|
|
|
|
#ifdef PLOT_ERROR_TIME
|
|
pet.plot();
|
|
#endif
|
|
|
|
#ifdef PLOT_ERROR_FUNC
|
|
pef.plot();
|
|
#endif
|
|
|
|
}
|
|
|
|
//std::this_thread::sleep_for(std::chrono::milliseconds(5));
|
|
|
|
curCtrl.resetAfterTransition();
|
|
|
|
// const MyGridNode* node = grid->getNodePtrFor(curEst.position);
|
|
// if (node) {
|
|
// try {
|
|
// pathToDest = trans->modDestination.getShortestPath(*node);
|
|
// } catch (...) {;}
|
|
// }
|
|
// mainController->getMapView()->showGridImportance();
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // EVALWALK_H
|