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

2
Controller.cpp
View File

@@ -228,7 +228,7 @@ WiFiModel* buildWiFiModelOnce(Floorplan::IndoorMap* map, const std::string& fpFi
WiFiModelFactory fac(map); WiFiModelFactory fac(map);
if(wifiModel){delete wifiModel;} if(wifiModel){delete wifiModel;}
wifiModel = (WiFiModelLogDistCeiling*) fac.loadXML(wifiModelFile); wifiModel = fac.loadXML(wifiModelFile);
} }
} else { } else {

2
Settings.h
View File

@@ -10,7 +10,7 @@
namespace Settings { namespace Settings {
const int numParticles = 2500; const int numParticles = 5000;
namespace IMU { namespace IMU {
const float turnSigma = 1.5; // 3.5 const float turnSigma = 1.5; // 3.5

20
nav/mesh/FilterMesh.h
View File

@@ -21,6 +21,7 @@
#include <Indoor/navMesh/NavMesh.h> #include <Indoor/navMesh/NavMesh.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h> #include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkSinkOrSwim.h>
#include "State.h" #include "State.h"
#include "../Observation.h" #include "../Observation.h"
@@ -74,7 +75,8 @@ namespace MeshBased {
public: 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::NavMeshWalkWifiRegional<NM::NavMeshTriangle>;
//using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>; //using MyNavMeshWalk = NM::NavMeshWalkUnblockable<NM::NavMeshTriangle>;
MyNavMeshWalk walker; MyNavMeshWalk walker;
@@ -87,8 +89,8 @@ namespace MeshBased {
PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){ PFTrans(NM::NavMesh<NM::NavMeshTriangle>* mesh) : walker(*mesh){
// how to evaluate drawn points // how to evaluate drawn points
//walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04)); walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<NM::NavMeshTriangle>(0.04));
//walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1)); walker.addEvaluator(new NM::WalkEvalDistance<NM::NavMeshTriangle>(0.1));
//walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9)); // 90% for particles moving towards the target //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) { double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75; const float kappa = 0.9;
switch (act) { switch (act) {
@@ -167,9 +169,9 @@ namespace MeshBased {
public: public:
//TODO: Was ist hier besser? Im Museum hatten wir das unterste. //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::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 { 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); const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
// sanity check // 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 // assign weights
#pragma omp parallel for num_threads(3) #pragma omp parallel for num_threads(3)
@@ -193,7 +195,9 @@ namespace MeshBased {
const double pStair = getStairProb(p, observation.activity); const double pStair = getStairProb(p, observation.activity);
const double pGPS = 1; 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; p.weight *= prob;
if (p.weight != p.weight) {throw Exception("nan");} 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/navMesh/NavMeshTriangle.h>
#include <Indoor/floorplan/v2/Floorplan.h> #include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
//#ifndef ANDROID //#ifndef ANDROID
//#include <valgrind/callgrind.h> //#include <valgrind/callgrind.h>
//#endif //#endif
@@ -38,12 +41,16 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh)); std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
// estimation // estimation
std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>> estimation(new SMC::ParticleFilterEstimationWeightedAverage<MeshBased::MyState>()); //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::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5));
// resampling // resampling
std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>()); 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>()); //std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// eval and transition // eval and transition
@@ -57,7 +64,7 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
pf->setEstimation(std::move(estimation)); pf->setEstimation(std::move(estimation));
pf->setResampling(std::move(resample)); 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.65); // still too low?
//pf->setNEffThreshold(0.25); // too low //pf->setNEffThreshold(0.25); // too low
@@ -164,7 +171,7 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, co
(void) ts; (void) ts;
curCtrl.activity = data.curActivity; curCtrl.activity = data.curActivity;
curObs.activity = data.curActivity; curObs.activity = data.curActivity;
//debugActivity(data.curActivity); debugActivity(data.curActivity);
gotSensorData(ts); gotSensorData(ts);
} }
@@ -174,16 +181,17 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();} if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
} }
// void debugActivity(const ActivityData& activity) { void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) {
// QString act; QString act;
// switch(activity.curActivity) { switch(activity.curActivity) {
// case Activity::STANDING: act = "STAY"; break; case Activity::STANDING: act = "STAY"; break;
// case Activity::WALKING_DOWN: act = "DOWN"; break; case Activity::WALKING: act = "WALK"; break;
// case Activity::WALKING_UP: act = "UP"; break; case Activity::WALKING_DOWN: act = "DOWN"; break;
// default: act = "???"; 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"); }
// } Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
}
/** particle-filter update loop */ /** particle-filter update loop */
void MeshBased::NavControllerMesh::filterUpdateLoop() { void MeshBased::NavControllerMesh::filterUpdateLoop() {
@@ -221,7 +229,7 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
lastTransition = curObs.currentTime; lastTransition = curObs.currentTime;
const Timestamp ts1 = Timestamp::fromUnixTime(); const Timestamp ts1 = Timestamp::fromUnixTime();
filterUpdate(); filterUpdate();
const Timestamp ts2 = Timestamp::fromUnixTime(); const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1; const Timestamp tsDiff = ts2-ts1;
const QString filterTime = QString::number(diff.ms()); const QString filterTime = QString::number(diff.ms());
@@ -231,7 +239,17 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
return true; 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; return false;
@@ -241,6 +259,25 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
DijkstraPath<MyGridNode> pathToDest; 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) */ /** perform a filter-update (called from a background-loop) */
void MeshBased::NavControllerMesh::filterUpdate() { 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 */ /** called when any sensor has received new data */
void gotSensorData(const Timestamp ts); void gotSensorData(const Timestamp ts);
// void debugActivity(const ActivityData& activity); void debugActivity(const ActivityData& activity);
/** particle-filter update loop */ /** particle-filter update loop */
void filterUpdateLoop(); void filterUpdateLoop();
@@ -79,6 +79,9 @@ namespace MeshBased {
/** perform a filter-update (called from a background-loop) */ /** perform a filter-update (called from a background-loop) */
void filterUpdate(); void filterUpdate();
/** perform a filter-update only with estimation (called from a background-loop) */
void filterUpdateEstimationOnly();
/** UI update loop */ /** UI update loop */
void updateMapViewLoop(); void updateMapViewLoop();

26
nav/mesh/State.h
View File

@@ -38,6 +38,32 @@ namespace MeshBased {
return copy; 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";
}
}; };

2
yasmin.pro
View File

@@ -22,7 +22,7 @@ ANDROID {
#QMAKE_CXXFLAGS += -mtune=cortex-a57 #QMAKE_CXXFLAGS += -mtune=cortex-a57
#QMAKE_CFLAGS += -mtune=cortex-a57 #QMAKE_CFLAGS += -mtune=cortex-a57
#QMAKE_CXXFLAGS += -O2 QMAKE_CXXFLAGS += -O2
#QMAKE_CFLAGS += -O3 #QMAKE_CFLAGS += -O3
#QMAKE_CXXFLAGS_DEBUG -= -O2 #QMAKE_CXXFLAGS_DEBUG -= -O2