FHWS/OTHER2017
Archived
3
0
Fork 0
Code Issues 4 Pull Requests Releases Wiki Activity

next commit

Browse Source
This commit is contained in:
FrankE
2017-04-05 18:45:00 +02:00
parent 6171582b40
commit b2adb16b49
18 changed files with 1602 additions and 360 deletions
Download Patch File Download Diff File Expand all files Collapse all files

130
pf/EvalWalk.h
View File

@@ -8,8 +8,9 @@
#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 "../PlotErrFunc.h"
#include "../plots/PlotErrFunc.h"
#include <thread>
@@ -23,6 +24,7 @@
#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"
@@ -34,6 +36,9 @@
#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>
@@ -48,6 +53,7 @@ class EvalWalk : public Offline::Listener {
WiFiModelLogDistCeiling wifiModel;
Plotty plotty;
PlotWifiMeasurements plotWifi;
Offline::FileReader reader;
Offline::FilePlayer player;
@@ -61,18 +67,24 @@ class EvalWalk : public Offline::Listener {
StepDetection stepDetect;
TurnDetection turnDetect;
ActivityDetector actDetect;
std::vector<Point3> groundTruth;
Floorplan::IndoorMap* map;
EarthMapping em;
float absHead = 0;
public:
EvalWalk(Floorplan::IndoorMap* map) : wifiModel(map), plotty(map), map(map) {
EvalWalk(Floorplan::IndoorMap* map) : wifiModel(map), plotty(map), map(map), em(map) {
const std::string saveFile = Settings::pathData + "/grid.dat";
grid = new Grid<MyGridNode>(Settings::Grid::gridSize_cm);
// once
plotty.buildFloorplan();
// deserialize grid
@@ -93,17 +105,24 @@ public:
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)) );
// TODO: flexible model
wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF, false);
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel) );
pf->setEvaluation( std::move(eval) );
//wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF, false);
wifiModel.saveXML("/tmp/test.xml");
wifiModel.loadXML("/tmp/test.xml");
// transition
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
// resampling step?
pf->setNEffThreshold(0.5);
//pf->setNEffThreshold(0.5);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
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.10)) );
//pf->setNEffThreshold(0.75);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.05)) );
pf->setNEffThreshold(1.0);
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(new K::ParticleFilterResamplingNEff<MyState>(0.50, 0.05)) );
// state estimation step
pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
@@ -117,15 +136,32 @@ public:
void walk1() {
runName = "path2_forward_simple";
std::string path = Settings::path1a;
groundTruth = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path1);
// path1
absHead = M_PI/2;
const std::string path = Settings::path1b;
const std::vector<int> pathPoints = Settings::GroundTruth::path1;
//GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
// path2
// absHead = 0;
// const std::string path = Settings::path2a;
// const std::vector<int> pathPoints = Settings::GroundTruth::path2;
runName = "";
// get ground-truth
groundTruth = FloorplanHelper::getGroundTruth(map, pathPoints);
// wifi model
WiFiModelLogDistCeiling wifiModel(map);
wifiModel.loadXML(Settings::wifiEachOptParPos);
// 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, Settings::GroundTruth::path1);
groundTruthLive = reader.getGroundTruth(map, pathPoints);
player.setReader(&reader);
player.setListener(this);
player.start();
@@ -152,6 +188,7 @@ public:
++curCtrl.numStepsSinceLastTransition;
}
gotSensorData(ts);
curCtrl.activityNew = actDetect.add(ts, data);
}
virtual void onGravity(const Timestamp ts, const GravityData data) override {
@@ -161,6 +198,8 @@ public:
virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) override {
std::cout << "WIFI" << std::endl;
curObs.wifi = data;
plotWifi.add(Settings::WiFiModel::vg_eval.group(data));
plotWifi.plot();
}
virtual void onBarometer(const Timestamp ts, const BarometerData data) override {
@@ -225,17 +264,31 @@ private:
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();
}
/** perform a filter-update (called from a background-loop) */
void filterUpdate() {
++updateCount;
static PlotErrTime pet("\\small{time (sec)}", "\\small{error (m)}", "\\small{APs visible}");
static PlotErrFunc pef("\\small{error (m)}", "\\small{updates (\\%)}");
pef.showMarkers(true);
std::cout << "update" << std::endl;
MyControl ctrlCopy = curCtrl;
static float absHead = M_PI/2; absHead += ctrlCopy.turnSinceLastTransition_rad;
//static float absHead = relHeadingOffset;
absHead += ctrlCopy.turnSinceLastTransition_rad;
//lastEst = curEst;
curEst = pf->update(&curCtrl, curObs);
@@ -245,25 +298,41 @@ private:
plotty.setCurEst(curEst.position.inMeter());
plotty.setGroundTruth(curGT);
// error between ground-truth and estimation
const float estRealErr = curEst.position.inMeter().getDistance(curGT);
statsErr.add(estRealErr);
pef.clear();
pef.add("", statsErr);
pef.plot();
if (updateCount > 4) {
// error between ground-truth and estimation
const float estRealErr = curEst.position.inMeter().getDistance(curGT);
statsErr.add(estRealErr);
pef.clear();
pef.add("", &statsErr);
pef.plot();
// timed error
pet.addErr(lastTransition, estRealErr);
pet.addB(lastTransition, getNumFHWSAPs(curObs.wifi));
pet.plot();
// update estimated path
const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm);
plotty.pathEst.add(p3/100);
}
std::cout << statsErr.asString() << std::endl;
const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm);
plotty.pathEst.add(p3/100);
// show particles
float maxWeight = 0;
float minWeight = 99;
plotty.particles.clear();
for (const auto p : pf->getParticles()) {
const K::GnuplotPoint3 p3(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
plotty.particles.add(p3/100);
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";
// GT
// show ground-truth
plotty.pathReal.clear();
for (const Point3 pt : groundTruth) {
plotty.pathReal.add(K::GnuplotPoint3(pt.x, pt.y, pt.z));
@@ -272,9 +341,12 @@ private:
std::string title =
" time " + std::to_string(curObs.currentTime.sec()) +
" steps: " + std::to_string(ctrlCopy.numStepsSinceLastTransition) +
" turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad);
" turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad) +
" APs: " + std::to_string(curObs.wifi.entries.size()) +
" Act: " + std::to_string((int)curCtrl.activityNew);
plotty.setTitle(title);
// relative heading and compass
{
Point2 cen(0.1, 0.9);
Point2 dir(std::cos(absHead), std::sin(absHead));
@@ -289,7 +361,7 @@ private:
// plot
plotty.plot();
std::this_thread::sleep_for(std::chrono::milliseconds(30));
std::this_thread::sleep_for(std::chrono::milliseconds(5));
curCtrl.resetAfterTransition();