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

updated sensors and filter to current code version

Browse Source 
removed KLib stuff
added new activity
filter is uncommand!
at the moment, the app is not able to load new maps and breaks using old maps
This commit is contained in:
toni
2018-07-12 18:39:27 +02:00
parent b4a1a3d969
commit 625f5fe04d
22 changed files with 325 additions and 261 deletions
Download Patch File Download Diff File Expand all files Collapse all files

207
nav/mesh/FilterMesh.h
View File

@@ -1,13 +1,18 @@
#ifndef FILTERMESH_
#define FILTERMESH_
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <Indoor/smc/Particle.h>
#include <Indoor/smc/filtering/ParticleFilter.h>
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
@@ -18,6 +23,7 @@
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include "State.h"
#include "../Observation.h"
#include "../../Settings.h"
#include <omp.h>
@@ -25,33 +31,33 @@
namespace MeshBased {
class PFInit : public K::ParticleFilterInitializer<MyState> {
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
private:
NM::NavMesh<NM::NavMeshTriangle>* mesh;
const NM::NavMesh<NM::NavMeshTriangle>* mesh;
public:
PFInit(NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
;
}
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
std::minstd_rand gen;
std::uniform_real_distribution<float> distHead(0, 2*M_PI);
NM::NavMeshRandom<NM::NavMeshTriangle> rnd = mesh->getRandom();
for (K::Particle<MyState>& p : particles) {
for (SMC::Particle<MyState>& p : particles) {
p.state.loc = rnd.draw();
p.state.heading = Heading(distHead(gen)); // random heading
p.weight = 1.0 / particles.size(); // equal weight
}
// // fix position + heading
// for (K::Particle<MyState>& p : particles) {
// for (SMC::Particle<MyState>& p : particles) {
//// const int idx = 9000;
//// const MyGridNode& node = (*grid)[idx];
// const MyGridNode& node = grid->getNodeFor(GridPoint(2000, 2000, 0)); // center of the testmap
@@ -63,107 +69,109 @@ namespace MeshBased {
};
/*
class PFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
public:
// local, static control-data COPY
using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker;
// local, static control-data COPY
MyControl ctrl;
Grid<MyGridNode>* grid;
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;
NodeResampling<MyState, MyGridNode> resampler;
std::minstd_rand gen;
public:
PFTrans(Grid<MyGridNode>* grid) : grid(grid), modHeading(&ctrl, Settings::IMU::turnSigma), modDestination(*grid), modActivity(&ctrl), resampler(*grid) {
PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){
//walker.addModule(&modFavorZ);
walker.addModule(&modHeading);
//walker.addModule(&modImportance);
walker.addModule(&modActivity);
if (Settings::destination != GridPoint(0,0,0)) {
//walker.addModule(&modDestination);
modDestination.setDestination(grid->getNodeFor(Settings::destination));
}
// 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
}
void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* _ctrl) override {
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* _ctrl) override {
// local copy!! observation might be changed async outside!! (will really produces crashes!)
this->ctrl = *_ctrl;
((MyControl*)_ctrl)->resetAfterTransition();
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
// 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);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
//for (K::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
// how to walk
NM::NavMeshWalkParams<NM::NavMeshTriangle> params;
params.heading = p.state.heading + ctrl.turnSinceLastTransition_rad + dHeading.draw();
params.numSteps = ctrl.numStepsSinceLastTransition;
params.start = p.state.loc;
//#pragma omp atomic
const float dist_m = std::abs(ctrl.numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
K::Particle<MyState>& p = particles[i];
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
params.stepSizes.stepSizeFloor_m = 0.1;
params.stepSizes.stepSizeStair_m = 0.1;
}
double prob;
p.state = walker.getDestination(*grid, p.state, dist_m, prob);
//p.weight *= prob;//(prob > 0.01) ? (1.0) : (0.15);
//p.weight = (prob > 0.01) ? (1.0) : (0.15);
//p.weight = prob;
//p.weight = 1.0; // reset
//p.weight = std::pow(p.weight, 0.1); // make all particles a little more equal [less strict]
//p.weight *= std::pow(prob, 0.1); // add grid-walk-probability
p.weight = prob; // grid-walk-probability
if (p.weight != p.weight) {throw Exception("nan");}
// walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
}
// assign back to particle's state
p.weight *= res.probability;
p.state.loc = res.location;
p.state.heading = res.heading;
}
}
};
class PFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
Grid<MyGridNode>* grid;
class PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModelLogDistCeiling& wifiModel;
WiFiModel& wifiModel;
WiFiObserverFree wifiProbability;
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
//WiFiObserverFree wiFiProbability; // free-calculation
WiFiObserverGrid<MyGridNode> wiFiProbability; // grid-calculation
const float kappa = 0.75;
// smartphone is 1.3 meter above ground
const Point3 person = Point3(0,0,Settings::smartphoneAboveGround);
switch (act) {
case Activity::WALKING:
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.loc.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
}
public:
PFEval(Grid<MyGridNode>* grid, WiFiModelLogDistCeiling& wifiModel) :
grid(grid), wifiModel(wifiModel),
//wiFiProbability(Settings::WiFiModel::sigma, wifiModel) { // WiFi free
wiFiProbability(Settings::WiFiModel::sigma) { // WiFi grid
//TODO: Was ist hier besser? Im Museum hatten wir das unterste.
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel){}
//PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
PFEval(WiFiModel* wifiModel) : wifiModel(*wifiModel), wifiProbability(Settings::WiFiModel::sigma, *wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
}
double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& _observation) override {
double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
double sum = 0;
@@ -171,50 +179,35 @@ namespace MeshBased {
const MyObservation observation = _observation;
// vap-grouping
const int numAP1 = observation.wifi.entries.size();
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(_observation.wifi);
const int numAP2 = wifiObs.entries.size();
Log::add("Filter", "VAP: " + std::to_string(numAP1) + " -> " + std::to_string(numAP2));
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
// sanity check
Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
#pragma omp parallel for num_threads(3)
for (int i = 0; i < Settings::numParticles; ++i) {
// assign weights
#pragma omp parallel for num_threads(3)
for (size_t i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
K::Particle<MyState>& p = particles[i];
const double pWifi = wifiProbability.getProbability(p.state.loc.pos, observation.currentTime, wifiObs);
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
// WiFi free
//const double pWiFi = wiFiProbability.getProbability(p.state.position.inMeter()+person, observation.currentTime, vg.group(observation.wifi));
const double prob = pWifi * pStair * pGPS;
// WiFi grid
const MyGridNode& node = grid->getNodeFor(p.state.position);
const double pWiFi = wiFiProbability.getProbability(node, observation.currentTime, wifiObs);
p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic
sum += p.weight;
}
//Log::add("xxx", std::to_string(observation.currentTime.ms()) + "_" + std::to_string(wifiObs.entries[0].ts.ms()));
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
const double prob = pWiFi * pGPS * pStair;
p.weight *= prob; // NOTE: keeps the weight returned by the transition step!
//p.weight = prob; // does NOT keep the weights returned by the transition step
if (p.weight != p.weight) {throw Exception("nan");}
#pragma omp atomic
sum += p.weight;
}
return sum;
return sum;
}
};
*/
}
#endif // FILTERMESH_

84
nav/mesh/NavControllerMesh.cpp
View File

@@ -31,37 +31,35 @@
Q_DECLARE_METATYPE(const void*)
/** ctor */
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh) :
NavController(mainController, im), navMesh(navMesh), wifiModel(im) {
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
// filter init
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new MeshBased::PFInit(navMesh));
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
// // estimation
// //std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
// std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// estimation
std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>());
//std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// // resampling
// std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
// //std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
// //std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
// //std::unique_ptr<RegionalResampling> resample(new RegionalResampling());
// resampling
//std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.05));
std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// // eval and transition
// wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
// std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
// std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));
// eval and transition
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new MeshBased::PFTrans(navMesh));
// // setup the filter
// pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
// pf->setTransition(std::move(transition));
// pf->setEvaluation(std::move(eval));
// pf->setEstimation(std::move(estimation));
// pf->setResampling(std::move(resample));
// setup the filter
pf = std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>>(new SMC::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
pf->setTransition(std::move(transition));
pf->setEvaluation(std::move(eval));
pf->setEstimation(std::move(estimation));
pf->setResampling(std::move(resample));
// pf->setNEffThreshold(0.85); //before 0.75, edit by toni
// //pf->setNEffThreshold(0.65); // still too low?
// //pf->setNEffThreshold(0.25); // too low
pf->setNEffThreshold(0.85); //before 0.75, edit by toni
//pf->setNEffThreshold(0.65); // still too low?
//pf->setNEffThreshold(0.25); // too low
// attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this);
@@ -70,7 +68,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this);
//SensorFactory::get().getActivity().addListener(this);
SensorFactory::get().getActivity().addListener(this);
// hacky.. but we need to call this one from the main thread!
//mainController->getMapView()->showParticles(pf->getParticles());
@@ -161,14 +159,14 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor<TurnData>* sensor, const
gotSensorData(ts);
}
// void NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
// (void) sensor;
// (void) ts;
// curCtrl.activity = data.curActivity;
// curObs.activity = data.curActivity;
// debugActivity(data.curActivity);
// gotSensorData(ts);
// }
void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
(void) sensor;
(void) ts;
curCtrl.activity = data.curActivity;
curObs.activity = data.curActivity;
//debugActivity(data.curActivity);
gotSensorData(ts);
}
/** called when any sensor has received new data */
void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
@@ -176,16 +174,16 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
}
// void debugActivity(const ActivityData& activity) {
// QString act;
// switch(activity.curActivity) {
// case ActivityButterPressure::Activity::STAY: act = "STAY"; break;
// case ActivityButterPressure::Activity::DOWN: act = "DOWN"; break;
// case ActivityButterPressure::Activity::UP: act = "UP"; break;
// default: act = "???"; break;
// }
// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
// }
// void debugActivity(const ActivityData& activity) {
// QString act;
// switch(activity.curActivity) {
// case Activity::STANDING: act = "STAY"; break;
// case Activity::WALKING_DOWN: act = "DOWN"; break;
// case Activity::WALKING_UP: act = "UP"; break;
// default: act = "???"; break;
// }
// Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
// }
/** particle-filter update loop */
void MeshBased::NavControllerMesh::filterUpdateLoop() {

9
nav/mesh/NavControllerMesh.h
View File

@@ -8,6 +8,7 @@
#include "../sensors/SensorFactory.h"
#include "../sensors/StepSensor.h"
#include "../sensors/TurnSensor.h"
#include "../sensors/ActivitySensor.h"
#include <Indoor/navMesh/NavMeshLocation.h>
#include <Indoor/navMesh/NavMesh.h>
@@ -28,16 +29,16 @@ namespace MeshBased {
private:
NM::NavMesh<NM::NavMeshTriangle>* navMesh;
WiFiModelLogDistCeiling wifiModel;
WiFiModel* wifiModel;
std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;
std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>> pf;
MyObservation curObs;
MyControl curCtrl;
public:
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh);
NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel);
void start() override;
@@ -59,7 +60,7 @@ namespace MeshBased {
void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override;
// void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override ;
private:

28
nav/mesh/State.h
View File

@@ -22,24 +22,26 @@ namespace MeshBased {
;
}
// MyState& operator += (const MyState& o) {
// position += o.position;
// return *this;
// }
MyState& operator += (const MyState& o) {
loc.pos += o.loc.pos;
return *this;
}
// MyState& operator /= (const float val) {
// position /= val;
// return *this;
// }
MyState& operator /= (const float val) {
loc.pos /= val;
return *this;
}
// MyState operator * (const float val) const {
// MyState copy = *this;
// copy.position = copy.position * val;
// return copy;
// }
MyState operator * (const float val) const {
MyState copy = *this;
copy.loc.pos = copy.loc.pos * val;
return copy;
}
};
}
#endif // MESH_STATE_H