625f5fe04d
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
214 lines
7.8 KiB
C++
214 lines
7.8 KiB
C++
#ifndef FILTERMESH_
|
|
#define FILTERMESH_
|
|
|
|
#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/resampling/ParticleFilterResamplingSimpleImpoverishment.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>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
|
|
|
|
#include <Indoor/navMesh/NavMesh.h>
|
|
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
|
|
|
|
#include "State.h"
|
|
#include "../Observation.h"
|
|
#include "../../Settings.h"
|
|
|
|
#include <omp.h>
|
|
#include <future>
|
|
|
|
namespace MeshBased {
|
|
|
|
class PFInit : public SMC::ParticleFilterInitializer<MyState> {
|
|
|
|
private:
|
|
|
|
const NM::NavMesh<NM::NavMeshTriangle>* mesh;
|
|
|
|
public:
|
|
|
|
PFInit(const NM::NavMesh<NM::NavMeshTriangle>* mesh) : mesh(mesh) {
|
|
;
|
|
}
|
|
|
|
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 (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 (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
|
|
// p.state.position = node;
|
|
// p.state.heading.direction = Heading(0);
|
|
// }
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
class PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
|
|
|
|
public:
|
|
|
|
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;
|
|
|
|
public:
|
|
|
|
PFTrans(NM::NavMesh<NM::NavMeshTriangle>* 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
|
|
|
|
}
|
|
|
|
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();
|
|
|
|
// 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);
|
|
|
|
#pragma omp parallel for num_threads(3)
|
|
for (int i = 0; i < particles.size(); ++i) {
|
|
SMC::Particle<MyState>& p = particles[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;
|
|
|
|
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
|
|
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
|
|
|
|
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;
|
|
}
|
|
|
|
// 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 SMC::ParticleFilterEvaluation<MyState, MyObservation> {
|
|
|
|
WiFiModel& wifiModel;
|
|
WiFiObserverFree wifiProbability;
|
|
|
|
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
|
|
|
|
const float kappa = 0.75;
|
|
|
|
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:
|
|
|
|
//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<SMC::Particle<MyState>>& particles, const MyObservation& _observation) override {
|
|
|
|
double sum = 0;
|
|
|
|
// local copy!! observation might be changed async outside!! (will really produces crashes!)
|
|
const MyObservation observation = _observation;
|
|
|
|
// vap-grouping
|
|
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!");
|
|
|
|
// assign weights
|
|
#pragma omp parallel for num_threads(3)
|
|
for (size_t i = 0; i < particles.size(); ++i) {
|
|
SMC::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;
|
|
|
|
const double prob = pWifi * pStair * pGPS;
|
|
|
|
p.weight *= prob;
|
|
if (p.weight != p.weight) {throw Exception("nan");}
|
|
|
|
#pragma omp atomic
|
|
sum += p.weight;
|
|
}
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
}
|
|
|
|
#endif // FILTERMESH_
|