diff --git a/Controller.cpp b/Controller.cpp index c974e83..eae8af9 100644 --- a/Controller.cpp +++ b/Controller.cpp @@ -190,13 +190,14 @@ void buildGridOnce(Grid* grid, Floorplan::IndoorMap* map, const std: } -void buildWiFiModelOnce(WiFiModel* wifiModel, Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& wifiModelFile){ +WiFiModel* buildWiFiModelOnce(Floorplan::IndoorMap* map, const std::string& fpFile, const std::string& wifiModelFile){ // ask questions const QMessageBox::StandardButton replyWiFiFP = QMessageBox::question(nullptr, "WiFi", "Use Fingerprints for WiFiCalibration?\n\nYes: Use fingerprints and num-optimize AP-Params\nNo: Use APs from the map.xml (pos+params)", QMessageBox::Yes|QMessageBox::No); + WiFiModel* wifiModel = new WiFiModelLogDistCeiling(map); + // WiFi setup - wifiModel = new WiFiModelLogDistCeiling(map); if (replyWiFiFP == QMessageBox::Yes) { std::ifstream inp(wifiModelFile, std::ifstream::binary); @@ -214,7 +215,7 @@ void buildWiFiModelOnce(WiFiModel* wifiModel, Floorplan::IndoorMap* map, const s const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE); for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) { const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf); - ((WiFiModelLogDistCeiling*)wifiModel)->addAP(ap.mac, entry); + ((WiFiModelLogDistCeiling*) wifiModel)->addAP(ap.mac, entry); } wifiModel->saveXML(wifiModelFile); @@ -225,6 +226,8 @@ void buildWiFiModelOnce(WiFiModel* wifiModel, Floorplan::IndoorMap* map, const s // load WiFiModel from file. The factory will create the correct instance //WiFiModel->loadXML(setup.wifiModel); WiFiModelFactory fac(map); + + if(wifiModel){delete wifiModel;} wifiModel = fac.loadXML(wifiModelFile); } @@ -236,6 +239,7 @@ void buildWiFiModelOnce(WiFiModel* wifiModel, Floorplan::IndoorMap* map, const s Assert::isFalse(wifiModel->getAllAPs().empty(), "no AccessPoints stored within the map.xml"); } + return wifiModel; } @@ -327,11 +331,12 @@ void Controller::loadNavMesh(QDir dir) { const std::string sWiFiFP = fWiFiFP.fileName().toStdString(); const std::string sWiFiModel = fWiFiModel.fileName().toStdString(); - buildWiFiModelOnce(wifiModel, im, sWiFiFP, sWiFiModel); + wifiModel = buildWiFiModelOnce(im, sWiFiFP, sWiFiModel); // create navmesh if (navMesh) {delete navMesh; navMesh = nullptr;} NM::NavMeshSettings settings; + navMesh = new NM::NavMesh(); NM::NavMeshFactory fac(navMesh, settings); fac.build(im); @@ -344,8 +349,8 @@ void Controller::loadNavMesh(QDir dir) { getMapView3D()->setMap(im); getMapView2D()->setMap(wifiCalib, im); - getMapView3D()->showGridImportance(grid); - getMapView2D()->showGridImportance(grid); + //getMapView3D()->showGridImportance(grid); + //getMapView2D()->showGridImportance(grid); getMapView3D()->setVisible(false); diff --git a/Settings.h b/Settings.h index 3dd5ded..c387656 100644 --- a/Settings.h +++ b/Settings.h @@ -10,7 +10,7 @@ namespace Settings { - const int numParticles = 5000; + const int numParticles = 2500; namespace IMU { const float turnSigma = 1.5; // 3.5 @@ -39,12 +39,12 @@ namespace Settings { namespace WiFiModel { - constexpr float sigma = 13.0; //TODO: im Museum hatten wir 8.0 + constexpr float sigma = 8.0; //TODO: im Museum hatten wir 8.0 /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ - constexpr float TXP = -48; - constexpr float EXP = 2.5; - constexpr float WAF = -5.0; + constexpr float TXP = -45; + constexpr float EXP = 2.3; + constexpr float WAF = -5.0; // how to perform VAP grouping. see // - calibration in Controller.cpp @@ -88,7 +88,10 @@ namespace Settings { std::string tried; QDir dStorage("/storage"); QStringList lst = dStorage.entryList(); - lst.append("/sdcard/YASMIN/"); + lst.append("sdcard/YASMIN/"); + lst.append("emulated/0"); + + //TODO: THIS IS A MESS, PURE CHAOS xD // try each potential folder for (const QString subfolder : lst) { diff --git a/nav/mesh/FilterMesh.h b/nav/mesh/FilterMesh.h index d181793..4409c62 100644 --- a/nav/mesh/FilterMesh.h +++ b/nav/mesh/FilterMesh.h @@ -193,7 +193,7 @@ namespace MeshBased { const double pStair = getStairProb(p, observation.activity); const double pGPS = 1; - const double prob = pWifi * pStair * pGPS; + const double prob = pWifi; // * pStair * pGPS; p.weight *= prob; if (p.weight != p.weight) {throw Exception("nan");} diff --git a/nav/mesh/NavControllerMesh.cpp b/nav/mesh/NavControllerMesh.cpp index 4350d11..0050966 100644 --- a/nav/mesh/NavControllerMesh.cpp +++ b/nav/mesh/NavControllerMesh.cpp @@ -42,9 +42,9 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo //std::unique_ptr> estimation(new SMC::ParticleFilterEstimationOrderedWeightedAverage(0.5)); // resampling - //std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimple()); + std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimple()); //std::unique_ptr> resample(new SMC::ParticleFilterResamplingPercent(0.05)); - std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment()); + //std::unique_ptr> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment()); // eval and transition std::unique_ptr> eval(new MeshBased::PFEval(wifiModel)); @@ -218,16 +218,17 @@ void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) { // as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one const Timestamp err = diff - Settings::Filter::updateEvery; - lastTransition = curObs.currentTime - err; + lastTransition = curObs.currentTime; const Timestamp ts1 = Timestamp::fromUnixTime(); filterUpdate(); const Timestamp ts2 = Timestamp::fromUnixTime(); const Timestamp tsDiff = ts2-ts1; - const QString filterTime = QString::number(tsDiff.ms()); + const QString filterTime = QString::number(diff.ms()); avgSum += tsDiff.ms(); ++avgCount; //Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount)); - QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime)); + QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime)); + return true; } else { diff --git a/ui/map/2D/ColorPoints2D.h b/ui/map/2D/ColorPoints2D.h index 7bd5fb9..d86eea7 100644 --- a/ui/map/2D/ColorPoints2D.h +++ b/ui/map/2D/ColorPoints2D.h @@ -11,6 +11,11 @@ #include "../nav/grid/Node.h" #include "../nav/Observation.h" +#include "../nav/grid/State.h" +#include "../nav/mesh/State.h" + +#include + /** * debug color points */ @@ -59,38 +64,59 @@ public: } /** NOTE: must be called from Qt's main thread! */ - template void setFromParticles(const std::vector>& particles) { + void setFromParticles(const std::vector>& particles){ - points.clear(); + points.clear(); - // group particles by grid-point - std::unordered_map weights; - for (const SMC::Particle& p : particles) { - const GridPoint gp = p.state.position; - if (weights.find(gp) != weights.end()) {continue;} - weights[gp] += p.weight; - } + // group particles by grid-point + std::unordered_map weights; + for (const SMC::Particle& p : particles) { + const GridPoint gp = p.state.position; + if (weights.find(gp) != weights.end()) {continue;} + weights[gp] += p.weight; + } - // find min/max - float min = +INFINITY; - float max = -INFINITY; - for (auto it : weights) { - if (it.second > max) {max = it.second;} - if (it.second < min) {min = it.second;} - } + // find min/max + float min = +INFINITY; + float max = -INFINITY; + for (auto it : weights) { + if (it.second > max) {max = it.second;} + if (it.second < min) {min = it.second;} + } - // draw colored - for (auto it : weights) { - const GridPoint gp = it.first; - const float w = it.second; - const float p = (w-min) / (max-min); // [0:1] - const Point3 pt(gp.x_cm/100.0f, gp.y_cm/100.0f + 0.1f, gp.z_cm/100.0f); - float h = 0.66 - (p*0.66); // 0.66 is blue on the HSV-scale - const QColor color = QColor::fromHsvF(h, 1, 1); - points.push_back(PT(pt, color)); - } + // draw colored + for (auto it : weights) { + const GridPoint gp = it.first; + const float w = it.second; + const float p = (w-min) / (max-min); // [0:1] + const Point3 pt(gp.x_cm/100.0f, gp.y_cm/100.0f + 0.1f, gp.z_cm/100.0f); + float h = 0.66 - (p*0.66); // 0.66 is blue on the HSV-scale + const QColor color = QColor::fromHsvF(h, 1, 1); + points.push_back(PT(pt, color)); + } - } + } + + void setFromParticles(const std::vector>& particles){ + + points.clear(); + + float min = +INFINITY; + float max = -INFINITY; + for (const auto p : particles){ + if (p.weight > max) {max = p.weight;} + if (p.weight < min) {min = p.weight;} + } + + for (const auto p : particles) { + const Point3 pt(p.state.loc.pos.x, p.state.loc.pos.y, p.state.loc.pos.z); + const float prob = (p.weight-min) / (max-min); + float h = 0.66 - (prob*0.66); // 0.66 is blue on the HSV-scale + const QColor color = QColor::fromHsvF(h, 1, 1); + points.push_back(PT(pt, color)); + + } + } protected: diff --git a/ui/map/2D/MapView2D.cpp b/ui/map/2D/MapView2D.cpp index 6ae40c5..9d8488c 100644 --- a/ui/map/2D/MapView2D.cpp +++ b/ui/map/2D/MapView2D.cpp @@ -115,6 +115,10 @@ void MapView2D::showParticles(const std::vectorcolorPoints->setFromParticles(*particles); } +void MapView2D::showParticles(const std::vector>* particles) { + this->colorPoints->setFromParticles(*particles); +} + void MapView2D::setCurrentEstimation(const Point3 pos_m, const Point3 dir) { (void) dir; setRenderHeight(pos_m.z); diff --git a/ui/map/2D/MapView2D.h b/ui/map/2D/MapView2D.h index 3fccf8f..7e7c7ce 100644 --- a/ui/map/2D/MapView2D.h +++ b/ui/map/2D/MapView2D.h @@ -13,6 +13,7 @@ #include #include "nav/grid/State.h" +#include "nav/mesh/State.h" namespace Floorplan { class IndoorMap; @@ -101,13 +102,16 @@ public: /** NOTE: must be called from Qt's main thread! */ Q_INVOKABLE void showParticles(const void* particles) { - showParticles((const std::vector>*) particles); + showParticles((const std::vector>*) particles); } /** NOTE: must be called from Qt's main thread! */ void showParticles(const std::vector>* particles); + /** NOTE: must be called from Qt's main thread! */ + void showParticles(const std::vector>* particles); + /** set the currently estimated position */ void setCurrentEstimation(const Point3 pos, const Point3 dir); diff --git a/yasmin.pro b/yasmin.pro index b805239..b913ad7 100644 --- a/yasmin.pro +++ b/yasmin.pro @@ -22,7 +22,7 @@ ANDROID { #QMAKE_CXXFLAGS += -mtune=cortex-a57 #QMAKE_CFLAGS += -mtune=cortex-a57 - #QMAKE_CXXFLAGS += -O3 + #QMAKE_CXXFLAGS += -O2 #QMAKE_CFLAGS += -O3 #QMAKE_CXXFLAGS_DEBUG -= -O2