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

many changes

Browse Source
This commit is contained in:
FrankE
2017-04-01 09:13:45 +02:00
parent e718dc8cca
commit 6171582b40
6 changed files with 85 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
PlotErrFunc.h
View File

@@ -19,6 +19,7 @@ class PlotErrFunc {
};
std::vector<Entry> entries;
std::vector<K::GnuplotPlotElementLines*> lines;
K::Gnuplot gp;
K::GnuplotPlot gplot;
@@ -39,6 +40,15 @@ public:
/** add one curve */
void add(const std::string name, const K::Statistics<float> stats) {
entries.push_back(Entry(name, stats));
K::GnuplotPlotElementLines* gpel = new K::GnuplotPlotElementLines();
gpel->setTitle(name);
gpel->setLineWidth(2);
lines.push_back(gpel);
gplot.add(gpel);
}
void clear() {
entries.clear();
}
K::Gnuplot& getGP() {
@@ -57,12 +67,11 @@ public:
for (size_t i = 0; i < entries.size(); ++i) {
const Entry e = entries[i];
K::GnuplotPlotElementLines* line = lines[i];
K::GnuplotPlotElementLines* line = new K::GnuplotPlotElementLines();
line->setTitle(e.name);
line->clear();
//line.setTitle(e.name);
line->setColorHex(colors[i]);
line->setLineWidth(2);
gplot.add(line);
// 0 - 80%
for (int i = 0; i <= 85; i+= 5) {
@@ -83,7 +92,6 @@ public:
}
gp.flush();
gp.close();
}

13
Plotty.h
View File

@@ -133,10 +133,10 @@ public:
splot.add(&mapOutlineDrywall);
splot.add(&mapOutlineGlass);
splot.add(&particles); particles.setPointSize(0.33); particles.setColorHex("#0000ff");
splot.add(&particles); particles.setPointSize(0.20); particles.setColorHex("#777777");
splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#0000ff");
splot.add(&pathEst);
splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#000000");
splot.add(&pathEst); pathEst.setLineWidth(2); pathEst.setColorHex("#0000ff");
splot.add(&points);
points.setPointType(7);
@@ -148,6 +148,13 @@ public:
}
void setGroundTruth(const Point3 pos_m) {
gp << "set arrow 998 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n";
}
void setCurEst(const Point3 pos_m) {
gp << "set arrow 999 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n";
}
void setPaletteRedBlue() {

14
Settings.h
View File

@@ -8,6 +8,7 @@ namespace Settings {
const std::string pathData = "/apps/android/workspace/OTHER2017/data/";
const std::string pathWalks = "/apps/android/workspace/OTHER2017/data/";
const std::string pathWiFi = "/apps/android/workspace/OTHER2017/data/wifi/";
const std::string path1a = pathWalks + "path1/1490208103510.csv";
const std::string path1b = pathWalks + "path1/1490208519267.csv";
@@ -15,8 +16,14 @@ namespace Settings {
const std::string path2a = pathWalks + "path2/1490208940015.csv";
const std::string path2b = pathWalks + "path2/1490209320343.csv";
const std::string wifiAllFixed = pathWiFi + "allFixed.xml";
const std::string wifiAllOptPar = pathWiFi + "allOptPar.xml";
const std::string wifiEachOptPar = pathWiFi + "eachOptPar.xml";
const std::string wifiEachOptParPos = pathWiFi + "eachOptParPos.xml";
const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL37.xml";
const std::string fMap = "/apps/android/workspace/IndoorMap/maps/SHL38.xml";
const std::string fCalib = "/apps/android/workspace/OTHER2017/data/wifi_fp_all.dat";
const std::string fPathGFX = "/apps/android/workspace/OTHER2017/tex/gfx/";
@@ -26,9 +33,10 @@ namespace Settings {
float smartphoneAboveGround = 1.3;
namespace IMU {
const float turnSigma = 1.5 + 1.5;//1.5;
const float stepLength = 0.80;
const float turnSigma = 1.5 + 0.5;//1.5;
const float stepLength = 0.85;
const float stepSigma = 0.40;
const float absHeadSigma = 90;
}
namespace Grid {

2
main.cpp
View File

@@ -22,10 +22,10 @@
#include "EvalCompareOpt.h"
#include "EvalApOpt.h"
#include "EvalWalk.h"
#include "EvalData.h"
#include "EvalWiFi.h"
#include "EvalWiFiSigStrength.h"
#include "pf/EvalWalk.h"
#include "PlotErrFunc.h"

52
EvalWalk.h → pf/EvalWalk.h
View File

@@ -9,6 +9,8 @@
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include "../PlotErrFunc.h"
#include <thread>
#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
@@ -30,9 +32,10 @@
#include "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h"
#include "Indoor/sensors/offline/FileReader.h"
#include "Helper.h"
#include "pf/PF.h"
#include "../Helper.h"
#include "PF.h"
#include <Indoor/sensors/offline/FilePlayer.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/sensors/offline/Listener.h>
@@ -47,6 +50,8 @@ class EvalWalk : public Offline::Listener {
Plotty plotty;
Offline::FileReader reader;
Offline::FilePlayer player;
Offline::FileReader::GroundTruth groundTruthLive;
Timestamp lastTransition;
@@ -79,6 +84,7 @@ public:
// 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();
@@ -91,6 +97,9 @@ public:
std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel) );
pf->setEvaluation( std::move(eval) );
wifiModel.saveXML("/tmp/test.xml");
wifiModel.loadXML("/tmp/test.xml");
// resampling step?
pf->setNEffThreshold(0.5);
//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
@@ -115,9 +124,14 @@ public:
//GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );
reader.open(path, this);
reader.open(path);
groundTruthLive = reader.getGroundTruth(map, Settings::GroundTruth::path1);
player.setReader(&reader);
player.setListener(this);
player.start();
// wait for completion
player.join();
}
@@ -159,7 +173,11 @@ public:
virtual void onCompass(const Timestamp ts, const CompassData data) override {
const float newAzimuth =- data.azimuth + M_PI/2; // oriented towards north for our map
curCtrl.compass_azimuth = curCtrl.compass_azimuth * 0.99 + newAzimuth * 0.01;
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);
//curCtrl.compassAzimuth_rad = curCtrl.compassAzimuth_rad * 0.99 + newAzimuth * 0.01;
}
@@ -206,9 +224,14 @@ private:
K::Statistics<float> statsErr;
/** perform a filter-update (called from a background-loop) */
void filterUpdate() {
static PlotErrFunc pef("\\small{error (m)}", "\\small{updates (\\%)}");
std::cout << "update" << std::endl;
MyControl ctrlCopy = curCtrl;
@@ -216,17 +239,20 @@ private:
//lastEst = curEst;
curEst = pf->update(&curCtrl, curObs);
//Log::add("Nav", "cur est: " + curEst.position.asString());
// inform listeners about the new estimation
// for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());}
const Point3 curGT = groundTruthLive.get(lastTransition);
// update estimated path
//estPath.push_back(curEst.position.inMeter());
// PFTrans* trans = (PFTrans*)pf->getTransition();
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();
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);
@@ -254,7 +280,7 @@ private:
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.compass_azimuth), std::sin(ctrlCopy.compass_azimuth));
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";
}

