FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

updates the visualisation

Browse Source 
removed obsolte parts
fixed baromter stuff
worked on eval
added ground-truth
This commit is contained in:
FrankE
2016-01-30 19:50:58 +01:00
parent b2ea3145f4
commit cc899d1c46
18 changed files with 388 additions and 190 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
code/Helper.h
View File

@@ -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<int, Point3> 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;
}

138
code/OldGroundTruth.h Normal file
View File

@@ -0,0 +1,138 @@
#ifndef OLDGROUNDTRUTH_H
#define OLDGROUNDTRUTH_H
#include <KLib/geo/Point.h>
#include <unordered_map>
#include <KLib/gfx/svg/SVGLoader.h>
#include <Indoor/geo/Point2.h>
/**
* 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<int, Point2> 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<int, Point2>& 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

2
code/Settings.h
View File

@@ -16,7 +16,7 @@ namespace MiscSettings {
const int timeSteps = 500;
const int numParticles = 1000;
const int numParticles = 2500;
}

46
code/Vis.h
View File

@@ -10,6 +10,8 @@
#include <Indoor/geo/Length.h>
#include <Indoor/floorplan/Floor.h>
#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<Point3>& 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 <typename T> void addState(const GridWalkState<T>& n) {
particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
}

12
code/eval/Eval.h
View File

@@ -9,6 +9,8 @@
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
class Eval : public EvalBase {
@@ -26,15 +28,16 @@ public:
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
//GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
//GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
pf->setTransition( std::unique_ptr<MyTransition>( 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<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( 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<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
}

88
code/eval/EvalBase.h
View File

@@ -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<int> 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<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
std::vector<int> 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<int, Point3>& waypoints, std::vector<int> ids) {
// construct the ground-truth-path by using all contained waypoint ids
std::vector<Point3> 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<Point3> pathEst;
uint64_t lastTransitionTS = 0;
bool firstReading = true;
int64_t start_time = -1;
K::Statistics<double> 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<MyState> p : pf->getParticles()) {vis.addState(p.state.walkState);}
vis.setTimestamp(se.ts);
vis.addGroundTruth(gtw);
vis.addEstPath(pathEst);
vis.setEstAndShould(curEst, curGT);
vis.show();;
}

2
code/eval/GroundTruthWay.h
View File

@@ -16,7 +16,7 @@ public:
}
/** get the ground truth way */
const std::vector<Entry>& getWay() const {return entries;}
const std::vector<InterpolatorEntry>& getWay() const {return entries;}
};

4
code/frank/BeaconEvaluation.h
View File

@@ -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());

2
code/frank/PositionedBeacon.h
View File

@@ -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) {
;
}

24
code/frank/Settings.h
View File

@@ -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);

13
code/frank/WiFiEvaluation.h
View File

@@ -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);

12
code/particles/MyEvaluation.h
View File

@@ -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);

10
code/particles/MyState.h
View File

@@ -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 {

20
code/particles/MyTransition.h
View File

@@ -48,7 +48,7 @@ public:
*/
MyTransition(Grid<MyGridNode>& grid, GridWalk<MyGridNode>& 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

31
code/particles/P3.h
View File

@@ -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

25
code/plan.svg
View File

@@ -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">
<text
xml:space="preserve"
style="font-style:normal;font-weight:normal;font-size:16px;line-height:125%;font-family:Times;-inkscape-font-specification:Times;text-align:center;letter-spacing:0px;word-spacing:0px;text-anchor:middle;fill:#000000;fill-opacity:1;stroke:none"
@@ -1680,7 +1680,7 @@
inkscape:groupmode="layer"
id="layer16"
inkscape:label="floor_0"
style="display:inline">
style="display:none">
<path
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 386.22047,67.322799 0,42.519681"
@@ -3683,7 +3683,7 @@
inkscape:groupmode="layer"
id="layer8"
inkscape:label="floor_3"
style="display:none"
style="display:inline"
transform="translate(0,4.2364502e-5)">
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
@@ -4386,7 +4386,7 @@
sodipodi:nodetypes="cc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,336.61418 0,67.32283 389.76378,0 0,-67.32283 -389.76378,0"
d="m 442.91339,340.1574 0,60.23622 389.76378,0 0,-60.23622 -389.76378,0"
id="path132-3"
inkscape:connector-curvature="0"
sodipodi:nodetypes="ccccc" />
@@ -4398,7 +4398,7 @@
sodipodi:nodetypes="ccc" />
<path
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 442.91339,457.08654 0,279.92129"
d="m 446.45669,460.62984 0,276.37796"
id="path138-5"
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
@@ -4423,9 +4423,10 @@
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 506.69291,460.62985 -67.32283,0"
d="m 506.69291,460.62985 -60.23622,-10e-6"
id="path4163"
inkscape:connector-curvature="0" />
inkscape:connector-curvature="0"
sodipodi:nodetypes="cc" />
<path
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 467.71654,198.42512 0,-63.77953"
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 224 KiB

After

Width:  |  Height:  |  Size: 224 KiB

20
code/toni/BarometerEvaluation.h
View File

@@ -13,10 +13,10 @@ public:
double getProbability(const MyState& state, const BarometerObservation* obs) const {
return 1;
//return 1;
// //rho_z
// double barometerSigma = 0.3;
double barometerSigma = 0.09;
// //The height of the single floor levels.
// const static double floor_height[3] = {4.1, 3.4, 3.4};
@@ -39,23 +39,23 @@ public:
// }
// else {
// // constant value for sigma if we assume all floors are same in height
// barometerSigma = 0.30 / 1.0; //hPa
// barometerSigma = 0.30 / 1.0; //hPa
// }
// // evaluate the current particle with a normal distribution
// const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->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;
}

115
code/toni/BarometerSensorReader.h
View File

@@ -5,25 +5,29 @@
#include "../reader/SensorReader.h"
#include <sstream>
#include <Indoor/math/MovingAVG.h>
//circular_buffer<double> measurementHistory(1000);
class BarometerSensorReader{
private:
circular_buffer<double> measurementHistory;
// circular_buffer<double> measurementHistory;
MovingAVG<float> 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