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

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() {