14
pf/PF.h
View File

@@ -21,6 +21,7 @@
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleAbsoluteHeadingControl.h>
#include "../Settings.h"
@@ -108,7 +109,7 @@ struct MyControl {
int numStepsSinceLastTransition = 0;
/** absolute heading in radians */
float compass_azimuth = 0;
float compassAzimuth_rad = 0;
// TODO: switch to a general activity enum/detector using barometer + accelerometer?
/** currently detected activity */
@@ -163,19 +164,24 @@ public:
GridWalker<MyGridNode, MyState> walker;
WalkModuleFavorZ<MyGridNode, MyState> modFavorZ;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modHeading;
WalkModuleNodeImportance<MyGridNode, MyState> modImportance;
WalkModuleFollowDestination<MyGridNode, MyState> modDestination;
WalkModuleActivityControl<MyGridNode, MyState, MyControl> modActivity;
WalkModuleHeadingControl<MyGridNode, MyState, MyControl> modRelHead;
WalkModuleAbsoluteHeadingControl<MyGridNode, MyState, MyControl> modAbsHead;
std::minstd_rand gen;
public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl) {
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modRelHead(&ctrl, Settings::IMU::turnSigma), modAbsHead(&ctrl, Settings::IMU::absHeadSigma), modDestination(*grid), modActivity(&ctrl) {
walker.addModule(&modRelHead);
walker.addModule(&modAbsHead);
//walker.addModule(&modFavorZ);
walker.addModule(&modHeading);
//walker.addModule(&modImportance);
//walker.addModule(&modActivity);