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

strange bug... a lot of zero weight particles

Browse Source
This commit is contained in:
toni
2018-02-09 17:05:23 +01:00
parent de2570cc0c
commit 46abeae148
5 changed files with 196 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
navMesh/filter.h
View File

@@ -2,20 +2,26 @@
#define NAV_MESH_FILTER_H
#include "mesh.h"
#include "../Settings.h"
#include <Indoor/geo/Heading.h>
#include <Indoor/math/Distributions.h>
#include <KLib/math/filter/particles/Particle.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/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/Particle.h>
#include <Indoor/smc/filtering/ParticleFilter.h>
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
struct MyState {
/** the state's position (within the mesh) */
@@ -44,6 +50,16 @@ struct MyState {
return res;
}
float getBinValue(const int dim) const {
switch (dim) {
case 0: return this->pos.pos.x;
case 1: return this->pos.pos.y;
case 2: return this->pos.pos.z;
case 3: return this->heading.getRAD();
}
throw "cant find this value within the bin";
}
};
struct MyControl {
@@ -61,11 +77,19 @@ struct MyControl {
struct MyObservation {
// pressure
float sigmaPressure = 0.10f;
float relativePressure = 0;
//wifi
WiFiMeasurements wifi;
//time
Timestamp currentTime;
};
class MyPFInitUniform : public K::ParticleFilterInitializer<MyState> {
class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
const MyNavMesh* mesh;
@@ -75,12 +99,12 @@ public:
;
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
/** random position and heading within the mesh */
Distribution::Uniform<float> dHead(0, 2*M_PI);
MyNavMeshRandom rnd = mesh->getRandom();
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
p.state.pos = rnd.draw();
p.state.heading = dHead.draw();
p.weight = 1.0 / particles.size();
@@ -89,7 +113,7 @@ public:
};
class MyPFInitFixed : public K::ParticleFilterInitializer<MyState> {
class MyPFInitFixed : public SMC::ParticleFilterInitializer<MyState> {
const MyNavMesh* mesh;
const Point3 pos;
@@ -100,11 +124,11 @@ public:
;
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
/** random position and heading within the mesh */
Distribution::Uniform<float> dHead(0, 2*M_PI);
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
p.state.pos = mesh->getLocation(pos);
p.state.heading = dHead.draw();
p.weight = 1.0 / particles.size();
@@ -114,7 +138,7 @@ public:
};
class MyPFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
MyNavMeshWalk walker;
@@ -124,20 +148,20 @@ public:
MyPFTrans(MyNavMesh& mesh) : walker(mesh) {
// 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::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
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
}
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.10);
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
// how to walk
MyNavMeshWalkParams params;
@@ -148,10 +172,10 @@ public:
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
// assign back to particle's state
p.weight *= res.probability;
p.weight *= res.probability;
p.state.pos = res.location;
p.state.heading = res.heading;
@@ -165,20 +189,38 @@ public:
};
class MyPFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
public:
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
return 1.0;
virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& observation) override {
double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
for (SMC::Particle<MyState>& p : particles) {
double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
const double prob = pWifi;
p.weight *= prob;
sum += prob;
}
return sum;
}
};
using MyFilter = K::ParticleFilter<MyState, MyControl, MyObservation>;
using MyFilter = SMC::ParticleFilter<MyState, MyControl, MyObservation>;
#endif

157
navMesh/main.h
View File

