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

code for journal presentation

Browse Source
This commit is contained in:
toni
2018-05-15 09:32:39 +02:00
parent 97ce18f5f1
commit a536b1daec
7 changed files with 165 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -59,7 +59,7 @@ ADD_DEFINITIONS(
-fstack-protector-all
-g3
#-O2
-O2
-march=native
-DWITH_TESTS

25
Plotty.h
View File

@@ -167,6 +167,8 @@ public:
cpoints.setPointSize(2);
cpoints.setPointType(7);
splot.getView().setEnabled(false);
}
void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) {
@@ -313,6 +315,22 @@ public:
splot.getCustom() << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << " front\n";
}
void setActivity(const int act) {
std::string activity = "Unkown";
if(act == 0){
activity = "Standing";
} else if(act == 1) {
activity = "Walking";
} else if(act == 2) {
activity = "Up";
} else if(act == 3) {
activity = "Down";
}
gp << "set label 1002 at screen 0.02, 0.94 'Act: " << activity << "'\n";
}
void addRectangle(const Point3 p1, const Point3 p2, const Color c, bool front = false, bool fill = true) {
std::vector<Point3> points = {
Point3(p1.x, p1.y, p1.z),
@@ -516,6 +534,13 @@ public:
gp.flush();
}
void printOverview(const std::string& path) {
gp << "set terminal png size 2000,1500\n";
gp << "set output '" << path << "_overview" << ".png'\n";
gp << "set view 75,60\n";
gp << "set autoscale xy\n";
gp << "set autoscale z\n";
}
void buildFloorplan() {

2
Settings.h
View File

@@ -165,7 +165,7 @@ namespace Settings {
dataDir + "museum/Nexus/Path3_4519.csv",
dataDir + "museum/Nexus/Path3_5929.csv",
dataDir + "museum/Pixel/Path3_6307.csv",
dataDir + "museum/Pixel/Path3_5451.csv",
//dataDir + "museum/Pixel/Path3_5451.csv", //geht nur bis gt-punkt 31
dataDir + "museum/Pixel/Path3_9243.csv",
//dataDir + "museum/Samsung/Path3_7610.csv",
//dataDir + "museum/Samsung/Path3_7819.csv"

84
main.cpp
View File

@@ -93,8 +93,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
const Point3 srcPath0(26, 43, 7.5);
const Point3 srcPath1(62, 38, 1.7);
//const Point3 srcPath2(62, 38, 1.8);
//const Point3 srcPath3(62, 38, 1.8);
const Point3 srcPath2(62, 38, 1.8);
const Point3 srcPath3(62, 38, 1.8);
// add shortest-path to destination
//const Point3 dst(51, 45, 1.7);
@@ -115,17 +115,18 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
// particle-filter
const int numParticles = 5000;
auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
//auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>(WiFiModel);
auto trans = std::make_unique<MyPFTrans>(mesh);
auto trans = std::make_unique<MyPFTrans>(mesh, WiFiModel);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingKLD<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
// setup
MyFilter pf(numParticles, std::move(init));
@@ -154,6 +155,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
if (e.type == Offline::Sensor::WIFI) {
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
ctrl.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == Offline::Sensor::ACC) {
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
@@ -184,6 +186,8 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
if (ctrl.numStepsSinceLastEval > 0) {
obs.currentTime = ts;
ctrl.currentTime = ts;
// if(ctrl.numStepsSinceLastEval > 0){
// pf.updateTransitionOnly(&ctrl);
// }
@@ -192,7 +196,7 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0,0,0.1);
lastTimestamp = ts;
ctrl.lastEstimate = est.pos.pos;
//plot
//dbg.showParticles(pf.getParticles());
@@ -207,22 +211,16 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
plot.setCurEst(est.pos.pos);
plot.setGroundTruth(gtPos);
plot.addEstimationNode(est.pos.pos);
plot.plot();
plot.setActivity((int) act.get());
//plot.plot();
// error calc
float err_m = gtPos.getDistance(est.pos.pos);
errorStats.add(err_m);
errorFile << err_m << "\n";
errorFile << ts.ms() << " " << err_m << "\n";
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
if(ts.ms() == 13410 || ts.ms() == 20802){
std::ofstream plotFile;
plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_zwischendrin_" + std::to_string(ts.ms()) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
}
}
}
@@ -241,14 +239,15 @@ Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string
errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n";
errorFile.close();
//save the .gp buffer into a file
// std::ofstream plotFile;
// plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot" + ".gp");
// dbg.saveToFile(plotFile);
// plotFile.close();
/* plot in gp file */
std::ofstream plotFile;
plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
//save also a png image, just for a better overview
// dbg.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t));
plot.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t));
plot.plot();
return errorStats;
}
@@ -261,13 +260,38 @@ int main(int argc, char** argv) {
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
Settings::DataSetup set = Settings::data.Path1;
std::string evaluationName = "museum/Path1_Bulli_2D_PlotsPaper";
std::string evaluationName = "museum/Path3_Museum_KLD_SimpleDistance";
for(int i = 0; i < 1; ++i){
for(int i = 0; i < 100; ++i){
for(int j = 0; j < 1; ++j){
tmp = run(set, j, evaluationName);
//TODO: in transition die distance über KLD noch einkommentieren als Test
// for(int j = 0; j < Settings::data.Path0.training.size(); ++j){
// tmp = run(Settings::data.Path0, j, evaluationName);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
// }
// for(int j = 0; j < Settings::data.Path1.training.size(); ++j){
// tmp = run(Settings::data.Path1, j, evaluationName);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
// }
// for(int j = 0; j < Settings::data.Path2.training.size(); ++j){
// tmp = run(Settings::data.Path2, j, evaluationName);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
// }
for(int j = 0; j < Settings::data.Path3.training.size(); ++j){
tmp = run(Settings::data.Path3, j, evaluationName);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());
@@ -299,6 +323,6 @@ int main(int argc, char** argv) {
finalStatisticFile.close();
std::this_thread::sleep_for(std::chrono::seconds(60));
//std::this_thread::sleep_for(std::chrono::seconds(60));
}

6
main.h Normal file
View File

@@ -0,0 +1,6 @@
#ifndef NAV_MESH_MAIN_H
#define NAV_MESH_MAIN_H
#endif

82
navMesh/filter.h
View File

@@ -16,13 +16,23 @@
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
#include <Indoor/navMesh/walk/NavMeshWalkWifi.h>
#include <Indoor/navMesh/walk/NavMeshWalkWifiRegional.h>
#include <Indoor/navMesh/walk/NavMeshWalkUnblockable.h>
#include <Indoor/navMesh/walk/NavMeshWalkKLD.h>
#include <Indoor/navMesh/NavMeshRandom.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/activity/ActivityDetector.h>
#include <Indoor/math/divergence/KullbackLeibler.h>
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
struct MyState {
/** the state's position (within the mesh) */
@@ -83,6 +93,14 @@ struct MyControl {
headingChangeSinceLastEval = 0;
}
//wifi
WiFiMeasurements wifi;
//time
Timestamp currentTime;
//last estimation
Point3 lastEstimate = Point3(26, 43, 7.5);
};
@@ -154,26 +172,75 @@ public:
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkKLD<MyNavMeshTriangle>;
MyNavMeshWalk walker;
WiFiQualityAnalyzer analyzer;
WiFiObserverFree wifiProbability;
const double lambda = 0.0003;
SMC::ParticleFilterEstimationBoxKDE<MyState> estimator;
public:
MyPFTrans(MyNavMesh& mesh) : walker(mesh) {
MyPFTrans(MyNavMesh& mesh, WiFiModel& wifiModel) :
walker(mesh),
wifiProbability(Settings::WiFiModel::sigma, wifiModel){
// how to evaluate drawn points
walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
//walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
estimator = SMC::ParticleFilterEstimationBoxKDE<MyState>(&mesh, 0.2, Point2(1,1));
}
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
// walking and heading random
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.1);
// wifi and quality of wifi
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(control->wifi);
if(!wifiObs.entries.empty()){
analyzer.add(wifiObs);
}
float qualityWifi = analyzer.getQuality();
if(std::isnan(qualityWifi)){
qualityWifi = 1.0;
} else if(qualityWifi == 0) {
qualityWifi = 0.00000001;
}
// divergence between eval and transition
std::vector<SMC::Particle<MyState>> wifiParticles;
NM::NavMeshRandom<MyNavMeshTriangle> rnd = walker.getMesh().getRandom();
for(int i = 0; i < 10000; ++i){
NM::NavMeshLocation<MyNavMeshTriangle> tmpLocation = rnd.draw();
double weight = wifiProbability.getProbability(tmpLocation.pos, control->currentTime, wifiObs);
SMC::Particle<MyState> tmpParticle(MyState(tmpLocation.pos), weight);
wifiParticles.push_back(tmpParticle);
}
MyState wifiEstimate = estimator.estimate(wifiParticles);
// fake kld
const double kld = control->lastEstimate.getDistance(wifiEstimate.pos.pos);
//const double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);;
//std::cout << "KLD: " << kld << std::endl;
//std::cout << "Quality: " << qualityWifi << std::endl;
//update wifi
//walker.updateWiFi(wifiObs, control->currentTime, control->lastEstimate);
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
@@ -192,8 +259,11 @@ public:
params.stepSizes.stepSizeStair_m = 0.1;
}
double deltaUnblockable = 0.01;
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
//MyNavMeshWalk::ResultEntry res = walker.getOne(params);
MyNavMeshWalk::ResultEntry res = walker.getOne(params, kld, lambda, qualityWifi);
// assign back to particle's state
p.weight *= res.probability;
@@ -257,9 +327,7 @@ public:
//HACK HACK HACK HACK
double prob = 1.0;
if(observation.currentTime.ms() > 20801){
prob = pWifi * pStair;
}
p.weight *= prob;

1
navMesh/mesh.h
View File

@@ -18,7 +18,6 @@ class MyNavMeshTriangle : public NM::NavMeshTriangle, public NM::NavMeshTriangle
public:
MyNavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, uint8_t type) : NM::NavMeshTriangle(p1, p2, p3, type) {
;
}
};