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");}

71
nav/mesh/NavControllerMesh.cpp
View File

@@ -22,6 +22,9 @@
#include <Indoor/navMesh/NavMeshTriangle.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
//#ifndef ANDROID
//#include <valgrind/callgrind.h>
//#endif
@@ -38,12 +41,16 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
// 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));
//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<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::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>(navMesh, 0.2, Point2(1,1)));
//std::unique_ptr<SMC::ParticleFilterResamplingKLD<MyState>> resample(new SMC::ParticleFilterResamplingKLD<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.95));
//std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// eval and transition
@@ -57,7 +64,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
pf->setEstimation(std::move(estimation));
pf->setResampling(std::move(resample));
pf->setNEffThreshold(0.85); //before 0.75, edit by toni
pf->setNEffThreshold(0.75); //before 0.75, edit by toni
//pf->setNEffThreshold(0.65); // still too low?
//pf->setNEffThreshold(0.25); // too low
@@ -164,7 +171,7 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, co
(void) ts;
curCtrl.activity = data.curActivity;
curObs.activity = data.curActivity;
//debugActivity(data.curActivity);
debugActivity(data.curActivity);
gotSensorData(ts);
}
@@ -174,16 +181,17 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
}
// 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");
// }
void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) {
QString act;
switch(activity.curActivity) {
case Activity::STANDING: act = "STAY"; break;
case Activity::WALKING: act = "WALK"; 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() {
@@ -221,7 +229,7 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
lastTransition = curObs.currentTime;
const Timestamp ts1 = Timestamp::fromUnixTime();
filterUpdate();
filterUpdate();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString filterTime = QString::number(diff.ms());
@@ -231,7 +239,17 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
return true;
} else {
} else if(diff >= Timestamp::fromMS(1000)) {
filterUpdateEstimationOnly();
lastTransition = curObs.currentTime;
const QString evalUpdate = "evalUpdate";
QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, evalUpdate));
return true;
} else {
return false;
@@ -241,6 +259,25 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
DijkstraPath<MyGridNode> pathToDest;
void MeshBased::NavControllerMesh::filterUpdateEstimationOnly() {
//lastEst = curEst;
MyState sCurEst = pf->updateEvaluationOnly(curObs);
curEst.pos_m = sCurEst.loc.pos;
curEst.head = sCurEst.heading;
// inform listeners about the new estimation
for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.pos_m);}
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
// update estimated path
estPath.push_back(curEst.pos_m);
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
}
/** perform a filter-update (called from a background-loop) */
void MeshBased::NavControllerMesh::filterUpdate() {

5
nav/mesh/NavControllerMesh.h
View File

@@ -68,7 +68,7 @@ namespace MeshBased {
/** called when any sensor has received new data */
void gotSensorData(const Timestamp ts);
// void debugActivity(const ActivityData& activity);
void debugActivity(const ActivityData& activity);
/** particle-filter update loop */
void filterUpdateLoop();
@@ -79,6 +79,9 @@ namespace MeshBased {
/** perform a filter-update (called from a background-loop) */
void filterUpdate();
/** perform a filter-update only with estimation (called from a background-loop) */
void filterUpdateEstimationOnly();
/** UI update loop */
void updateMapViewLoop();

26
nav/mesh/State.h
View File

@@ -38,6 +38,32 @@ namespace MeshBased {
return copy;
}
float getX(){
return loc.pos.x;
}
float getY() {
return loc.pos.y;
}
float getZ() {
return loc.pos.z;
}
void setPosition(Point3 pos){
loc.pos = pos;
}
float getBinValue(const int dim) const {
switch (dim) {
case 0: return this->loc.pos.x;
case 1: return this->loc.pos.y;
case 2: return this->loc.pos.z;
case 3: return this->heading.getRAD();
}
throw "cant find this value within the bin";
}
};