@@ -3,27 +3,80 @@
#include "mesh.h"
#include "filter.h"
#include "../Settings.h"
#include <memory>
#include <thread>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
void navMeshMain() {
std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml";
//std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml";
std::string mapFile = "../map/map42_ap.xml";
// reading file
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile);
Offline::FileReader fr("../measurements/museum/Pixel/Path1_2468.csv");
WiFiFingerprints fingerprints("../measurements/museum/Nexus/fingerprints/wifi_fp.dat");
const std::string wifiModelFile = "../measurements/museum/wifimodel.dat";
std::ifstream inp(wifiModelFile, std::ifstream::binary);
// wifi
WiFiModelLogDistCeiling WiFiModel(map);
// with optimization
if(Settings::WiFiModel::optimize){
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp);
}
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
WiFiModel.addAP(ap.mac, entry);
}
WiFiModel.saveXML(wifiModelFile);
} else {
WiFiModel.loadXML(wifiModelFile);
}
} else {
// without optimization
WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
}
// mesh
NM::NavMeshSettings set;
MyNavMesh mesh;
MyNavMeshFactory fac(&mesh, set);
fac.build(map);
const Point3 src(26, 43, 7.5);
const Point3 srcPath0(26, 43, 7.5);
const Point3 srcPath1(62, 38, 1.8);
// add shortest-path to destination
//const Point3 dst(51, 45, 1.7);
const Point3 dst(25, 45, 0);
NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);
//const Point3 dst(25, 45, 0);
//NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);
// debug show
NM::NavMeshDebug dbg;
@@ -32,13 +85,13 @@ void navMeshMain() {
dbg.draw();
// particle-filter
const int numParticles = 1000;
auto init = std::make_unique<MyPFInitFixed>(&mesh, src); // known position
//auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>();
const int numParticles = 1000;
//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 resample = std::make_unique<K::ParticleFilterResamplingSimple<MyState>>();
auto estimate = std::make_unique<K::ParticleFilterEstimationWeightedAverage<MyState>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
// setup
MyFilter pf(numParticles, std::move(init));
@@ -46,49 +99,85 @@ void navMeshMain() {
pf.setTransition(std::move(trans));
pf.setResampling(std::move(resample));
pf.setEstimation(std::move(estimate));
pf.setNEffThreshold(1);
pf.setNEffThreshold(1);
// sensors
MyControl ctrl;
MyObservation obs;
//Distribution::Uniform<float> dHead(0, 2*M_PI);
Distribution::Normal<float> dHead(0, 0.1);
StepDetection sd;
PoseDetection pd;
TurnDetection td(&pd);
RelativePressure relBaro;
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
Timestamp lastTimestamp = Timestamp::fromMS(0);
for (int i = 0; i < 10000; ++i) {
// parse each sensor-value within the offline data
for (const Offline::Entry& e : fr.getEntries()) {
ctrl.numStepsSinceLastEval = 1;
ctrl.headingChangeSinceLastEval = dHead.draw();
const Timestamp ts = Timestamp::fromMS(e.ts);
MyState est = pf.update(&ctrl, obs);
if (e.type == Offline::Sensor::WIFI) {
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
ctrl.afterEval();
} else if (e.type == Offline::Sensor::ACC) {
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
++ctrl.numStepsSinceLastEval;
}
const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
pd.addAccelerometer(ts, _acc.data);
try {
MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos);
auto path = loc.tria->getPathToDestination<MyNavMeshTriangle>(loc.pos);
dbg.addDijkstra(path);
} catch (...) {;}
} else if (e.type == Offline::Sensor::GYRO) {
const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
const float delta_gyro = td.addGyroscope(ts, _gyr.data);
const int d = (i * 1) % 360;
dbg.plot.getView().setCamera(60, d);
dbg.showParticles(pf.getParticles());
dbg.setCurPos(est.pos.pos);
ctrl.headingChangeSinceLastEval += delta_gyro;
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
} else if (e.type == Offline::Sensor::BARO) {
relBaro.add(ts, fr.getBarometer()[e.idx].data);
obs.relativePressure = relBaro.getPressureRealtiveToStart();
obs.sigmaPressure = relBaro.getSigma();
}
std::cout << i << std::endl;
if (ts.ms() - lastTimestamp.ms() > 500 && ctrl.numStepsSinceLastEval > 0) {
dbg.draw();
obs.currentTime = ts;
// if(ctrl.numStepsSinceLastEval > 0){
// pf.updateTransitionOnly(&ctrl);
// ctrl.afterEval();
// }
// MyState est = pf.updateEvaluationOnly(obs);
// lastTimestamp = ts;
std::this_thread::sleep_for(std::chrono::milliseconds(5));
MyState est = pf.update(&ctrl, obs);
ctrl.afterEval();
lastTimestamp = ts;
}
// try {
// MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos);
// auto path = loc.tria->getPathToDestination<MyNavMeshTriangle>(loc.pos);
// dbg.addDijkstra(path);
// } catch (...) {;}
// const int d = (i * 1) % 360;
// dbg.plot.getView().setCamera(60, d);
dbg.showParticles(pf.getParticles());
dbg.setCurPos(est.pos.pos);
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
// std::cout << i << std::endl;
dbg.draw();
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
}
#endif