diff --git a/CMakeLists.txt b/CMakeLists.txt index f473d22..491a77f 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,8 +41,8 @@ FILE(GLOB SOURCES ./*/*.cpp ./*/*/*.cpp ./*/*/*/*.cpp - ../Indoor/lib/tinyxml/tinyxml2.cpp - ../Indoor/lib/Recast/*.cpp + ../../Indoor/lib/tinyxml/tinyxml2.cpp + ../../Indoor/lib/Recast/*.cpp ) diff --git a/Settings.h b/Settings.h index 4b855b5..ad46368 100644 --- a/Settings.h +++ b/Settings.h @@ -51,9 +51,11 @@ namespace Settings { namespace WiFiModel { constexpr float sigma = 8.0; /** if the wifi-signal-strengths are stored on the grid-nodes, this needs a grid rebuild! */ - constexpr float TXP = -40; - constexpr float EXP = 2.2; - constexpr float WAF = -8.0; + constexpr float TXP = -45; + constexpr float EXP = 2.3; + constexpr float WAF = -11.0; + + const bool optimize = true; // how to perform VAP grouping. see // - calibration in Controller.cpp diff --git a/main.cpp b/main.cpp index b8895b9..319f3dd 100755 --- a/main.cpp +++ b/main.cpp @@ -1,5 +1,4 @@ - #include "navMesh/main.h" int main(int argc, char** argv) { diff --git a/navMesh/filter.h b/navMesh/filter.h index f6bd84b..05c6fc2 100644 --- a/navMesh/filter.h +++ b/navMesh/filter.h @@ -2,20 +2,26 @@ #define NAV_MESH_FILTER_H #include "mesh.h" +#include "../Settings.h" #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include +#include +#include + +#include + struct MyState { /** the state's position (within the mesh) */ @@ -44,6 +50,16 @@ struct MyState { return res; } + float getBinValue(const int dim) const { + switch (dim) { + case 0: return this->pos.pos.x; + case 1: return this->pos.pos.y; + case 2: return this->pos.pos.z; + case 3: return this->heading.getRAD(); + } + throw "cant find this value within the bin"; + } + }; struct MyControl { @@ -61,11 +77,19 @@ struct MyControl { struct MyObservation { + // pressure + float sigmaPressure = 0.10f; + float relativePressure = 0; + //wifi + WiFiMeasurements wifi; + + //time + Timestamp currentTime; }; -class MyPFInitUniform : public K::ParticleFilterInitializer { +class MyPFInitUniform : public SMC::ParticleFilterInitializer { const MyNavMesh* mesh; @@ -75,12 +99,12 @@ public: ; } - virtual void initialize(std::vector>& particles) override { + virtual void initialize(std::vector>& particles) override { /** random position and heading within the mesh */ Distribution::Uniform dHead(0, 2*M_PI); MyNavMeshRandom rnd = mesh->getRandom(); - for (K::Particle& p : particles) { + for (SMC::Particle& p : particles) { p.state.pos = rnd.draw(); p.state.heading = dHead.draw(); p.weight = 1.0 / particles.size(); @@ -89,7 +113,7 @@ public: }; -class MyPFInitFixed : public K::ParticleFilterInitializer { +class MyPFInitFixed : public SMC::ParticleFilterInitializer { const MyNavMesh* mesh; const Point3 pos; @@ -100,11 +124,11 @@ public: ; } - virtual void initialize(std::vector>& particles) override { + virtual void initialize(std::vector>& particles) override { /** random position and heading within the mesh */ Distribution::Uniform dHead(0, 2*M_PI); - for (K::Particle& p : particles) { + for (SMC::Particle& p : particles) { p.state.pos = mesh->getLocation(pos); p.state.heading = dHead.draw(); p.weight = 1.0 / particles.size(); @@ -114,7 +138,7 @@ public: }; -class MyPFTrans : public K::ParticleFilterTransition { +class MyPFTrans : public SMC::ParticleFilterTransition { using MyNavMeshWalk = NM::NavMeshWalkSimple; MyNavMeshWalk walker; @@ -124,20 +148,20 @@ public: MyPFTrans(MyNavMesh& mesh) : walker(mesh) { // how to evaluate drawn points - //walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal(0.04)); - //walker.addEvaluator(new NM::WalkEvalDistance(0.1)); - walker.addEvaluator(new NM::WalkEvalApproachesTarget(0.9)); // 90% for particles moving towards the target + walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal(0.04)); + walker.addEvaluator(new NM::WalkEvalDistance(0.1)); + //walker.addEvaluator(new NM::WalkEvalApproachesTarget(0.9)); // 90% for particles moving towards the target } - void transition(std::vector>& particles, const MyControl* control) override { + void transition(std::vector>& particles, const MyControl* control) override { Distribution::Normal dStepSizeFloor(0.70, 0.1); Distribution::Normal dStepSizeStair(0.35, 0.1); Distribution::Normal dHeading(0.0, 0.10); - for (K::Particle& p : particles) { + for (SMC::Particle& p : particles) { // how to walk MyNavMeshWalkParams params; @@ -148,10 +172,10 @@ public: params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); // walk - MyNavMeshWalk::ResultEntry res = walker.getOne(params); + MyNavMeshWalk::ResultEntry res = walker.getOne(params); // assign back to particle's state - p.weight *= res.probability; + p.weight *= res.probability; p.state.pos = res.location; p.state.heading = res.heading; @@ -165,20 +189,38 @@ public: }; -class MyPFEval : public K::ParticleFilterEvaluation { +class MyPFEval : public SMC::ParticleFilterEvaluation { + + WiFiModel& wifiModel; + WiFiObserverFree wifiProbability; public: - virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { + MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){} - return 1.0; + virtual double evaluation(std::vector>& particles, const MyObservation& observation) override { + + double sum = 0; + const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); + + for (SMC::Particle& p : particles) { + + double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs); + + const double prob = pWifi; + + p.weight *= prob; + sum += prob; + } + + return sum; } }; -using MyFilter = K::ParticleFilter; +using MyFilter = SMC::ParticleFilter; #endif diff --git a/navMesh/main.h b/navMesh/main.h index 728b88b..a5d1a84 100644 --- a/navMesh/main.h +++ b/navMesh/main.h @@ -3,27 +3,80 @@ #include "mesh.h" #include "filter.h" +#include "../Settings.h" + #include #include + #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + void navMeshMain() { - std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml"; + //std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml"; + std::string mapFile = "../map/map42_ap.xml"; + // reading file Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile); + Offline::FileReader fr("../measurements/museum/Pixel/Path1_2468.csv"); + WiFiFingerprints fingerprints("../measurements/museum/Nexus/fingerprints/wifi_fp.dat"); + const std::string wifiModelFile = "../measurements/museum/wifimodel.dat"; + std::ifstream inp(wifiModelFile, std::ifstream::binary); + // wifi + WiFiModelLogDistCeiling WiFiModel(map); + + // with optimization + if(Settings::WiFiModel::optimize){ + + if (!inp.good() || (inp.peek()&&0) || inp.eof()) { + Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!"); + WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib); + for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) { + opt.addFingerprint(fp); + } + 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); + WiFiModel.addAP(ap.mac, entry); + } + + WiFiModel.saveXML(wifiModelFile); + } else { + WiFiModel.loadXML(wifiModelFile); + } + + } else { + // without optimization + WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF); + Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml"); + } + + + + // mesh NM::NavMeshSettings set; MyNavMesh mesh; MyNavMeshFactory fac(&mesh, set); fac.build(map); - const Point3 src(26, 43, 7.5); + const Point3 srcPath0(26, 43, 7.5); + const Point3 srcPath1(62, 38, 1.8); // add shortest-path to destination //const Point3 dst(51, 45, 1.7); - const Point3 dst(25, 45, 0); - NM::NavMeshDijkstra::stamp(mesh, dst); + //const Point3 dst(25, 45, 0); + //NM::NavMeshDijkstra::stamp(mesh, dst); // debug show NM::NavMeshDebug dbg; @@ -32,13 +85,13 @@ void navMeshMain() { dbg.draw(); // particle-filter - const int numParticles = 1000; - auto init = std::make_unique(&mesh, src); // known position - //auto init = std::make_unique(&mesh); // uniform distribution - auto eval = std::make_unique(); + const int numParticles = 1000; + //auto init = std::make_unique(&mesh, srcPath1); // known position + auto init = std::make_unique(&mesh); // uniform distribution + auto eval = std::make_unique(WiFiModel); auto trans = std::make_unique(mesh); - auto resample = std::make_unique>(); - auto estimate = std::make_unique>(); + auto resample = std::make_unique>(); + auto estimate = std::make_unique>(); // setup MyFilter pf(numParticles, std::move(init)); @@ -46,49 +99,85 @@ void navMeshMain() { pf.setTransition(std::move(trans)); pf.setResampling(std::move(resample)); pf.setEstimation(std::move(estimate)); - pf.setNEffThreshold(1); - + pf.setNEffThreshold(1); + // sensors MyControl ctrl; MyObservation obs; - //Distribution::Uniform dHead(0, 2*M_PI); - Distribution::Normal dHead(0, 0.1); + StepDetection sd; + PoseDetection pd; + TurnDetection td(&pd); + RelativePressure relBaro; + relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) ); + Timestamp lastTimestamp = Timestamp::fromMS(0); - for (int i = 0; i < 10000; ++i) { + // parse each sensor-value within the offline data + for (const Offline::Entry& e : fr.getEntries()) { - ctrl.numStepsSinceLastEval = 1; - ctrl.headingChangeSinceLastEval = dHead.draw(); + const Timestamp ts = Timestamp::fromMS(e.ts); - MyState est = pf.update(&ctrl, obs); + if (e.type == Offline::Sensor::WIFI) { + obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data; - ctrl.afterEval(); + } else if (e.type == Offline::Sensor::ACC) { + if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) { + ++ctrl.numStepsSinceLastEval; + } + const Offline::TS& _acc = fr.getAccelerometer()[e.idx]; + pd.addAccelerometer(ts, _acc.data); - try { - MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos); - auto path = loc.tria->getPathToDestination(loc.pos); - dbg.addDijkstra(path); - } catch (...) {;} + } else if (e.type == Offline::Sensor::GYRO) { + const Offline::TS& _gyr = fr.getGyroscope()[e.idx]; + const float delta_gyro = td.addGyroscope(ts, _gyr.data); - const int d = (i * 1) % 360; - dbg.plot.getView().setCamera(60, d); - dbg.showParticles(pf.getParticles()); - dbg.setCurPos(est.pos.pos); + ctrl.headingChangeSinceLastEval += delta_gyro; - //dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png"); - //dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30)); + } else if (e.type == Offline::Sensor::BARO) { + relBaro.add(ts, fr.getBarometer()[e.idx].data); + obs.relativePressure = relBaro.getPressureRealtiveToStart(); + obs.sigmaPressure = relBaro.getSigma(); + } - std::cout << i << std::endl; + if (ts.ms() - lastTimestamp.ms() > 500 && ctrl.numStepsSinceLastEval > 0) { - dbg.draw(); + obs.currentTime = ts; +// if(ctrl.numStepsSinceLastEval > 0){ +// pf.updateTransitionOnly(&ctrl); +// ctrl.afterEval(); +// } +// MyState est = pf.updateEvaluationOnly(obs); +// lastTimestamp = ts; - std::this_thread::sleep_for(std::chrono::milliseconds(5)); + MyState est = pf.update(&ctrl, obs); + ctrl.afterEval(); + lastTimestamp = ts; - } + // try { + // MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos); + // auto path = loc.tria->getPathToDestination(loc.pos); + // dbg.addDijkstra(path); + // } catch (...) {;} + // const int d = (i * 1) % 360; + // dbg.plot.getView().setCamera(60, d); + dbg.showParticles(pf.getParticles()); + dbg.setCurPos(est.pos.pos); + //dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png"); + //dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30)); + + // std::cout << i << std::endl; + + dbg.draw(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + + } + + } } #endif