From cc899d1c4679d4c8a237c0d2fd6f4f270f5d6139 Mon Sep 17 00:00:00 2001 From: FrankE Date: Sat, 30 Jan 2016 19:50:58 +0100 Subject: [PATCH] updates the visualisation removed obsolte parts fixed baromter stuff worked on eval added ground-truth --- code/Helper.h | 14 +++ code/OldGroundTruth.h | 138 ++++++++++++++++++++++++++++++ code/Settings.h | 2 +- code/Vis.h | 46 ++++++++++ code/eval/Eval.h | 12 ++- code/eval/EvalBase.h | 88 ++++++++++++------- code/eval/GroundTruthWay.h | 2 +- code/frank/BeaconEvaluation.h | 4 +- code/frank/PositionedBeacon.h | 2 +- code/frank/Settings.h | 24 +++--- code/frank/WiFiEvaluation.h | 13 ++- code/particles/MyEvaluation.h | 12 +-- code/particles/MyState.h | 10 ++- code/particles/MyTransition.h | 20 +---- code/particles/P3.h | 31 ------- code/plan.svg | 25 +++--- code/toni/BarometerEvaluation.h | 20 ++--- code/toni/BarometerSensorReader.h | 115 ++++++++++++++----------- 18 files changed, 388 insertions(+), 190 deletions(-) create mode 100644 code/OldGroundTruth.h delete mode 100755 code/particles/P3.h diff --git a/code/Helper.h b/code/Helper.h index dcf5fee..58eb290 100644 --- a/code/Helper.h +++ b/code/Helper.h @@ -9,6 +9,7 @@ #include "Settings.h" #include "MyGridNode.h" +#include "OldGroundTruth.h" class Helper { @@ -62,6 +63,9 @@ public: const LengthF h2 = LengthF::cm(align(getHeight(2))); const LengthF h3 = LengthF::cm(align(getHeight(3))); + // all ground-truth points + std::unordered_map gtwp; + FHWSFloors() {;} }; @@ -81,6 +85,16 @@ public: f.s12 = fpFac.getStairs("staircase_1_2"); f.s23 = fpFac.getStairs("staircase_2_3"); + OldGroundTruth gtwp0(MiscSettings::floorplan, "ground_truth_0", 2.822222); + OldGroundTruth gtwp1(MiscSettings::floorplan, "ground_truth_1", 2.822222); + OldGroundTruth gtwp2(MiscSettings::floorplan, "ground_truth_2", 2.822222); + OldGroundTruth gtwp3(MiscSettings::floorplan, "ground_truth_3", 2.822222); + + for (auto it : gtwp0.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(0));} + for (auto it : gtwp1.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(1));} + for (auto it : gtwp2.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(2));} + for (auto it : gtwp3.getWaypoints()) {f.gtwp[it.first] = Point3(it.second.x, it.second.y, getHeight(3));} + return f; } diff --git a/code/OldGroundTruth.h b/code/OldGroundTruth.h new file mode 100644 index 0000000..c3a4f91 --- /dev/null +++ b/code/OldGroundTruth.h @@ -0,0 +1,138 @@ +#ifndef OLDGROUNDTRUTH_H +#define OLDGROUNDTRUTH_H + +#include +#include +#include +#include + +/** + * TODO: REMOVE + */ +class OldGroundTruth { + + /** + * helper class for SVG floorplans. + * + * converts between the SVG's scale and real-world scale + */ + class SVGScaler { + + private: + + /** the scaling factor to apply to the svg data */ + double scalingFactor; + + public: + + /** ctor */ + SVGScaler(const double scalingFactor) : scalingFactor(scalingFactor) { + ; + } + + /** scale (x, y) into (_x, _y) */ + void scale(const double x, const double y, float& _x, float& _y) const { + _x = x * scalingFactor; + _y = y * scalingFactor; + } + + /** scale the given point into a new output point */ + Point2 scale(const K::Point p) const { + Point2 ret; + scale (p.x, p.y, ret.x, ret.y); + return ret; + } + + /** scale the given line into a new output line */ + Line2 scale(const K::Line l) const { + Line2 ret; + scale (l.p1.x, l.p1.y, ret.p1.x, ret.p1.y); + scale (l.p2.x, l.p2.y, ret.p2.x, ret.p2.y); + return ret; + } + + }; + +private: + + /** helper to scale the SVG into real-world-scale */ + SVGScaler scaler; + + /** all ground-truth waypoints within the floorplan */ + std::unordered_map points; + +public: + + OldGroundTruth() :scaler(0) {;} + + /** + * ctor + * @param file the svg's filename + * @param layerName the name of the layer (within the SVG) to load + * @param scalingFactor the scaling to apply to convert between SVG and real-world scale + */ + OldGroundTruth(const std::string& file, const std::string& layerName, const double scalingFactor) : scaler(scalingFactor) { + + K::SVGFile svg; + K::SVGLoader::load(K::File(file), &svg); + K::SVGComposite* sc = svg.getLayers(); + + K::SVGLayer* layer = sc->getContainedLayerNamed(layerName); + if (!layer) {throw "svg has no layer named '" + layerName + "'";} + load(layer); + + } + + /** get all waypoints */ + const std::unordered_map& getWaypoints() const { + return points; + } + + +private: + + + + /** recursive loading/parsing of nested SVG elements */ + void load(K::SVGElement* el) { + + switch (el->getType()) { + + case SVGElementType::COMPOSITE: { + for (K::SVGElement* sub : ((K::SVGComposite*)el)->getChilds()) { + load(sub); + } + break; + } + + case SVGElementType::LAYER: { + K::SVGLayer* layer = (K::SVGLayer*) el; + for (K::SVGElement* sub : layer->getChilds()) { + load(sub); + } + break; + } + + case SVGElementType::TEXT: { + const K::SVGText* text = (K::SVGText*) el; + if (text->getText().empty()) {break;} + const int id = std::stoi(text->getText()); + points[id] = scaler.scale(K::Point(text->getPosition().x, text->getPosition().y)); + break; + } + + case SVGElementType::PATH: { + break; + } + + default: + throw "should not happen!"; + + } + + } + + +}; + +#endif // OLDGROUNDTRUTH_H diff --git a/code/Settings.h b/code/Settings.h index 1722a92..bbb1900 100644 --- a/code/Settings.h +++ b/code/Settings.h @@ -16,7 +16,7 @@ namespace MiscSettings { const int timeSteps = 500; - const int numParticles = 1000; + const int numParticles = 2500; } diff --git a/code/Vis.h b/code/Vis.h index d90fbeb..d7d4035 100644 --- a/code/Vis.h +++ b/code/Vis.h @@ -10,6 +10,8 @@ #include #include +#include "eval/GroundTruthWay.h" + class Vis { public: @@ -20,6 +22,8 @@ public: K::GnuplotSplotElementColorPoints gridNodes; K::GnuplotSplotElementLines gridEdges; K::GnuplotSplotElementPoints particles; + K::GnuplotSplotElementLines groundTruth; + K::GnuplotSplotElementLines estPath; public: @@ -35,11 +39,18 @@ public: gp << "unset ztics\n"; gp << "unset border\n"; + groundTruth.setLineWidth(2); + groundTruth.setColorHex("#666666"); + + estPath.setLineWidth(2); + // attach all layers splot.add(&floors); splot.add(&gridNodes); splot.add(&gridEdges); splot.add(&particles); + splot.add(&groundTruth); + splot.add(&estPath); } @@ -81,6 +92,27 @@ public: return *this; } + void addGroundTruth(GroundTruthWay& gtw) { + groundTruth.clear(); + for (auto it : gtw.getWay()) { + K::GnuplotPoint3 gp(it.value.x, it.value.y, it.value.z); + groundTruth.add(gp); + } + } + + void addEstPath(std::vector& est) { + estPath.clear();; + for (const Point3& p : est) { + K::GnuplotPoint3 gp(p.x, p.y, p.z); + estPath.add(gp); + } + } + + void setTimestamp(uint64_t ts) { + static uint64_t firstTs = ts; + gp << "set label 1 \"" << ((ts-firstTs)/1000.0f) << "\" at screen 0.02,0.98\n"; + } + void removeGrid() { gridNodes.clear();; } @@ -89,6 +121,20 @@ public: particles.clear(); } + void addObject(const int idx, const Point3& p) { + gp << "set object " << idx << " polygon "; + gp << "from " << p.x << "," << p.y << "," << p.z; + gp << " to " << p.x << "," << p.y << "," << p.z + 200; + gp << " to " << p.x << "," << p.y << "," << p.z; // close + gp << " lw 2 "; + gp << "\n"; + } + + void setEstAndShould(const Point3& est, const Point3& should) { + addObject(2,est); + addObject(3,should); + } + template void addState(const GridWalkState& n) { particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm)); } diff --git a/code/eval/Eval.h b/code/eval/Eval.h index 45b8b8d..dd8a7bf 100644 --- a/code/eval/Eval.h +++ b/code/eval/Eval.h @@ -9,6 +9,8 @@ #include #include #include +#include +#include class Eval : public EvalBase { @@ -26,15 +28,16 @@ public: //GridWalkLightAtTheEndOfTheTunnel* walk = new GridWalkLightAtTheEndOfTheTunnel(grid, DijkstraMapper(grid), end); //GridWalkRandomHeadingUpdate* walk = new GridWalkRandomHeadingUpdate(); - //GridWalkRandomHeadingUpdateAdv* walk = new GridWalkRandomHeadingUpdateAdv(); - GridWalkPushForward* walk = new GridWalkPushForward(); + GridWalkRandomHeadingUpdateAdv* walk = new GridWalkRandomHeadingUpdateAdv(); + //GridWalkPushForward* walk = new GridWalkPushForward(); pf->setTransition( std::unique_ptr( new MyTransition(grid, *walk)) ); sr = new SensorReader("./measurements/13/Galaxy/Path2/1433588396094.csv"); srt = new SensorReaderTurn("./measurements/13/Galaxy/Path2/Turns.txt"); srs = new SensorReaderStep("./measurements/13/Galaxy/Path2/Steps2.txt"); - //gtw = getGroundTruthWay(*sr, gtwp, way2); + + gtw = getGroundTruthWay(*sr, floors.gtwp, way2); } @@ -47,7 +50,7 @@ public: // the particle filter's evaluation method std::unique_ptr eval = std::unique_ptr( new MyEvaluation() ); - eval.get()->setUsage(true, false, false, false, false); + eval.get()->setUsage(true, false, false, true, true); pf->setEvaluation( std::move(eval) ); // resampling step? @@ -56,6 +59,7 @@ public: // state estimation step pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationWeightedAverage())); + //pf->setEstimation( std::unique_ptr>(new K::ParticleFilterEstimationRegionalWeightedAverage())); } diff --git a/code/eval/EvalBase.h b/code/eval/EvalBase.h index 41b0714..99a4c15 100644 --- a/code/eval/EvalBase.h +++ b/code/eval/EvalBase.h @@ -10,7 +10,6 @@ #include "GroundTruthWay.h" -#include "../particles/P3.h" #include "../particles/MyState.h" #include "../particles/MyObservation.h" #include "../particles/MyEvaluation.h" @@ -43,10 +42,15 @@ protected: SensorReaderTurn* srt; SensorReaderStep* srs; - GroundTruthWay gtw; + std::string runName; + GroundTruthWay gtw; + std::vector way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0}; + std::vector way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0}; + std::vector way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29}; + public: EvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors()) { @@ -62,11 +66,39 @@ public: } + GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map& waypoints, std::vector ids) { + + // construct the ground-truth-path by using all contained waypoint ids + std::vector path; + for (int id : ids) { + auto it = waypoints.find(id); + assert(it != waypoints.end()); + path.push_back(it->second); + } + + // new created the timed path + GroundTruthWay gtw; + int i = 0; + while (sr.hasNext()) { + const SensorEntry se = sr.getNext(); + if (se.data.empty()) {continue;} // why necessary?? + if (se.idx == 99) { + gtw.add(se.ts, path[i]); + ++i; + } + } + + // ensure the sensor-data contained usable timestamps for the ground-truth mapping + assert(i>0); + + sr.rewind(); + return gtw; + + } + void run() { - // read CSV input - // const int s_wifi = 0; - //SensorReader sr("/apps/workspaces/ipin2015/measurements/2/1427362412784.csv"); + // sensor numbers const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_linearAcceleration = 2; @@ -111,10 +143,10 @@ public: std::vector pathEst; uint64_t lastTransitionTS = 0; - bool firstReading = true; int64_t start_time = -1; K::Statistics stats; + int cnt = 0; // process each sensor reading while(sr->hasNext()) { @@ -127,12 +159,6 @@ public: if (start_time == -1) {start_time = se.ts;} int64_t current_time = se.ts - start_time; - // ensure the graph timestamp starts with the first reading - if (firstReading) { - //vis.debugProcess(se.ts, pathEst, gtw, pf, layers); - firstReading = false; - } - switch(se.idx) { case s_wifi: { @@ -203,37 +229,33 @@ public: } - // let the transition know the current timestamp to determine the time since the last transition - //if (!useSimpleTrans) { - ((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS); - //} else { - // ((MyTransitionSimple*)pf->getTransition())->setCurrentTime(lastTransitionTS); - //} + // timed updates + ((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS); + // update the particle filter (transition + eval), estimate a new current position and add it to the estimated path const MyState est = pf->update(nullptr, obs); const Point3 curEst = est.pCur; - pathEst.push_back(curEst); - - // debug print current particle set. - //vis.debugProcess(se.ts, pathEst, gtw, pf, layers); // error calculation. compare ground-truth to estimation -// const Point3 curGT = gtw.getPosAtTime(se.ts - 750); + const Point3 curGT = gtw.getPosAtTime(se.ts - 750); + const Point3 diff = curEst - curGT; -// // TODO -// const Point3 diff = curEst - curGT; - -// //if (std::abs(diff.z) < 0.1) { -// const float err = diff.length(); -// std::cout << err << std::endl; -// stats.add(err); -// std::cout << stats.asString() << std::endl; -// //} + // skip the first 8 scans due to uniform distribution start + if (++cnt > 8) { + pathEst.push_back(curEst); + const float err = diff.length(); + stats.add(err); + std::cout << stats.asString() << std::endl; + } + // plot vis.clearStates(); for (const K::Particle p : pf->getParticles()) {vis.addState(p.state.walkState);} - + vis.setTimestamp(se.ts); + vis.addGroundTruth(gtw); + vis.addEstPath(pathEst); + vis.setEstAndShould(curEst, curGT); vis.show();; } diff --git a/code/eval/GroundTruthWay.h b/code/eval/GroundTruthWay.h index b7631dc..2e1387c 100644 --- a/code/eval/GroundTruthWay.h +++ b/code/eval/GroundTruthWay.h @@ -16,7 +16,7 @@ public: } /** get the ground truth way */ - const std::vector& getWay() const {return entries;} + const std::vector& getWay() const {return entries;} }; diff --git a/code/frank/BeaconEvaluation.h b/code/frank/BeaconEvaluation.h index 3ac5eda..19f38b1 100755 --- a/code/frank/BeaconEvaluation.h +++ b/code/frank/BeaconEvaluation.h @@ -23,7 +23,7 @@ public: double prob = 1.0; // const double tx = -74; - const double waf = 20.0; + const double waf = 8.0; // // get the ap the client had the strongest measurement for // const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP); @@ -40,7 +40,7 @@ public: // distance (in meter) between particle and AP //const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0; - const double distToBeacon_m = state.pCur.getDistance(Point3(beacon->x, beacon->y, beacon->z)) / 100.0; + const double distToBeacon_m = state.pCur.getDistance(*beacon) / 100.0; // floor difference? //const double floorDist = std::abs(beacon->zNr - state.getFloorNr()); diff --git a/code/frank/PositionedBeacon.h b/code/frank/PositionedBeacon.h index 534df8c..58f76e4 100755 --- a/code/frank/PositionedBeacon.h +++ b/code/frank/PositionedBeacon.h @@ -21,7 +21,7 @@ public: /** ctor */ PositionedBeacon(const MACAddress& mac, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) : - mac(mac), tx(tx), pl(pl), Point3(x_cm, y_cm, z_cm) { + Point3(x_cm, y_cm, z_cm), mac(mac), tx(tx), pl(pl) { ; } diff --git a/code/frank/Settings.h b/code/frank/Settings.h index cc82dbb..6818d7c 100755 --- a/code/frank/Settings.h +++ b/code/frank/Settings.h @@ -20,27 +20,27 @@ public: Settings() { - const double pl = 2.7; + const double pl = 2.7; // 2.7 const double tx = -46; - addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl-0.5); - addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl-0.5); - addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-0.5); + addAP(("00:04:96:6b:64:99"), "i.3.20", 290, 1300, Helper::getHeight(3), tx, pl); + addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, Helper::getHeight(3), tx, pl); + addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, Helper::getHeight(3), tx, pl-1); addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, Helper::getHeight(3), tx, pl); addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, Helper::getHeight(3), tx, pl); // 2nd floor (vague AP position) - addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, Helper::getHeight(2), tx, pl-0.5); + addAP(("00:04:96:6c:3a:a9"), "I.2.1", 6750, 3350, Helper::getHeight(2), tx, pl); addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, Helper::getHeight(2), tx, pl); addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, Helper::getHeight(2), tx, pl); - addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, Helper::getHeight(2), tx, pl-0.5); - addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl-0.5); + addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, Helper::getHeight(2), tx, pl); + addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, Helper::getHeight(2), tx, pl); // 1st floor (vague AP position) addAP(("00:04:96:6c:cf:19"), "I.1.2", 6150, 3420, Helper::getHeight(1), tx, pl); addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, Helper::getHeight(1), tx, pl); - addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, Helper::getHeight(1), tx, pl-0.25); - addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, Helper::getHeight(1), tx, pl-0.25); + addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, Helper::getHeight(1), tx, pl); + addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, Helper::getHeight(1), tx, pl); addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, Helper::getHeight(1), tx, pl); addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, Helper::getHeight(1), tx, pl); @@ -63,9 +63,9 @@ public: addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, Helper::getHeight(2)); // idis outside addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, Helper::getHeight(2)); // idis toni - addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, Helper::getHeight(1)); // i.1.47 - addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, Helper::getHeight(1)); // fachschaft - addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, Helper::getHeight(1)); // neben fachschaft + //addBeacon("78:A5:04:1E:B1:50", -88+ibOff-4, ibPLE, 6108, 4528, Helper::getHeight(1)); // i.1.47 + //addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, Helper::getHeight(1)); // fachschaft + //addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, Helper::getHeight(1)); // neben fachschaft // addBeacon("00:07:80:78:F7:B3", -82, ibPLE, 1038, 4018, 3); // addBeacon("78:A5:04:1F:93:02", -88, ibPLE, 1538, 4038, 3); diff --git a/code/frank/WiFiEvaluation.h b/code/frank/WiFiEvaluation.h index 16ea667..c33ebf4 100755 --- a/code/frank/WiFiEvaluation.h +++ b/code/frank/WiFiEvaluation.h @@ -38,10 +38,9 @@ public: //const double tx = -48; // tablet //const double pl = 3.15; const float waf = 7;//10.0; // was 7 before?! has something todo with the floor heights / levels - // const int floor_height_cm = 350; // get the ap the client had the strongest measurement for - const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP); + //const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP); //const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0; //const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr); //const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf); @@ -53,13 +52,13 @@ public: const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap); // distance (in meter) between particle and AP - //const double distToAP_m = state.getDistance3D(ap->xCM, ap->yCM, floor_height_cm) / 100.0; - const float distToAP_m = state.pCur.getDistance(Point3(ap->x, ap->y, ap->z)) / 100.0; + const float distToAP_m = state.pCur.getDistance(*ap) / 100.0; // floor difference? - const float floorDiff = std::round( - std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z)) - ); + const float floorDiff = //std::ceil( + std::abs(Helper::getFloorNrFloat(ap->z) - Helper::getFloorNrFloat(state.pCur.z)); + //); + //const float floorDiff = std::abs(ap->z - state.pCur.z) / 340; // estimate the rssi depending on above distance const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf); diff --git a/code/particles/MyEvaluation.h b/code/particles/MyEvaluation.h index 4c011d6..09a02f8 100755 --- a/code/particles/MyEvaluation.h +++ b/code/particles/MyEvaluation.h @@ -53,13 +53,13 @@ public: weight *= wifiEval.getProbability(p.state, observation); } -// if (useBaro && observation.barometer) { -// weight *= barometerEval.getProbability(p.state, observation.barometer); -// } + if (useBaro && observation.barometer) { + weight *= barometerEval.getProbability(p.state, observation.barometer); + } -// if (useIB) { -// weight *= beaconEval.getProbability(p.state, observation); -// } + if (useIB) { + weight *= beaconEval.getProbability(p.state, observation); + } // if (useStep) { // weight *= stepEval.getProbability(p.state, observation.step); diff --git a/code/particles/MyState.h b/code/particles/MyState.h index 1fec717..48ffd3d 100755 --- a/code/particles/MyState.h +++ b/code/particles/MyState.h @@ -65,14 +65,14 @@ struct MyState { MyState& operator += (const MyState& o) { pCur += o.pCur; - //hPa += o.hPa; + hPa += o.hPa; //distanceWalked += o.distanceWalked; return *this; } MyState& operator /= (const double d) { pCur /= d; - //hPa /= d; + hPa /= d; //distanceWalked /= d; return *this; } @@ -80,7 +80,7 @@ struct MyState { MyState operator * (const double d) const { MyState s = MyState(*this); s.pCur *= d; - //s.hPa *= d; + s.hPa *= d; //distanceWalked *= d; return s; } @@ -101,6 +101,10 @@ struct MyState { // return *this; // } + bool belongsToRegion(const MyState& o) const { + return o.pCur.getDistance(pCur) < 700; + } + // /** rejection for the regional estimator. reject after 150cm distance */ // bool belongsToRegion(const MyState& o) const { diff --git a/code/particles/MyTransition.h b/code/particles/MyTransition.h index 29a6fc5..824d02e 100755 --- a/code/particles/MyTransition.h +++ b/code/particles/MyTransition.h @@ -48,7 +48,7 @@ public: */ MyTransition(Grid& grid, GridWalk& walker) : grid(grid), walker(walker), - distWalkStop(0.0, 1.0), distWalk(1.3, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) { + distWalkStop(0.0, 1.0), distWalk(1.5, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) { distWalkStop.setSeed(1234); distWalk.setSeed(1234); @@ -101,20 +101,6 @@ public: // update cumulative distance p.state.distanceWalkedCM += std::abs(dist_m * 100.0); - // find the node (square) the particle is within - // just to be safe, we round z to the nearest floor - //Node3* src = graph->getNearestNode(p.state.x_cm, p.state.y_cm, std::round(p.state.z_nr)); - const MyGridNode* src = p.state.walkState.node; - - // might happen during initialization: - // the particle is nowhere near the grid.. replace it with a random on on the grid - // alternative: just ignore.. resampling will fix this issue quickly ;) -// if (!src) { -// auto it = graph->getNodes().begin(); -// std::advance(it, rand() % graph->getNodes().size()); -// src = it->second; -// } - // get new destination //const Node3* dst = choice->getTarget(src, p.state, dist_m); p.state.walkState = walker.getDestination(grid, p.state.walkState, dist_m ); @@ -129,7 +115,7 @@ public: // --- ATTENTION HORRIBLE CODE INCOMING. --- - + p.state.hPa += (p.state.pOld.z - p.state.pCur.z) / 100.0f * 0.105f; // //how many floors are changed? and in what direction (given by the sign) // double numFloorChanged = p.state.z_nr_old - p.state.z_nr; @@ -167,7 +153,7 @@ public: // if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING) // p.state.hPa += numFloorChanged * distBaro.draw(); // else -// p.state.hPa = numFloorChanged * distBaro.draw(); + // p.state.hPa = numFloorChanged * distBaro.draw(); // } // // sanity check diff --git a/code/particles/P3.h b/code/particles/P3.h deleted file mode 100755 index cd46a23..0000000 --- a/code/particles/P3.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef P3_H -#define P3_H - -struct P3 { - - double x; - double y; - double z; - - P3() : x(0), y(0), z(0) {;} - P3(const double x, const double y, const double z) : x(x), y(y), z(z) {;} - - P3 operator - (const P3& o) const { - return P3(x-o.x, y-o.y, z-o.z); - } - - P3 operator + (const P3& o) const { - return P3(x+o.x, y+o.y, z+o.z); - } - - P3 operator * (const double v) const { - return P3(x*v, y*v, z*v); - } - - double getLength(const double floorHeight_cm) const { - return std::sqrt(x*x + y*y + z*floorHeight_cm*z*floorHeight_cm); - } - -}; - -#endif // P3_H diff --git a/code/plan.svg b/code/plan.svg index 8ec6368..5cb035b 100755 --- a/code/plan.svg +++ b/code/plan.svg @@ -43,16 +43,16 @@ inkscape:pageopacity="0" inkscape:pageshadow="2" inkscape:window-width="1600" - inkscape:window-height="782" + inkscape:window-height="845" id="namedview3234" showgrid="true" - inkscape:zoom="2.4027176" - inkscape:cx="555.925" - inkscape:cy="388.39069" + inkscape:zoom="0.84948895" + inkscape:cx="272.89941" + inkscape:cy="1411.2257" inkscape:window-x="0" inkscape:window-y="0" inkscape:window-maximized="1" - inkscape:current-layer="layer16" + inkscape:current-layer="layer8" inkscape:object-nodes="true" units="px" showborder="true" @@ -118,7 +118,7 @@ inkscape:groupmode="layer" id="layer1" inkscape:label="roomnames_3" - style="display:none"> + style="display:inline"> + style="display:none"> @@ -4398,7 +4398,7 @@ sodipodi:nodetypes="ccc" /> @@ -4423,9 +4423,10 @@ sodipodi:nodetypes="cc" /> + inkscape:connector-curvature="0" + sodipodi:nodetypes="cc" /> hpa); + const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma, obs->hpa); // //Just for the visualization. i'm a lazy bastard -// g_BarometerObservation = obs->hpa; + //g_BarometerObservation = obs->hpa; -// assert(barometerProbability == barometerProbability); -// assert(state.hPa == state.hPa); -// assert(obs->hpa == obs->hpa); + assert(barometerProbability == barometerProbability); + assert(state.hPa == state.hPa); + assert(obs->hpa == obs->hpa); // //std::cout << barometerProbability << std::endl; -// return pow(2.0, barometerProbability); -// //return barometerProbability; + //return pow(2.0, barometerProbability); + return barometerProbability; } diff --git a/code/toni/BarometerSensorReader.h b/code/toni/BarometerSensorReader.h index ee6ca28..77c2318 100755 --- a/code/toni/BarometerSensorReader.h +++ b/code/toni/BarometerSensorReader.h @@ -5,25 +5,29 @@ #include "../reader/SensorReader.h" #include +#include + //circular_buffer measurementHistory(1000); class BarometerSensorReader{ private: - circular_buffer measurementHistory; +// circular_buffer measurementHistory; + + MovingAVG avg; public: - BarometerSensorReader(){ - if(!USE_STATIC_CIRCULAR_BUFFERING){ - //8.33min - measurementHistory.reserve(10000); - } - else{ - //30 * 500ms = 1,5s - measurementHistory.reserve(30); - } + BarometerSensorReader(): avg(3) { +// if(!USE_STATIC_CIRCULAR_BUFFERING){ +// //8.33min +// measurementHistory.reserve(10000); +// } +// else{ +// //30 * 500ms = 1,5s +// measurementHistory.reserve(30); +// } } BarometerObservation* readBarometer(const SensorEntry& se) { @@ -31,52 +35,63 @@ public: std::string tmp = se.data; BarometerObservation* obs = new BarometerObservation(); - //Read the hPa - double hPa = stod(tmp); + // get the next hPa reading and average it + avg.add(stod(tmp)); + const double hPa = avg.get(); - // load the measurement at current time into the history - double currentMeasurement = hPa - measurementHistory[0]; + // everything realtive to the first measurement + static double first_hPa = 0; + if (avg.getNumUsed() < avg.getSize()) {first_hPa = avg.get();} - if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){ + obs->hpa = hPa - first_hPa; - //smoothing with alpha value - if(measurementHistory.size() > 1){ - double alpha = 0.1; - double lastMeasurement = measurementHistory[measurementHistory.size() - 1]; - currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement); - - obs->hpa = currentMeasurement; - }else{ - obs->hpa = 0; - } - - measurementHistory.push_back(currentMeasurement); - } - else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){ - - currentMeasurement = hPa; - measurementHistory.push_back(currentMeasurement); - - // calculate the relative air pressure by getting the mean of the first and last three entrys of the history - // and subtract them. - if (measurementHistory.size() > 5){ - double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0; - double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0; - - obs->hpa = meanHead - meanTail; - } - else{ - obs->hpa = 0; - } - } - else //no data smoothing - { - measurementHistory.push_back(currentMeasurement); - obs->hpa = currentMeasurement; - } + std::cout << obs->hpa << std::endl; + // done return obs; + + +// if(USE_BAROMETER_SMOOTHING_RC_LOWPASS){ + +// //smoothing with alpha value +// if(measurementHistory.size() > 1){ +// double alpha = 0.1; +// double lastMeasurement = measurementHistory[measurementHistory.size() - 1]; +// currentMeasurement = (alpha * currentMeasurement) + ((1.0 - alpha) * lastMeasurement); + +// obs->hpa = currentMeasurement; +// }else{ +// obs->hpa = 0; +// } + +// measurementHistory.push_back(currentMeasurement); +// } +// else if (USE_BAROMETER_SMOOTHING_HEAD_TAIL){ + +// currentMeasurement = hPa; +// measurementHistory.push_back(currentMeasurement); + +// // calculate the relative air pressure by getting the mean of the first and last three entrys of the history +// // and subtract them. +// if (measurementHistory.size() > 5){ +// double meanTail = (measurementHistory[0] + measurementHistory[1] + measurementHistory[2]) / 3.0; +// double meanHead = (measurementHistory[measurementHistory.size() - 1] + measurementHistory[measurementHistory.size() - 2] + measurementHistory[measurementHistory.size() - 3]) / 3.0; + +// obs->hpa = meanHead - meanTail; +// } +// else{ +// obs->hpa = 0; +// } +// } +// else //no data smoothing +// { +// measurementHistory.push_back(currentMeasurement); +// obs->hpa = currentMeasurement; +// } + +// return obs; + } //TODO