init push
This commit is contained in:
toni
318
navMesh/filter.h
Normal file
318
navMesh/filter.h
Normal file
@@ -0,0 +1,318 @@
|
||||
#ifndef NAV_MESH_FILTER_H
|
||||
#define NAV_MESH_FILTER_H
|
||||
|
||||
#include "mesh.h"
|
||||
#include "../Settings.h"
|
||||
#include <omp.h>
|
||||
|
||||
#include <Indoor/geo/Heading.h>
|
||||
#include <Indoor/math/Distributions.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/ParticleFilterEstimationBoxKDE.h>
|
||||
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.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/walk/NavMeshWalkSinkOrSwim.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) */
|
||||
MyNavMeshLocation pos;
|
||||
|
||||
/** the state's heading */
|
||||
Heading heading;
|
||||
|
||||
MyState() : pos(), heading(0) {}
|
||||
|
||||
MyState(Point3 p) : pos(p, nullptr), heading(0){}
|
||||
|
||||
MyState& operator += (const MyState& o) {
|
||||
pos.tria = nullptr; // impossible
|
||||
pos.pos += o.pos.pos;
|
||||
return *this;
|
||||
}
|
||||
|
||||
MyState& operator /= (const float val) {
|
||||
pos.tria = nullptr; // impossible
|
||||
pos.pos /= val;
|
||||
return *this;
|
||||
}
|
||||
|
||||
MyState operator * (const float val) const {
|
||||
MyState res;
|
||||
res.pos.pos = pos.pos * val;
|
||||
return res;
|
||||
}
|
||||
|
||||
float getX(){
|
||||
return pos.pos.x;
|
||||
}
|
||||
|
||||
float getY() {
|
||||
return pos.pos.y;
|
||||
}
|
||||
|
||||
float getZ() {
|
||||
return pos.pos.z;
|
||||
}
|
||||
|
||||
|
||||
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 {
|
||||
|
||||
int numStepsSinceLastEval = 0;
|
||||
float headingChangeSinceLastEval = 0;
|
||||
|
||||
void afterEval() {
|
||||
numStepsSinceLastEval = 0;
|
||||
headingChangeSinceLastEval = 0;
|
||||
}
|
||||
|
||||
//wifi
|
||||
WiFiMeasurements wifi;
|
||||
|
||||
//time
|
||||
Timestamp currentTime;
|
||||
|
||||
//last estimation
|
||||
Point3 lastEstimate = Point3(26, 43, 7.5);
|
||||
|
||||
};
|
||||
|
||||
struct MyObservation {
|
||||
|
||||
// pressure
|
||||
float sigmaPressure = 0.10f;
|
||||
float relativePressure = 0;
|
||||
|
||||
//wifi
|
||||
WiFiMeasurements wifi;
|
||||
|
||||
//time
|
||||
Timestamp currentTime;
|
||||
|
||||
//activity
|
||||
Activity activity;
|
||||
|
||||
};
|
||||
|
||||
class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
|
||||
|
||||
const MyNavMesh* mesh;
|
||||
|
||||
public:
|
||||
|
||||
MyPFInitUniform(const MyNavMesh* mesh) : mesh(mesh) {
|
||||
}
|
||||
|
||||
virtual ~MyPFInitUniform(){}
|
||||
|
||||
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
|
||||
|
||||
/** random position and heading within the mesh */
|
||||
Distribution::Uniform<double> dHead(0, 2*M_PI);
|
||||
MyNavMeshRandom rnd = mesh->getRandom();
|
||||
for (SMC::Particle<MyState>& p : particles) {
|
||||
p.state.pos = rnd.draw();
|
||||
p.state.heading = static_cast<float>(dHead.draw());
|
||||
p.weight = 1.0 / particles.size();
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class MyPFInitFixed : public SMC::ParticleFilterInitializer<MyState> {
|
||||
|
||||
const MyNavMesh* mesh;
|
||||
const Point3 pos;
|
||||
|
||||
public:
|
||||
|
||||
MyPFInitFixed(const MyNavMesh* mesh, const Point3 pos) : mesh(mesh), pos(pos) {
|
||||
}
|
||||
|
||||
virtual ~MyPFInitFixed(){}
|
||||
|
||||
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
|
||||
|
||||
/** random position and heading within the mesh */
|
||||
Distribution::Uniform<double> dHead(0, 2*M_PI);
|
||||
for (SMC::Particle<MyState>& p : particles) {
|
||||
p.state.pos = mesh->getLocation(pos);
|
||||
p.state.heading = static_cast<float>(dHead.draw());
|
||||
p.weight = 1.0 / particles.size();
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
class MyPFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
|
||||
|
||||
//using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
|
||||
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<MyNavMeshTriangle>;
|
||||
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<MyNavMeshTriangle>;
|
||||
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<MyNavMeshTriangle>;
|
||||
|
||||
MyNavMeshWalk walker;
|
||||
|
||||
WiFiQualityAnalyzer analyzer;
|
||||
WiFiObserverFree wifiProbability;
|
||||
|
||||
SMC::ParticleFilterEstimationBoxKDE<MyState> estimator;
|
||||
|
||||
public:
|
||||
|
||||
//std::vector<double> listRadiusSub;
|
||||
|
||||
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::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
|
||||
}
|
||||
|
||||
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
|
||||
// walking and heading random
|
||||
Distribution::Normal<float> dStepSizeFloor(0.70f, 0.1f);
|
||||
Distribution::Normal<float> dStepSizeStair(0.35f, 0.1f);
|
||||
Distribution::Normal<float> dHeading(0.0, 0.1f);
|
||||
|
||||
// #pragma omp parallel for num_threads(3)
|
||||
for (unsigned long i = 0; i < particles.size(); ++i) {
|
||||
SMC::Particle<MyState>& p = particles[i];
|
||||
|
||||
// how to walk
|
||||
MyNavMeshWalkParams params;
|
||||
params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw();
|
||||
params.numSteps = control->numStepsSinceLastEval;
|
||||
params.start = p.state.pos;
|
||||
|
||||
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
|
||||
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
|
||||
|
||||
if(params.stepSizes.stepSizeFloor_m < 0.1f || params.stepSizes.stepSizeStair_m < 0.1f){
|
||||
params.stepSizes.stepSizeFloor_m = 0.1f;
|
||||
params.stepSizes.stepSizeStair_m = 0.1f;
|
||||
}
|
||||
|
||||
// walk
|
||||
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
|
||||
|
||||
// assign back to particle's state
|
||||
p.weight *= res.probability;
|
||||
p.state.pos = res.location;
|
||||
p.state.heading = res.heading;
|
||||
|
||||
}
|
||||
|
||||
// reset the control (0 steps, 0 delta-heading)
|
||||
//control->afterEval();
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
|
||||
|
||||
WiFiModel& wifiModel;
|
||||
WiFiObserverFree wifiProbability;
|
||||
|
||||
//TODO: add this to transition probability
|
||||
float getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
|
||||
|
||||
const float kappa = 0.85f;
|
||||
|
||||
switch (act) {
|
||||
|
||||
case Activity::WALKING:
|
||||
if (p.state.pos.tria->getType() == static_cast<int>(NM::NavMeshType::FLOOR_INDOOR)) {return kappa;}
|
||||
if (p.state.pos.tria->getType() == static_cast<int>(NM::NavMeshType::DOOR)) {return kappa;}
|
||||
if (p.state.pos.tria->getType() == static_cast<int>(NM::NavMeshType::STAIR_LEVELED)) {return kappa;}
|
||||
{return 1-kappa;}
|
||||
|
||||
case Activity::WALKING_UP:
|
||||
case Activity::WALKING_DOWN:
|
||||
if (p.state.pos.tria->getType() == static_cast<int>(NM::NavMeshType::STAIR_SKEWED)) {return kappa;}
|
||||
if (p.state.pos.tria->getType() == static_cast<int>(NM::NavMeshType::STAIR_LEVELED)) {return kappa;}
|
||||
if (p.state.pos.tria->getType() == static_cast<int>(NM::NavMeshType::ELEVATOR)) {return kappa;}
|
||||
{return 1-kappa;}
|
||||
}
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
|
||||
//MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::EXPONENTIAL){}
|
||||
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel, WiFiObserverFree::EvalDist::CAPPED_NORMAL_DISTRIBUTION){}
|
||||
|
||||
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);
|
||||
|
||||
//#pragma omp parallel for num_threads(3)
|
||||
for (size_t i = 0; i < particles.size(); ++i) {
|
||||
SMC::Particle<MyState>& p = particles[i];
|
||||
|
||||
double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
|
||||
double pBluetooth =
|
||||
double pStair = static_cast<double>(getStairProb(p, observation.activity));
|
||||
|
||||
double prob = pWifi * pStair;
|
||||
|
||||
p.weight *= prob;
|
||||
|
||||
#pragma omp atomic
|
||||
sum += prob;
|
||||
}
|
||||
|
||||
return sum;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
using MyFilter = SMC::ParticleFilter<MyState, MyControl, MyObservation>;
|
||||
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user