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

activity now debugable

Browse Source 
filter is updated every step and every 1000 ms an evaluation is calculated
also added sink or swim method from frank, works great
This commit is contained in:
toni
2018-07-23 15:56:12 +02:00
parent 6ed487e730
commit b9b9d8f9ac
7 changed files with 99 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
nav/mesh/FilterMesh.h
View File

@@ -21,6 +21,7 @@
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
#include "State.h"
#include "../Observation.h"
@@ -74,7 +75,8 @@ namespace MeshBased {
public:
using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkSimple<NM::NavMeshTriangle>;
using MyNavMeshWalk = NM::NavMeshWalkSinkOrSwim<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker;
@@ -87,8 +89,8 @@ namespace MeshBased {
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::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04));
walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target
}
@@ -143,7 +145,7 @@ namespace MeshBased {
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75;
const float kappa = 0.9;
switch (act) {
@@ -167,9 +169,9 @@ namespace MeshBased {
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){}
//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){}
//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 {
@@ -182,7 +184,7 @@ namespace MeshBased {
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!");
//Assert::equal((int)particles.size(), Settings::numParticles, "number of particles does not match the settings!");
// assign weights
#pragma omp parallel for num_threads(3)
@@ -193,7 +195,9 @@ namespace MeshBased {
const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1;
const double prob = pWifi; // * pStair * pGPS;
//TODO: reduziere das gewicht von partikelen die durch sample imp. oder was anderes sehr weit gesprungen sind.
const double prob = pWifi * pStair * pGPS;
p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");}