Merge branch 'master' of https://git.frank-ebner.de/FHWS/Fusion2016
This commit is contained in:
Toni
@@ -65,7 +65,6 @@ ADD_DEFINITIONS(
|
||||
|
||||
-g
|
||||
-O0
|
||||
|
||||
-DWITH_TESTS
|
||||
-DWITH_ASSERTIONS
|
||||
|
||||
|
||||
29
code/DijkstraMapper.h
Normal file
29
code/DijkstraMapper.h
Normal file
@@ -0,0 +1,29 @@
|
||||
#ifndef DIJKSTRAMAPPER_H
|
||||
#define DIJKSTRAMAPPER_H
|
||||
|
||||
#include "MyGridNode.h"
|
||||
|
||||
/**
|
||||
* allows dijkstra calculation on top of our data-structure
|
||||
*/
|
||||
class DijkstraMapper {
|
||||
|
||||
Grid<MyGridNode>& grid;
|
||||
|
||||
public:
|
||||
|
||||
DijkstraMapper(Grid<MyGridNode>& grid) : grid(grid) {;}
|
||||
|
||||
int getNumNeighbors(const MyGridNode& node) const {return node.getNumNeighbors();}
|
||||
|
||||
const MyGridNode* getNeighbor(const MyGridNode& node, const int idx) const {return &grid.getNeighbor(node, idx);}
|
||||
|
||||
float getWeightBetween(const MyGridNode& n1, const MyGridNode& n2) const {
|
||||
float d = ((Point3)n1 - (Point3)n2).length(2.0);
|
||||
//if (d > 20) {d*= 1.30;}
|
||||
return d / std::pow(n2.imp, 3);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // DIJKSTRAMAPPER_H
|
||||
161
code/Helper.h
Normal file
161
code/Helper.h
Normal file
@@ -0,0 +1,161 @@
|
||||
#ifndef HELPER_H
|
||||
#define HELPER_H
|
||||
|
||||
#include <Indoor/grid/Grid.h>
|
||||
#include <Indoor/grid/factory/GridFactory.h>
|
||||
#include <Indoor/grid/factory/GridImportance.h>
|
||||
|
||||
#include <Indoor/floorplan/FloorplanFactorySVG.h>
|
||||
|
||||
#include "Settings.h"
|
||||
#include "MyGridNode.h"
|
||||
#include "OldGroundTruth.h"
|
||||
|
||||
class Helper {
|
||||
|
||||
private:
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/** convert height (in cm) to floor-numbers */
|
||||
static int getFloorNr(float z_cm) {
|
||||
// if (z_cm < 360) {return 0;}
|
||||
// if (z_cm < 360+340) {return 1;}
|
||||
// if (z_cm < 360+340+340) {return 2;}
|
||||
// return 3;
|
||||
if (z_cm < 180) {return 0;}
|
||||
if (z_cm < 360+180) {return 1;}
|
||||
if (z_cm < 360+340+180) {return 2;}
|
||||
return 3;
|
||||
}
|
||||
|
||||
/** convert height (in cm) to floor-numbers */
|
||||
static int getFloorNrFloat(float z_cm) {
|
||||
return z_cm / 340.0f;
|
||||
}
|
||||
|
||||
static int getHeight(const int floorNr) {
|
||||
switch(floorNr) {
|
||||
case 0: return 0;
|
||||
case 1: return 360;
|
||||
case 2: return 360+340;
|
||||
case 3: return 360+340+340;
|
||||
default: throw "error";
|
||||
}
|
||||
}
|
||||
|
||||
/** align the given value onto the grid */
|
||||
static int align(const int val) {
|
||||
return val / MiscSettings::gridSize_cm * MiscSettings::gridSize_cm;
|
||||
}
|
||||
|
||||
/** all floors within the building */
|
||||
struct FHWSFloors {
|
||||
|
||||
Floor f0, f1, f2, f3;
|
||||
|
||||
Stairs s01, s12, s23;
|
||||
|
||||
const LengthF h0 = LengthF::cm(align(getHeight(0)));
|
||||
const LengthF h1 = LengthF::cm(align(getHeight(1)));
|
||||
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() {;}
|
||||
|
||||
};
|
||||
|
||||
/** load the entire floorplan */
|
||||
static FHWSFloors getFloors() {
|
||||
|
||||
FloorplanFactorySVG fpFac(MiscSettings::floorplan, 2.822222);
|
||||
FHWSFloors f;
|
||||
|
||||
f.f0 = fpFac.getFloor("floor_0");
|
||||
f.f1 = fpFac.getFloor("floor_1");
|
||||
f.f2 = fpFac.getFloor("floor_2");
|
||||
f.f3 = fpFac.getFloor("floor_3");
|
||||
|
||||
f.s01 = fpFac.getStairs("staircase_0_1");
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
template <typename T> static void buildTheGrid(Grid<T>& grid, FHWSFloors floors) {
|
||||
|
||||
GridFactory<MyGridNode> gridFac(grid);
|
||||
|
||||
gridFac.addFloor(floors.f0, floors.h0.cm());
|
||||
gridFac.addFloor(floors.f1, floors.h1.cm());
|
||||
gridFac.addFloor(floors.f2, floors.h2.cm());
|
||||
gridFac.addFloor(floors.f3, floors.h3.cm());
|
||||
|
||||
gridFac.addStairs(floors.s01, floors.h0.cm(), floors.h1.cm());
|
||||
gridFac.addStairs(floors.s12, floors.h1.cm(), floors.h2.cm());
|
||||
gridFac.addStairs(floors.s23, floors.h2.cm(), floors.h3.cm());
|
||||
|
||||
// maybe the two sides are wrong?
|
||||
PlatformStair psUpperLeft;
|
||||
psUpperLeft.platform = BBox2(Point2(1560, 4778), Point2(1730, 5128));
|
||||
psUpperLeft.s1 = Stair(Line2( 1278,4790+000, 1278,4790+140 ), Point2(+280,0));
|
||||
psUpperLeft.s2 = Stair(Line2( 1278,4790+160, 1278,4790+160+140 ), Point2(+280,0));
|
||||
gridFac.buildPlatformStair(psUpperLeft, floors.h0.cm(), floors.h1.cm());
|
||||
gridFac.buildPlatformStair(psUpperLeft, floors.h1.cm(), floors.h2.cm());
|
||||
gridFac.buildPlatformStair(psUpperLeft, floors.h2.cm(), floors.h3.cm());
|
||||
// vis.gp << "set xrange [1100:1800]\n";
|
||||
// vis.gp << "set yrange [4500:5200]\n";
|
||||
|
||||
PlatformStair psUpperRight;
|
||||
psUpperRight.platform = BBox2(Point2(6290, 4778), Point2(6500, 5098));
|
||||
psUpperRight.s1 = Stair(Line2( 6758,4790+160, 6758,4790+160+140 ), Point2(-280,0));
|
||||
psUpperRight.s2 = Stair(Line2( 6758,4790+000, 6758,4790+140 ), Point2(-280,0));
|
||||
gridFac.buildPlatformStair(psUpperRight, floors.h0.cm(), floors.h1.cm());
|
||||
gridFac.buildPlatformStair(psUpperRight, floors.h1.cm(), floors.h2.cm());
|
||||
gridFac.buildPlatformStair(psUpperRight, floors.h2.cm(), floors.h3.cm());
|
||||
// vis.gp << "set xrange [6100:6900]\n";
|
||||
// vis.gp << "set yrange [4500:5200]\n";
|
||||
|
||||
PlatformStair psLowerLeft;
|
||||
psLowerLeft.platform = BBox2(Point2(1510, 658), Point2(1820, 900));
|
||||
psLowerLeft.s1 = Stair(Line2( 1510+000,1148, 1510+140,1148 ), Point2(0,-280));
|
||||
psLowerLeft.s2 = Stair(Line2( 1510+170,1148, 1510+300,1148 ), Point2(0,-280));
|
||||
gridFac.buildPlatformStair(psLowerLeft, floors.h0.cm(), floors.h1.cm());
|
||||
gridFac.buildPlatformStair(psLowerLeft, floors.h1.cm(), floors.h2.cm());
|
||||
gridFac.buildPlatformStair(psLowerLeft, floors.h2.cm(), floors.h3.cm());
|
||||
// vis.gp << "set xrange [1300:2100]\n";
|
||||
// vis.gp << "set yrange [400:1400]\n";
|
||||
|
||||
// remove all isolated nodes not attached to 300,300,floor0
|
||||
gridFac.removeIsolated( (MyGridNode&)grid.getNodeFor(GridPoint(300,300,floors.h0.cm())) );
|
||||
|
||||
// stamp importance information onto the grid-nodes
|
||||
GridImportance gridImp;
|
||||
gridImp.addImportance(grid, floors.h0.cm());
|
||||
gridImp.addImportance(grid, floors.h1.cm());
|
||||
gridImp.addImportance(grid, floors.h2.cm());
|
||||
gridImp.addImportance(grid, floors.h3.cm());
|
||||
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // HELPER_H
|
||||
26
code/MyGridNode.h
Normal file
26
code/MyGridNode.h
Normal file
@@ -0,0 +1,26 @@
|
||||
#ifndef MYGRIDNODE_H
|
||||
#define MYGRIDNODE_H
|
||||
|
||||
#include <Indoor/grid/GridNode.h>
|
||||
#include <Indoor/grid/GridPoint.h>
|
||||
|
||||
/**
|
||||
* the nodes we add to our grid
|
||||
*/
|
||||
struct MyGridNode : public GridNode, public GridPoint {
|
||||
|
||||
/** distance to the desired target */
|
||||
float distToTarget = 1.0;
|
||||
|
||||
/** node importance based on surroundings */
|
||||
float imp = 1.0;
|
||||
|
||||
public:
|
||||
|
||||
/** needed ctor */
|
||||
MyGridNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // MYGRIDNODE_H
|
||||
138
code/OldGroundTruth.h
Normal file
138
code/OldGroundTruth.h
Normal 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
|
||||
26
code/Settings.h
Normal file
26
code/Settings.h
Normal file
@@ -0,0 +1,26 @@
|
||||
#ifndef OTHER_SETTINGS_H
|
||||
#define OTHER_SETTINGS_H
|
||||
|
||||
#define USE_STATIC_CIRCULAR_BUFFERING false
|
||||
#define USE_BAROMETER_SMOOTHING_RC_LOWPASS false
|
||||
#define USE_BAROMETER_SMOOTHING_HEAD_TAIL false
|
||||
#define USE_BAROMETRIC_FORMULAR false
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace MiscSettings {
|
||||
|
||||
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
|
||||
|
||||
const int gridSize_cm = 40;
|
||||
|
||||
const int timeSteps = 500;
|
||||
|
||||
const int numParticles = 2500;
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif // OTHER_SETTINGS_H
|
||||
|
||||
|
||||
100
code/Vis.h
100
code/Vis.h
@@ -5,10 +5,13 @@
|
||||
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
|
||||
|
||||
#include <Indoor/geo/Length.h>
|
||||
#include <Indoor/floorplan/Floor.h>
|
||||
|
||||
#include "eval/GroundTruthWay.h"
|
||||
|
||||
class Vis {
|
||||
|
||||
public:
|
||||
@@ -16,21 +19,38 @@ public:
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotSplot splot;
|
||||
K::GnuplotSplotElementLines floors;
|
||||
K::GnuplotSplotElementPoints gridNodes;
|
||||
K::GnuplotSplotElementColorPoints gridNodes;
|
||||
K::GnuplotSplotElementLines gridEdges;
|
||||
K::GnuplotSplotElementPoints particles;
|
||||
K::GnuplotSplotElementLines groundTruth;
|
||||
K::GnuplotSplotElementLines estPath;
|
||||
|
||||
public:
|
||||
|
||||
Vis() {
|
||||
|
||||
gp << "set hidden3d front\n";
|
||||
gp << "set view equal xy\n";
|
||||
//gp << "set view equal xy\n";
|
||||
gp << "set ticslevel 0\n";
|
||||
gp << "set cbrange[0.8:2.0]\n";
|
||||
|
||||
gp << "unset xtics\n";
|
||||
gp << "unset ytics\n";
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
@@ -50,13 +70,79 @@ public:
|
||||
|
||||
/** add the grid to the plot */
|
||||
template <typename T> Vis& addGrid(Grid<T>& grid) {
|
||||
|
||||
float max = 0;
|
||||
for (const T& n1 : grid) {
|
||||
if (n1.distToTarget > max) {max = n1.distToTarget;}
|
||||
}
|
||||
gp << "set cbrange[0.0:1.0]\n";
|
||||
//gp << "set cbrange[0.8:1.3]\n";
|
||||
|
||||
for (const T& n1 : grid) {
|
||||
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
|
||||
gridNodes.add(p1);
|
||||
for (const T& n2 : grid.neighbors(n1)) {
|
||||
const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
|
||||
gridEdges.addSegment(p1, p2);
|
||||
}
|
||||
//const float color = n1.imp;
|
||||
//const float color = n1.distToTarget/max;
|
||||
const float color = 0;
|
||||
gridNodes.add(p1, color);
|
||||
// for (const T& n2 : grid.neighbors(n1)) {
|
||||
// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
|
||||
// gridEdges.addSegment(p1, p2);
|
||||
// }
|
||||
}
|
||||
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();;
|
||||
}
|
||||
|
||||
void clearStates() {
|
||||
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));
|
||||
}
|
||||
|
||||
template <typename T> Vis& showStates(std::vector<GridWalkState<T>>& states) {
|
||||
particles.clear();;
|
||||
for (const GridWalkState<T>& n : states) {
|
||||
particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
70
code/eval/Eval.h
Normal file
70
code/eval/Eval.h
Normal file
@@ -0,0 +1,70 @@
|
||||
#ifndef EVAL_H
|
||||
#define EVAL_H
|
||||
|
||||
#include "EvalBase.h"
|
||||
#include "../DijkstraMapper.h"
|
||||
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
|
||||
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
|
||||
#include <Indoor/grid/walk/GridWalkPushForward.h>
|
||||
#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 {
|
||||
|
||||
public:
|
||||
|
||||
|
||||
|
||||
Eval() {
|
||||
|
||||
|
||||
pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );
|
||||
|
||||
MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
|
||||
MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
|
||||
|
||||
//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>();
|
||||
|
||||
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, floors.gtwp, way2);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void setEval1() {
|
||||
|
||||
|
||||
runName = "TODO";
|
||||
|
||||
// the particle filter's evaluation method
|
||||
std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
|
||||
eval.get()->setUsage(true, false, false, true, true);
|
||||
pf->setEvaluation( std::move(eval) );
|
||||
|
||||
// resampling step?
|
||||
pf->setNEffThreshold(1.0);
|
||||
pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
|
||||
|
||||
// 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>()));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // EVAL_H
|
||||
284
code/eval/EvalBase.h
Normal file
284
code/eval/EvalBase.h
Normal file
@@ -0,0 +1,284 @@
|
||||
#ifndef EVALBASE_H
|
||||
#define EVALBASE_H
|
||||
|
||||
#include "../Settings.h"
|
||||
#include "../Helper.h"
|
||||
#include "../Vis.h"
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilter.h>
|
||||
#include <KLib/math/statistics/Statistics.h>
|
||||
|
||||
#include "GroundTruthWay.h"
|
||||
|
||||
#include "../particles/MyState.h"
|
||||
#include "../particles/MyObservation.h"
|
||||
#include "../particles/MyEvaluation.h"
|
||||
#include "../particles/MyTransition.h"
|
||||
#include "../particles/MyInitializer.h"
|
||||
|
||||
#include "../reader/SensorReader.h"
|
||||
#include "../reader/SensorReaderStep.h"
|
||||
#include "../reader/SensorReaderTurn.h"
|
||||
|
||||
#include "../lukas/TurnObservation.h"
|
||||
#include "../lukas/StepObservation.h"
|
||||
|
||||
#include "../toni/BarometerSensorReader.h"
|
||||
|
||||
#include "../frank/WiFiSensorReader.h"
|
||||
#include "../frank/BeaconSensorReader.h"
|
||||
|
||||
class EvalBase {
|
||||
|
||||
protected:
|
||||
|
||||
Grid<MyGridNode> grid;
|
||||
Helper::FHWSFloors floors;
|
||||
Vis vis;
|
||||
|
||||
K::ParticleFilter<MyState, MyControl, MyObservation>* pf;
|
||||
|
||||
SensorReader* sr;
|
||||
SensorReaderTurn* srt;
|
||||
SensorReaderStep* srs;
|
||||
|
||||
|
||||
|
||||
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()) {
|
||||
|
||||
// build the grid
|
||||
Helper::buildTheGrid(grid, floors);
|
||||
|
||||
// setup the visualisation
|
||||
vis.addFloor(floors.f0, floors.h0);
|
||||
vis.addFloor(floors.f1, floors.h1);
|
||||
vis.addFloor(floors.f2, floors.h2);
|
||||
vis.addFloor(floors.f3, floors.h3);
|
||||
|
||||
}
|
||||
|
||||
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() {
|
||||
|
||||
// sensor numbers
|
||||
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5;
|
||||
const int s_linearAcceleration = 2;
|
||||
|
||||
std::list<TurnObservation> turn_observations;
|
||||
std::list<StepObservation> step_observations;
|
||||
|
||||
//Create an BarometerSensorReader
|
||||
BarometerSensorReader baroSensorReader;
|
||||
|
||||
|
||||
//Read all turn Observations
|
||||
while(srt->hasNext()) {
|
||||
|
||||
SensorEntryTurn set = srt->getNext();
|
||||
TurnObservation to;
|
||||
|
||||
to.ts = set.ts;
|
||||
to.delta_heading = set.delta_heading;
|
||||
to.delta_motion = set.delta_motion;
|
||||
|
||||
turn_observations.push_back(to);
|
||||
}
|
||||
|
||||
|
||||
//Step Observations
|
||||
while(srs->hasNext()) {
|
||||
|
||||
SensorEntryStep ses = srs->getNext();
|
||||
StepObservation so;
|
||||
|
||||
so.ts = ses.ts;
|
||||
|
||||
step_observations.push_back(so);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// the to-be-evaluated observation
|
||||
MyObservation obs;
|
||||
|
||||
|
||||
std::vector<Point3> pathEst;
|
||||
|
||||
uint64_t lastTransitionTS = 0;
|
||||
int64_t start_time = -1;
|
||||
|
||||
K::Statistics<double> stats;
|
||||
int cnt = 0;
|
||||
|
||||
// process each sensor reading
|
||||
while(sr->hasNext()) {
|
||||
|
||||
// get the next sensor reading from the CSV
|
||||
const SensorEntry se = sr->getNext();
|
||||
|
||||
//start_time needed for time calculation of steps and turns
|
||||
obs.latestSensorDataTS = se.ts;
|
||||
if (start_time == -1) {start_time = se.ts;}
|
||||
int64_t current_time = se.ts - start_time;
|
||||
|
||||
switch(se.idx) {
|
||||
|
||||
case s_wifi: {
|
||||
obs.wifi = WiFiSensorReader::readWifi(se);
|
||||
break;
|
||||
}
|
||||
|
||||
case s_beacons: {
|
||||
BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se);
|
||||
if (!boe.mac.empty()) {
|
||||
obs.beacons.entries.push_back(boe);
|
||||
} // add the observed beacon
|
||||
obs.beacons.removeOld(obs.latestSensorDataTS);
|
||||
break;
|
||||
}
|
||||
|
||||
case s_barometer: {
|
||||
obs.barometer = baroSensorReader.readBarometer(se);
|
||||
break;
|
||||
}
|
||||
|
||||
case s_linearAcceleration:{
|
||||
baroSensorReader.readVerticalAcceleration(se);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// scheduled transition every 500 ms
|
||||
if (lastTransitionTS == 0) {lastTransitionTS = se.ts;}
|
||||
for ( ; se.ts - lastTransitionTS > MiscSettings::timeSteps; lastTransitionTS += MiscSettings::timeSteps) {
|
||||
|
||||
//Steps are sorted in the list by timestamp.
|
||||
//If the current observation timestamp is bigger/equal
|
||||
//to the current step timestamp, use this step as observation
|
||||
//and remove it from the list.
|
||||
//The new first timestamp in the list will be then be the next one (timestamp-wise)
|
||||
StepObservation so;
|
||||
if(current_time >= step_observations.front().ts && !step_observations.empty()) {
|
||||
so.step = true;
|
||||
so.ts = current_time;
|
||||
|
||||
obs.step = &so;
|
||||
step_observations.pop_front();
|
||||
|
||||
}
|
||||
else {
|
||||
so.step = false;
|
||||
so.ts = current_time;
|
||||
|
||||
obs.step = &so;
|
||||
}
|
||||
|
||||
TurnObservation to;
|
||||
//same principal as for steps is applied for turns
|
||||
if(current_time >= turn_observations.front().ts && !turn_observations.empty()) {
|
||||
to = turn_observations.front();
|
||||
obs.turn = &to;
|
||||
|
||||
turn_observations.pop_front();
|
||||
}
|
||||
else {
|
||||
to.delta_heading = 0.0;
|
||||
to.delta_motion = 0.0;
|
||||
|
||||
obs.turn = &to;
|
||||
}
|
||||
|
||||
|
||||
// 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;
|
||||
|
||||
// error calculation. compare ground-truth to estimation
|
||||
const Point3 curGT = gtw.getPosAtTime(se.ts - 750);
|
||||
const Point3 diff = curEst - curGT;
|
||||
|
||||
// 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();;
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
{
|
||||
// vis.setShowParticles(false);
|
||||
// vis.setShowTime(false);
|
||||
// vis.setShowCurPos(false);
|
||||
// vis.debugProcess(0, pathEst, gtw, pf, layers);
|
||||
// std::ofstream out("/tmp/" + runName + ".data");
|
||||
// out << vis.getDataset();
|
||||
// out.close();
|
||||
}
|
||||
|
||||
sleep(1000);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // EVALBASE_H
|
||||
24
code/eval/GroundTruthWay.h
Normal file
24
code/eval/GroundTruthWay.h
Normal file
@@ -0,0 +1,24 @@
|
||||
#ifndef GROUNDTRUTHWAY_H
|
||||
#define GROUNDTRUTHWAY_H
|
||||
|
||||
#include <Indoor/math/Interpolator.h>
|
||||
#include <Indoor/geo/Point3.h>
|
||||
|
||||
/**
|
||||
* interpolated ground-trouth based on timed check-points
|
||||
*/
|
||||
class GroundTruthWay : public Interpolator<uint64_t, Point3> {
|
||||
|
||||
public:
|
||||
|
||||
Point3 getPosAtTime(const uint64_t ts) const {
|
||||
return get(ts);
|
||||
}
|
||||
|
||||
/** get the ground truth way */
|
||||
const std::vector<InterpolatorEntry>& getWay() const {return entries;}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // GROUNDTRUTHWAY_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);
|
||||
@@ -39,10 +39,12 @@ public:
|
||||
if (!beacon) {continue;}
|
||||
|
||||
// distance (in meter) between particle and AP
|
||||
const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0;
|
||||
//const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0;
|
||||
const double distToBeacon_m = state.pCur.getDistance(*beacon) / 100.0;
|
||||
|
||||
// floor difference?
|
||||
const double floorDist = std::abs(beacon->zNr - state.z_nr);
|
||||
//const double floorDist = std::abs(beacon->zNr - state.getFloorNr());
|
||||
const float floorDist = std::round(std::abs(Helper::getFloorNrFloat(beacon->z) - Helper::getFloorNrFloat(state.pCur.z)));
|
||||
|
||||
// estimate the rssi depending on above distance
|
||||
const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * waf);
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#ifndef BEACONSENSORREADER_H
|
||||
#define BEACONSENSORREADER_H
|
||||
|
||||
#include "../SensorReader.h"
|
||||
#include "../reader/SensorReader.h"
|
||||
#include "BeaconObservation.h"
|
||||
#include "Settings.h"
|
||||
#include <cassert>
|
||||
|
||||
@@ -2,19 +2,26 @@
|
||||
#define POSITIONEDBEACON_H
|
||||
|
||||
#include "WiFiAP.h"
|
||||
#include "Position3D.h"
|
||||
//#include "Position3D.h"
|
||||
#include <Indoor/geo/Point3.h>
|
||||
|
||||
class PositionedBeacon : public Position3D {
|
||||
class PositionedBeacon : public Point3 {
|
||||
|
||||
public:
|
||||
|
||||
MACAddress mac;
|
||||
double tx;
|
||||
double pl;
|
||||
float tx;
|
||||
float pl;
|
||||
|
||||
// /** ctor */
|
||||
// PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) :
|
||||
// mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) {
|
||||
// ;
|
||||
// }
|
||||
|
||||
/** ctor */
|
||||
PositionedBeacon(const MACAddress& mac, const double tx, const double pl, const double xM, const double yM, const int zNr) :
|
||||
mac(mac), tx(tx), pl(pl), Position3D(xM, yM, zNr) {
|
||||
PositionedBeacon(const MACAddress& mac, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
|
||||
Point3(x_cm, y_cm, z_cm), mac(mac), tx(tx), pl(pl) {
|
||||
;
|
||||
}
|
||||
|
||||
|
||||
@@ -2,16 +2,22 @@
|
||||
#define POSITIONEDWIFIAP_H
|
||||
|
||||
#include "WiFiAP.h"
|
||||
#include "Position3D.h"
|
||||
//#include "Position3D.h"
|
||||
#include <Indoor/geo/Point3.h>
|
||||
|
||||
|
||||
class PositionedWifiAP : public WiFiAP, public Position3D {
|
||||
class PositionedWifiAP : public WiFiAP, public Point3 {
|
||||
|
||||
public:
|
||||
|
||||
// /** ctor */
|
||||
// PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) :
|
||||
// WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) {
|
||||
// ;
|
||||
// }
|
||||
|
||||
/** ctor */
|
||||
PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const double tx, const double pl, const double xM, const double yM, const int zNr) :
|
||||
WiFiAP(mac, ssid, tx, pl), Position3D(xM, yM, zNr) {
|
||||
PositionedWifiAP(const MACAddress& mac, const std::string& ssid, const float tx, const float pl, const float x_cm, const float y_cm, const float z_cm) :
|
||||
WiFiAP(mac, ssid, tx, pl), Point3(x_cm, y_cm, z_cm) {
|
||||
;
|
||||
}
|
||||
|
||||
|
||||
@@ -7,6 +7,8 @@
|
||||
|
||||
#include <unordered_map>
|
||||
|
||||
#include "../Helper.h"
|
||||
|
||||
class Settings {
|
||||
|
||||
private:
|
||||
@@ -18,52 +20,52 @@ 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, 3, tx, pl-0.5);
|
||||
addAP(("00:04:96:6b:70:c9"), "i.3.25", 290, 3930, 3, tx, pl-0.5);
|
||||
addAP(("00:04:96:6b:82:79"), "i.3.16", 1860, 3400, 3, tx, pl-0.5);
|
||||
addAP(("00:04:96:77:ed:f9"), "i.3.39", 4700, 4850, 3, tx, pl);
|
||||
addAP(("00:04:96:77:ed:69"), "i.3.3", 6460, 3400, 3, tx, pl);
|
||||
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, 2, tx, pl-0.5);
|
||||
addAP(("00:04:96:6b:bf:f9"), "I.2.9", 3000, 3350, 2, tx, pl);
|
||||
addAP(("00:04:96:77:ec:a9"), "I.2.15", 290, 750, 2, tx, pl);
|
||||
addAP(("00:04:96:6b:0c:c9"), "I.2.19", 300, 4000, 2, tx, pl-0.5);
|
||||
addAP(("00:04:96:6b:db:69"), "I.2.34", 4320, 4780, 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);
|
||||
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, 1, tx, pl);
|
||||
addAP(("00:04:96:7d:07:79"), "I.1.9", 1800, 3300, 1, tx, pl);
|
||||
addAP(("00:04:96:69:48:c9"), "I.1.17", 1500, 300, 1, tx, pl-0.25);
|
||||
addAP(("00:04:96:77:eb:99"), "I.1.21", 500, 1700, 1, tx, pl-0.25);
|
||||
addAP(("00:04:96:6b:45:59"), "I.1.30", 800, 4800, 1, tx, pl);
|
||||
addAP(("00:04:96:77:ed:89"), "I.1.43", 4600, 4800, 1, tx, pl);
|
||||
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);
|
||||
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);
|
||||
|
||||
// 0th floor (exact AP position)
|
||||
addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530, 4970, 0, tx, pl);
|
||||
addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, 0, tx, pl);
|
||||
addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, 0, tx, pl);
|
||||
addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, 0, tx, pl);
|
||||
addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, 0, tx, pl);
|
||||
addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, 0, tx, pl); // vague!!
|
||||
addAP(("00:04:96:6C:6E:F9"), "I.0.27", 530, 4970, Helper::getHeight(0), tx, pl);
|
||||
addAP(("00:04:96:6C:A5:39"), "I.0.17", 1030, 270, Helper::getHeight(0), tx, pl);
|
||||
addAP(("00:04:96:6C:A4:A9"), "I.0.9", 1660, 2780, Helper::getHeight(0), tx, pl);
|
||||
addAP(("00:04:96:77:EE:69"), "I.0.7", 3560, 3380, Helper::getHeight(0), tx, pl);
|
||||
addAP(("00:04:96:6B:46:09"), "I.0.xx", 6860, 3690, Helper::getHeight(0), tx, pl);
|
||||
addAP(("00:04:96:6C:5E:39"), "I.0.36", 4480, 4800, Helper::getHeight(0), tx, pl); // vague!!
|
||||
|
||||
const int ibOff = +2;
|
||||
const float ibPLE = 1.9;
|
||||
addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, 3); // id:16
|
||||
addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, 2); // id:18
|
||||
addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, 1); // id:11
|
||||
addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, 0); // id:20
|
||||
addBeacon("78:A5:04:1F:87:64", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(3)); // id:16
|
||||
addBeacon("78:A5:04:1F:8A:59", -65+4, 2.0, 1088, 4858, Helper::getHeight(2)); // id:18
|
||||
addBeacon("1C:BA:8C:21:71:70", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(1)); // id:11
|
||||
addBeacon("78:A5:04:1F:88:9F", -71+ibOff, ibPLE, 1088, 4858, Helper::getHeight(0)); // id:20
|
||||
|
||||
addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, 2); // idis switchboard
|
||||
addBeacon("E5:6F:57:34:94:40", -77+ibOff, ibPLE, 7468, 5108, 2); // idis outside
|
||||
addBeacon("C6:FC:6E:25:F5:29", -77+ibOff, ibPLE, 6115, 4527, 2); // idis toni
|
||||
addBeacon("F9:CC:C0:A2:02:17", -77+ibOff, ibPLE, 7068, 4518, Helper::getHeight(2)); // idis switchboard
|
||||
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, 1); // i.1.47
|
||||
addBeacon("78:A5:04:1F:91:41", -88+ibOff-4, ibPLE, 6508, 4038, 1); // fachschaft
|
||||
addBeacon("78:A5:04:1F:8E:35", -88+ibOff-4, ibPLE, 6313, 4038, 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);
|
||||
|
||||
@@ -37,14 +37,13 @@ public:
|
||||
|
||||
//const double tx = -48; // tablet
|
||||
//const double pl = 3.15;
|
||||
const double waf = 7;//10.0;
|
||||
const double floor_height_cm = 350;
|
||||
const float waf = 7;//10.0; // was 7 before?! has something todo with the floor heights / levels
|
||||
|
||||
// get the ap the client had the strongest measurement for
|
||||
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);
|
||||
//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);
|
||||
|
||||
// process each detected AP
|
||||
for (const WiFiObservationEntry& entry : obs.entries) {
|
||||
@@ -53,20 +52,23 @@ 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(*ap) / 100.0;
|
||||
|
||||
// floor difference?
|
||||
const double floorDist = std::abs(ap->zNr - state.z_nr);
|
||||
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) - (floorDist * waf);
|
||||
const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf);
|
||||
|
||||
// the measured rssi
|
||||
const double realRSSI = entry.rssi;
|
||||
|
||||
// the measured relative rssi
|
||||
const double realRelRSSI = strongest.rssi - realRSSI;
|
||||
const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
|
||||
//const double realRelRSSI = strongest.rssi - realRSSI;
|
||||
//const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;
|
||||
|
||||
// probability? (sigma grows with measurement's age)
|
||||
const double sigma = 8 + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.0;
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#ifndef WIFISENSORREADER_H
|
||||
#define WIFISENSORREADER_H
|
||||
|
||||
#include "../SensorReader.h"
|
||||
#include "../reader/SensorReader.h"
|
||||
#include "WiFiObservation.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
@@ -8,10 +8,10 @@
|
||||
#include "StepObservation.h"
|
||||
#include <math.h>
|
||||
|
||||
static double mu_walk = 40;
|
||||
static double sigma_walk = 15;
|
||||
static double mu_stop = 0;
|
||||
static double sigma_stop = 5;
|
||||
static constexpr double mu_walk = 40;
|
||||
static constexpr double sigma_walk = 15;
|
||||
static constexpr double mu_stop = 0;
|
||||
static constexpr double sigma_stop = 5;
|
||||
|
||||
class StepEvaluation {
|
||||
|
||||
@@ -19,29 +19,31 @@ public:
|
||||
|
||||
double getProbability(const MyState& state, const StepObservation* obs) const {
|
||||
|
||||
double distance = state.distanceWalkedCM;
|
||||
return 1;
|
||||
|
||||
double a = 1.0;
|
||||
double mu_distance = 0; //cm
|
||||
double sigma_distance = 10.0; //cm
|
||||
// double distance = state.distanceWalkedCM;
|
||||
|
||||
if(obs->step) {
|
||||
a = 1.0;
|
||||
mu_distance = mu_walk;//80.0; //cm
|
||||
sigma_distance = sigma_walk;//40.0; //cm
|
||||
}
|
||||
// double a = 1.0;
|
||||
// double mu_distance = 0; //cm
|
||||
// double sigma_distance = 10.0; //cm
|
||||
|
||||
else {
|
||||
a = 0.0;
|
||||
mu_distance = mu_stop; //cm
|
||||
sigma_distance = sigma_stop; //cm
|
||||
}
|
||||
// if(obs->step) {
|
||||
// a = 1.0;
|
||||
// mu_distance = mu_walk;//80.0; //cm
|
||||
// sigma_distance = sigma_walk;//40.0; //cm
|
||||
// }
|
||||
|
||||
//Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
|
||||
const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
|
||||
(1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
|
||||
// else {
|
||||
// a = 0.0;
|
||||
// mu_distance = mu_stop; //cm
|
||||
// sigma_distance = sigma_stop; //cm
|
||||
// }
|
||||
|
||||
return p;
|
||||
// //Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
|
||||
// const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
|
||||
// (1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
|
||||
|
||||
// return p;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
@@ -4,10 +4,10 @@
|
||||
|
||||
#include "../particles/MyState.h"
|
||||
#include "TurnObservation.h"
|
||||
#include <boost/math/special_functions/bessel.hpp>
|
||||
//#include <boost/math/special_functions/bessel.hpp>
|
||||
#include <math.h>
|
||||
|
||||
static double sigma_heading = 35;
|
||||
static constexpr double sigma_heading = 35;
|
||||
|
||||
class TurnEvaluation {
|
||||
|
||||
@@ -17,55 +17,33 @@ public:
|
||||
|
||||
double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
|
||||
|
||||
//Particle's heading change
|
||||
double delta_heading_particle = state.heading - state.heading_old;
|
||||
return 1;
|
||||
|
||||
// //Particle's heading change
|
||||
// double delta_heading_particle = state.heading - state.heading_old;
|
||||
|
||||
|
||||
//Correct offset of the heading change
|
||||
if (delta_heading_particle < -180) {
|
||||
delta_heading_particle += 360;
|
||||
}
|
||||
else if (delta_heading_particle > 180) {
|
||||
delta_heading_particle -= 360;
|
||||
}
|
||||
// //Correct offset of the heading change
|
||||
// if (delta_heading_particle < -180) {
|
||||
// delta_heading_particle += 360;
|
||||
// }
|
||||
// else if (delta_heading_particle > 180) {
|
||||
// delta_heading_particle -= 360;
|
||||
// }
|
||||
|
||||
|
||||
//Switch between simple and improved evaluation
|
||||
//"Simple" only evaluates the deviation between the measured heading and the particle heading change using
|
||||
//normal distribution
|
||||
if(simple) {
|
||||
// //Switch between simple and improved evaluation
|
||||
// //"Simple" only evaluates the deviation between the measured heading and the particle heading change using
|
||||
// //normal distribution
|
||||
// //if(simple) {
|
||||
|
||||
double sigma_delta_heading = sigma_heading;
|
||||
// double sigma_delta_heading = sigma_heading;
|
||||
|
||||
const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
|
||||
// const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
|
||||
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
//use the von Mises distribution
|
||||
else {
|
||||
//Here some calculations must be done in rad
|
||||
|
||||
double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0;
|
||||
double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0;
|
||||
|
||||
//Equation for estimating kappa value of von Mises distribution
|
||||
//empirically estimated
|
||||
double kappa = 0.0;
|
||||
|
||||
kappa = 5.0 / exp(2 * delta_motion_rad);
|
||||
|
||||
double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0;
|
||||
|
||||
|
||||
|
||||
//pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution)
|
||||
const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa));
|
||||
|
||||
return p;
|
||||
|
||||
}
|
||||
// return p;
|
||||
// // }
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#ifndef TURNREADER_H
|
||||
#define TURNREADER_H
|
||||
|
||||
#include "../SensorReaderTurn.h"
|
||||
#include "../reader/SensorReaderTurn.h"
|
||||
#include "TurnObservation.h"
|
||||
|
||||
class TurnReader {
|
||||
|
||||
117
code/main.cpp
117
code/main.cpp
@@ -1,58 +1,87 @@
|
||||
#include <Indoor/grid/factory/GridFactory.h>
|
||||
#include <Indoor/floorplan/FloorplanFactorySVG.h>
|
||||
|
||||
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>
|
||||
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
|
||||
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
|
||||
#include <Indoor/grid/walk/GridWalkPushForward.h>
|
||||
|
||||
#include "Vis.h"
|
||||
#include "Helper.h"
|
||||
#include "MyGridNode.h"
|
||||
#include "Helper.h"
|
||||
#include "DijkstraMapper.h"
|
||||
|
||||
#include "eval/Eval.h"
|
||||
#include "eval/EvalBase.h"
|
||||
|
||||
Settings settings;
|
||||
|
||||
void testModelWalk() {
|
||||
|
||||
Grid<MyGridNode> grid(MiscSettings::gridSize_cm);
|
||||
|
||||
Helper::FHWSFloors floors = Helper::getFloors();
|
||||
Helper::buildTheGrid(grid, floors);
|
||||
|
||||
MyGridNode& start = (MyGridNode&)grid.getNodeFor(GridPoint(500,300,floors.h0.cm()));
|
||||
MyGridNode& end = (MyGridNode&)grid.getNodeFor(GridPoint(7000,5000,floors.h3.cm()));
|
||||
|
||||
Vis vis;
|
||||
vis.addFloor(floors.f0, floors.h0);
|
||||
vis.addFloor(floors.f1, floors.h1);
|
||||
vis.addFloor(floors.f2, floors.h2);
|
||||
vis.addFloor(floors.f3, floors.h3);
|
||||
|
||||
// vis.gp << "set xrange [1000:4000]\n";
|
||||
// vis.gp << "set yrange [1000:4000]\n";
|
||||
// vis.gp << "set zrange [0:600]\n";
|
||||
|
||||
// switch between different grid-walkers
|
||||
GridWalkRandomHeadingUpdate<MyGridNode> walk;
|
||||
//GridWalkRandomHeadingUpdateAdv<MyGridNode> walk;
|
||||
//GridWalkPushForward<MyGridNode> walk;
|
||||
//GridWalkLightAtTheEndOfTheTunnel<MyGridNode> walk(grid, DijkstraMapper(grid), end);
|
||||
|
||||
|
||||
std::vector<GridWalkState<MyGridNode>> states;
|
||||
for (int i = 0; i < 1000; ++i) { states.push_back(GridWalkState<MyGridNode>(&start, Heading::rnd())); }
|
||||
|
||||
// track the number-of-visits for each node to draw something like a particle-heat-map?
|
||||
|
||||
// show the importance factors
|
||||
// vis.addGrid(grid);
|
||||
// vis.show();
|
||||
// sleep(100);
|
||||
// vis.removeGrid();
|
||||
|
||||
Distribution::Normal<float> wDist(0.3, 0.3);
|
||||
|
||||
while(true) {
|
||||
for (GridWalkState<MyGridNode>& state : states) {
|
||||
state = walk.getDestination(grid, state, std::abs(wDist.draw()) );
|
||||
}
|
||||
usleep(1000*80);
|
||||
vis.showStates(states);
|
||||
vis.show();
|
||||
}
|
||||
|
||||
|
||||
|
||||
sleep(1000);
|
||||
|
||||
namespace Settings {
|
||||
const std::string floorplan = "/mnt/data/workspaces/Fusion2016/code/plan.svg";
|
||||
const int gridSize_cm = 200;
|
||||
}
|
||||
|
||||
struct MyNode : public GridNode, public GridPoint {
|
||||
public:
|
||||
MyNode(const float x_cm, const float y_cm, const float z_cm) : GridPoint(x_cm, y_cm, z_cm) {;}
|
||||
};
|
||||
|
||||
|
||||
int align(const int val) {
|
||||
return val / Settings::gridSize_cm * Settings::gridSize_cm;
|
||||
}
|
||||
|
||||
int main(void) {
|
||||
|
||||
Grid<MyNode> grid(Settings::gridSize_cm);
|
||||
GridFactory<MyNode> gridFac(grid);
|
||||
//testModelWalk();
|
||||
|
||||
|
||||
FloorplanFactorySVG fpFac(Settings::floorplan, 2.822222);
|
||||
|
||||
Floor f0 = fpFac.getFloor("floor_0");
|
||||
Floor f1 = fpFac.getFloor("floor_1");
|
||||
Floor f2 = fpFac.getFloor("floor_2");
|
||||
Floor f3 = fpFac.getFloor("floor_3");
|
||||
|
||||
Stairs f01 = fpFac.getStairs("staircase_0_1");
|
||||
Stairs f12 = fpFac.getStairs("staircase_1_2");
|
||||
Stairs f23 = fpFac.getStairs("staircase_2_3");
|
||||
|
||||
const LengthF h0 = LengthF::cm(align(0));
|
||||
const LengthF h1 = LengthF::cm(align(360));
|
||||
const LengthF h2 = LengthF::cm(align(360+340));
|
||||
const LengthF h3 = LengthF::cm(align(360+340+340));
|
||||
|
||||
gridFac.addFloor(f0, h0.cm());
|
||||
gridFac.addFloor(f1, h1.cm());
|
||||
gridFac.addFloor(f2, h2.cm());
|
||||
gridFac.addFloor(f3, h3.cm());
|
||||
//gridFac.removeIsolated();
|
||||
|
||||
Vis vis;
|
||||
vis.addFloor(f0, h0).addFloor(f1, h1).addFloor(f2, h2).addFloor(f3, h3);
|
||||
vis.addGrid(grid);
|
||||
vis.show();
|
||||
|
||||
sleep(1000);
|
||||
Eval eval;
|
||||
eval.setEval1();
|
||||
eval.run();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
8
code/particles/MyControl.h
Executable file
8
code/particles/MyControl.h
Executable file
@@ -0,0 +1,8 @@
|
||||
#ifndef MYCONTROL_H
|
||||
#define MYCONTROL_H
|
||||
|
||||
struct MyControl {
|
||||
|
||||
};
|
||||
|
||||
#endif // MYCONTROL_H
|
||||
85
code/particles/MyEvaluation.h
Executable file
85
code/particles/MyEvaluation.h
Executable file
@@ -0,0 +1,85 @@
|
||||
#ifndef MYEVALUATION_H
|
||||
#define MYEVALUATION_H
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
|
||||
#include "MyObservation.h"
|
||||
#include "MyState.h"
|
||||
|
||||
#include "../frank/WiFiEvaluation.h"
|
||||
#include "../frank/BeaconEvaluation.h"
|
||||
#include "../toni/BarometerEvaluation.h"
|
||||
#include "../lukas/StepEvaluation.h"
|
||||
#include "../lukas/TurnEvaluation.h"
|
||||
|
||||
class MyEvaluation : public K::ParticleFilterEvaluation<MyState, MyObservation> {
|
||||
|
||||
private:
|
||||
|
||||
WiFiEvaluation wifiEval;
|
||||
BeaconEvaluation beaconEval;
|
||||
BarometerEvaluation barometerEval;
|
||||
StepEvaluation stepEval;
|
||||
TurnEvaluation turnEval;
|
||||
|
||||
bool useWifi = true;
|
||||
bool useStep = true;
|
||||
bool useTurn = true;
|
||||
bool useBaro = true;
|
||||
bool useIB = true;
|
||||
|
||||
public:
|
||||
|
||||
void setUsage(bool useWifi, bool useStep, bool useTurn, bool useBaro, bool useIB) {
|
||||
this->useWifi = useWifi;
|
||||
this->useStep = useStep;
|
||||
this->useTurn = useTurn;
|
||||
this->useBaro = useBaro;
|
||||
this->useIB = useIB;
|
||||
}
|
||||
|
||||
virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
|
||||
|
||||
//if (observation.wifi) {
|
||||
wifiEval.nextObservation(observation.wifi);
|
||||
//}
|
||||
|
||||
// evalulate each particle
|
||||
double sum = 0;
|
||||
for (K::Particle<MyState>& p : particles) {
|
||||
|
||||
double weight = 1.0;
|
||||
|
||||
if (useWifi) {
|
||||
weight *= wifiEval.getProbability(p.state, observation);
|
||||
}
|
||||
|
||||
if (useBaro && observation.barometer) {
|
||||
weight *= barometerEval.getProbability(p.state, observation.barometer);
|
||||
}
|
||||
|
||||
if (useIB) {
|
||||
weight *= beaconEval.getProbability(p.state, observation);
|
||||
}
|
||||
|
||||
// if (useStep) {
|
||||
// weight *= stepEval.getProbability(p.state, observation.step);
|
||||
// p.state.distanceWalkedCM = 0.0;
|
||||
// }
|
||||
|
||||
// if (useTurn) {
|
||||
// weight *= turnEval.getProbability(p.state, observation.turn, true);
|
||||
// }
|
||||
|
||||
// set and accumulate
|
||||
p.weight = weight;
|
||||
sum += p.weight;
|
||||
|
||||
}
|
||||
|
||||
return sum;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // MYEVALUATION_H
|
||||
59
code/particles/MyInitializer.h
Executable file
59
code/particles/MyInitializer.h
Executable file
@@ -0,0 +1,59 @@
|
||||
#ifndef MYINITIALIZER3_H
|
||||
#define MYINITIALIZER3_H
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
|
||||
#include "MyState.h"
|
||||
|
||||
#include <Indoor/grid/Grid.h>
|
||||
|
||||
class MyInitializer : public K::ParticleFilterInitializer<MyState> {
|
||||
|
||||
private:
|
||||
int x_cm;
|
||||
int y_cm;
|
||||
int z_cm;
|
||||
int heading;
|
||||
|
||||
Grid<MyGridNode>& grid;
|
||||
|
||||
public:
|
||||
|
||||
/** q0 = random */
|
||||
MyInitializer(Grid<MyGridNode>& grid) : grid(grid), heading(0) {
|
||||
|
||||
}
|
||||
|
||||
/** q0 = given */
|
||||
MyInitializer(Grid<MyGridNode>& grid, int x_cm, int y_cm, int z_cm, int heading) :
|
||||
grid(grid), x_cm(x_cm), y_cm(y_cm), z_cm(z_cm), heading(heading) {
|
||||
|
||||
}
|
||||
|
||||
virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
|
||||
|
||||
std::minstd_rand gen;
|
||||
std::uniform_int_distribution<> dist(0, grid.getNumNodes());
|
||||
|
||||
for (K::Particle<MyState>& p : particles) {
|
||||
|
||||
MyGridNode& n = grid[dist(gen)];
|
||||
|
||||
//p.state.pCur = Point3(x_cm, y_cm, z_cm);
|
||||
//GridPoint gp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z);
|
||||
//p.state.walkState.node = &grid.getNodeFor(gp);
|
||||
|
||||
p.state.pCur = (Point3) n;
|
||||
p.state.walkState.node = &n;
|
||||
|
||||
p.state.pOld = p.state.pCur;
|
||||
p.state.walkState.heading = Heading::rnd();
|
||||
p.state.distanceWalkedCM = 0;
|
||||
p.state.hPa = 0;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // MYINITIALIZER_H
|
||||
49
code/particles/MyObservation.h
Executable file
49
code/particles/MyObservation.h
Executable file
@@ -0,0 +1,49 @@
|
||||
#ifndef MYOBSERVATION_H
|
||||
#define MYOBSERVATION_H
|
||||
|
||||
#include "../frank/WiFiObservation.h"
|
||||
#include "../frank/BeaconObservation.h"
|
||||
#include "../toni/BarometerObservation.h"
|
||||
#include "../lukas/StepObservation.h"
|
||||
#include "../lukas/TurnObservation.h"
|
||||
|
||||
/**
|
||||
* all available sensor readings
|
||||
*/
|
||||
struct MyObservation {
|
||||
|
||||
/** wifi observation */
|
||||
WiFiObservation wifi;
|
||||
|
||||
/** barometer observation data (if any) */
|
||||
BarometerObservation* barometer = nullptr;
|
||||
|
||||
/** beacon observation data */
|
||||
BeaconObservation beacons;
|
||||
|
||||
/** step observation data (if any) */
|
||||
StepObservation* step = nullptr;
|
||||
|
||||
/** turn observation data (if any) */
|
||||
TurnObservation* turn = nullptr;
|
||||
|
||||
/** timestamp of the youngest sensor data that resides within this observation. used to detect the age of all other observations! */
|
||||
uint64_t latestSensorDataTS = 0;
|
||||
|
||||
/** ctor */
|
||||
MyObservation() {
|
||||
// reset();
|
||||
}
|
||||
|
||||
// /** set all observations to null */
|
||||
// void reset() {
|
||||
// //delete wifi; wifi = nullptr;
|
||||
// delete barometer; barometer = nullptr;
|
||||
// delete beacons; beacons = nullptr;
|
||||
// //delete step; step = nullptr;
|
||||
// //delete turn; turn = nullptr;
|
||||
// }
|
||||
|
||||
};
|
||||
|
||||
#endif // MYOBSERVATION_H
|
||||
140
code/particles/MyState.h
Executable file
140
code/particles/MyState.h
Executable file
@@ -0,0 +1,140 @@
|
||||
#ifndef MYSTATE_H
|
||||
#define MYSTATE_H
|
||||
|
||||
#include <KLib/math/distribution/Normal.h>
|
||||
#include <KLib/math/optimization/NumOptVector.h>
|
||||
|
||||
#include <Indoor/grid/walk/GridWalkState.h>
|
||||
|
||||
#include "../MyGridNode.h"
|
||||
|
||||
/**
|
||||
* one possible state for the pedestrian
|
||||
* 3D position (x, y, floor-nr)
|
||||
*/
|
||||
struct MyState {
|
||||
|
||||
// current position
|
||||
Point3 pCur;
|
||||
|
||||
// previous position
|
||||
Point3 pOld;
|
||||
|
||||
// the grid-walk state
|
||||
GridWalkState<MyGridNode> walkState;
|
||||
|
||||
int distanceWalkedCM;
|
||||
|
||||
|
||||
// double heading_old;
|
||||
|
||||
// //double transHeading;
|
||||
// float numZChanges;
|
||||
|
||||
// // cumulative distance (in cm) this particle has taken. to-be-reset by the step detector whenever needed!
|
||||
// double distanceWalkedCM;
|
||||
|
||||
double hPa; //relative Pressure given by a history with size defined in BarometerSensorReader.h
|
||||
|
||||
// double vertical_acc; //vertical acceleration
|
||||
|
||||
// /** the pedestrian's current heading */
|
||||
// double heading;
|
||||
|
||||
/** empty ctor */
|
||||
MyState() : pCur(0,0,0), pOld(0,0,0), walkState(nullptr, Heading(0)) {
|
||||
;
|
||||
}
|
||||
|
||||
// /** get the 2D distance between this state and the given x,y (in centimter) */
|
||||
// double getDistance2D(const double x_cm, const double y_cm) const {
|
||||
// const double dx = (x_cm - this->x_cm);
|
||||
// const double dy = (y_cm - this->y_cm);
|
||||
// return std::sqrt( (dx*dx) + (dy*dy) );
|
||||
// }
|
||||
|
||||
// /** get the 3D distance between this state and the given x,y,floor (in centimter) */
|
||||
// double getDistance3D(const double x_cm, const double y_cm, const double floor_height_cm) const {
|
||||
// const double dx = (x_cm - this->x_cm);
|
||||
// const double dy = (y_cm - this->y_cm);
|
||||
// const double dz = (z_nr - this->z_nr) * floor_height_cm;
|
||||
// return std::sqrt( (dx*dx) + (dy*dy) + (dz*dz) );
|
||||
// }
|
||||
|
||||
/** -------- METHODS FOR THE PARTICLE FILTER -------- */
|
||||
|
||||
MyState& operator += (const MyState& o) {
|
||||
pCur += o.pCur;
|
||||
hPa += o.hPa;
|
||||
//distanceWalked += o.distanceWalked;
|
||||
return *this;
|
||||
}
|
||||
|
||||
MyState& operator /= (const double d) {
|
||||
pCur /= d;
|
||||
hPa /= d;
|
||||
//distanceWalked /= d;
|
||||
return *this;
|
||||
}
|
||||
|
||||
MyState operator * (const double d) const {
|
||||
MyState s = MyState(*this);
|
||||
s.pCur *= d;
|
||||
s.hPa *= d;
|
||||
//distanceWalked *= d;
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
// use the default one
|
||||
// MyState& operator = (const MyState& o) {
|
||||
// x_cm = o.x_cm;
|
||||
// y_cm = o.y_cm;
|
||||
// z_nr = o.z_nr;
|
||||
// x_cm_old = o.x_cm_old;
|
||||
// y_cm_old = o.y_cm_old;
|
||||
// z_nr_old = o.z_nr_old;
|
||||
// hPa = o.hPa;
|
||||
// heading_old = o.heading_old;
|
||||
// heading = o.heading;
|
||||
// distanceWalkedCM = o.distanceWalkedCM;
|
||||
// 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 {
|
||||
|
||||
//// // do NOT group particles in distinct floors!
|
||||
//// if (z_nr != o.z_nr) {return false;}
|
||||
|
||||
//// // get the 2D distance
|
||||
//// double d = (x_cm - o.x_cm)*(x_cm - o.x_cm) +
|
||||
//// (y_cm - o.y_cm)*(y_cm - o.y_cm);
|
||||
//// d = std::sqrt(d);
|
||||
|
||||
//// // 2D distance below grouping threshold?
|
||||
//// return d < 350.0;
|
||||
|
||||
// const double dx = (x_cm - o.x_cm);
|
||||
// const double dy = (y_cm - o.y_cm);
|
||||
// const double dz = (z_nr - o.z_nr) * 3000;
|
||||
|
||||
// // get the 2D distance
|
||||
// double d = dx*dx + dy*dy + dz*dz;
|
||||
// d = std::sqrt(d);
|
||||
// return d < 350.0;
|
||||
|
||||
// }
|
||||
|
||||
|
||||
|
||||
// MyState(K::NumOptVector<3>& params) : x_cm(params[0]), y_cm(params[1]), z_cm(params[2]) {;}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // MYSTATE_H
|
||||
175
code/particles/MyTransition.h
Executable file
175
code/particles/MyTransition.h
Executable file
@@ -0,0 +1,175 @@
|
||||
#ifndef MYTRANSITION_H
|
||||
#define MYTRANSITION_H
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
|
||||
#include <KLib/math/distribution/Normal.h>
|
||||
#include <KLib/math/distribution/Uniform.h>
|
||||
|
||||
|
||||
#include <Indoor/grid/Grid.h>
|
||||
#include <Indoor/grid/walk/GridWalk.h>
|
||||
|
||||
#include "MyState.h"
|
||||
#include "MyControl.h"
|
||||
//#include "Helper.h"
|
||||
|
||||
#include "../toni/barometric.h"
|
||||
|
||||
#include "../MyGridNode.h"
|
||||
|
||||
inline double sgn(double x){
|
||||
return ((x>0)?1 : ((x<0)?-1 : 1));
|
||||
}
|
||||
|
||||
class MyTransition : public K::ParticleFilterTransition<MyState, MyControl> {
|
||||
|
||||
private:
|
||||
|
||||
Grid<MyGridNode>& grid;
|
||||
GridWalk<MyGridNode>& walker;
|
||||
|
||||
|
||||
/** a simple normal distribution */
|
||||
K::UniformDistribution distWalkStop;
|
||||
K::NormalDistribution distWalk;
|
||||
K::NormalDistribution distStop;
|
||||
|
||||
|
||||
/** normal distribution for barometer */
|
||||
K::NormalDistribution distBaro;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* ctor
|
||||
* @param choice the choice to use for randomly drawing nodes
|
||||
* @param fp the underlying floorplan
|
||||
*/
|
||||
MyTransition(Grid<MyGridNode>& grid, GridWalk<MyGridNode>& walker) :
|
||||
grid(grid), walker(walker),
|
||||
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);
|
||||
distStop.setSeed(1234);
|
||||
|
||||
distBaro.setSeed(5678);
|
||||
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
uint64_t ts = 0;
|
||||
uint64_t deltaMS = 0;
|
||||
|
||||
/** set the current time in millisconds */
|
||||
void setCurrentTime(const uint64_t ts) {
|
||||
if (this->ts == 0) {
|
||||
this->ts = ts;
|
||||
deltaMS = 0;
|
||||
} else {
|
||||
deltaMS = ts - this->ts;
|
||||
this->ts = ts;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
|
||||
for (K::Particle<MyState>& p : particles) {
|
||||
|
||||
// TODO: depending on the time since the last update
|
||||
// random distance to move
|
||||
// const double distance = (distWalkStop.draw() > 0.2) ? (distWalk.draw()) : (distStop.draw());
|
||||
// double dist_m = distance * deltaMS / 1000.0;
|
||||
|
||||
// if (dist_m < 0) {dist_m = -dist_m; p.state.heading = rand() % 360;}
|
||||
|
||||
// update the old heading and the other old values
|
||||
//p.state.walkState.heading = p.state.heading;
|
||||
p.state.pOld = p.state.pCur;
|
||||
|
||||
|
||||
// 10% stand still, 90% walk
|
||||
double dist_m;
|
||||
if (distWalkStop.draw() > 0.9) {
|
||||
dist_m = std::abs(distStop.draw() * deltaMS / 1000.0);
|
||||
} else {
|
||||
dist_m = std::abs(distWalk.draw() * deltaMS / 1000.0);
|
||||
}
|
||||
|
||||
// update cumulative distance
|
||||
p.state.distanceWalkedCM += std::abs(dist_m * 100.0);
|
||||
|
||||
// get new destination
|
||||
//const Node3* dst = choice->getTarget(src, p.state, dist_m);
|
||||
p.state.walkState = walker.getDestination(grid, p.state.walkState, dist_m );
|
||||
|
||||
// randomly move the particle within its target grid (box)
|
||||
// (z remains unchanged!)
|
||||
const int grid_size_cm = grid.getGridSize_cm();
|
||||
|
||||
// new position (x,y) is randomly distributed within the target node
|
||||
Point3 noise = Point3(0,0,0); // TODO
|
||||
p.state.pCur = (Point3) *p.state.walkState.node + noise;
|
||||
|
||||
|
||||
// --- 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;
|
||||
|
||||
// //The height of the single floor levels.
|
||||
// const static double floor_height[3] = {4.1, 3.4, 3.4};
|
||||
|
||||
// //update barometer
|
||||
// if(USE_BAROMETRIC_FORMULAR){
|
||||
// //height the particle has climbed.
|
||||
// double h_1 = 0.0;
|
||||
// double mu = 0.0;
|
||||
|
||||
// //we need only the sign of the floors changed, since the pressure change between the floors
|
||||
// //is calculated within s_getAtmosphericPressure
|
||||
// numFloorChanged = sgn(numFloorChanged);
|
||||
|
||||
// for(int i = std::min(p.state.z_nr_old, p.state.z_nr); i < std::max(p.state.z_nr_old, p.state.z_nr); i++){
|
||||
// h_1 += floor_height[i];
|
||||
// }
|
||||
|
||||
// {
|
||||
// // use the barometric formular to calculate the relative pressure
|
||||
// // the calculation is done assuming sea level height at every floor.
|
||||
// double mslp = BarometricFormular::s_getSeaLevelPressure();
|
||||
// double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
|
||||
// mu = std::abs(mslp - pressure);
|
||||
// }
|
||||
|
||||
// if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
|
||||
// p.state.hPa += numFloorChanged * K::NormalDistribution::draw(mu, 0.005);
|
||||
// else
|
||||
// p.state.hPa = numFloorChanged * K::NormalDistribution::draw(mu, 0.15);
|
||||
// }
|
||||
// else{
|
||||
// if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
|
||||
// p.state.hPa += numFloorChanged * distBaro.draw();
|
||||
// else
|
||||
// p.state.hPa = numFloorChanged * distBaro.draw();
|
||||
// }
|
||||
|
||||
// // sanity check
|
||||
// if (p.state.heading != p.state.heading) {throw "detected NaN";}
|
||||
// if (p.state.z_nr != p.state.z_nr) {throw "detected NaN";}
|
||||
// if (p.state.x_cm != p.state.x_cm) {throw "detected NaN";}
|
||||
// if (p.state.y_cm != p.state.y_cm) {throw "detected NaN";}
|
||||
|
||||
// // ensure p.state.z_nr IS discreet
|
||||
// if ( std::abs(p.state.z_nr - std::round(p.state.z_nr)) > 0.01) {throw "detected continuous z_nr!";}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif // MYTRANSITION_H
|
||||
83
code/particles/MyTransitionSimple.h
Executable file
83
code/particles/MyTransitionSimple.h
Executable file
@@ -0,0 +1,83 @@
|
||||
#ifndef MYTRANSITIONSIMPLE_H
|
||||
#define MYTRANSITIONSIMPLE_H
|
||||
|
||||
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
|
||||
#include <KLib/math/distribution/Normal.h>
|
||||
#include "MyState.h"
|
||||
#include "MyControl.h"
|
||||
|
||||
class MyTransitionSimple : public K::ParticleFilterTransition<MyState, MyControl> {
|
||||
|
||||
private:
|
||||
|
||||
/** a simple normal distribution */
|
||||
K::NormalDistribution distX;
|
||||
K::NormalDistribution distY;
|
||||
K::NormalDistribution distZ;
|
||||
K::NormalDistribution distBaro;
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
MyTransitionSimple() : distX(0, 1.0), distY(0, 1.0), distZ(0, 1.0), distBaro(0.3, 0.05) {
|
||||
distX.setSeed(1234);
|
||||
distY.setSeed(1235);
|
||||
distZ.setSeed(1236);
|
||||
distBaro.setSeed(5678);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
uint64_t ts = 0;
|
||||
uint64_t deltaMS = 0;
|
||||
|
||||
/** set the current time in millisconds */
|
||||
void setCurrentTime(const uint64_t ts) {
|
||||
if (this->ts == 0) {
|
||||
this->ts = ts;
|
||||
deltaMS = 0;
|
||||
} else {
|
||||
deltaMS = ts - this->ts;
|
||||
this->ts = ts;
|
||||
}
|
||||
}
|
||||
|
||||
virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
|
||||
|
||||
for (K::Particle<MyState>& p : particles) {
|
||||
|
||||
p.state.heading_old = p.state.heading;
|
||||
p.state.x_cm_old = p.state.x_cm;
|
||||
p.state.y_cm_old = p.state.y_cm;
|
||||
p.state.z_nr_old = p.state.z_nr;
|
||||
|
||||
p.state.x_cm += (distX.draw() * deltaMS / 1000.0) * 250.0;
|
||||
p.state.y_cm += (distY.draw() * deltaMS / 1000.0) * 250.0;
|
||||
p.state.z_nr += (distZ.draw() * deltaMS / 1000.0) * 0.25;
|
||||
p.state.heading = Helper::angleBetween(p.state.x_cm_old, p.state.y_cm_old, p.state.x_cm, p.state.y_cm);
|
||||
|
||||
// if (p.state.z_nr < 0.5) {p.state.z_nr = 0.5;}
|
||||
// if (p.state.z_nr > 3.5) {p.state.z_nr = 3.5;}
|
||||
// if (p.state.x_cm < 0) {p.state.x_cm = 0;}
|
||||
// if (p.state.y_cm < 0) {p.state.y_cm = 0;}
|
||||
|
||||
|
||||
|
||||
//update barometer
|
||||
p.state.hPa += (p.state.z_nr_old - p.state.z_nr) * distBaro.draw();
|
||||
|
||||
// update walked distance (2D)
|
||||
const double dx = p.state.x_cm_old - p.state.x_cm;
|
||||
const double dy = p.state.y_cm_old - p.state.y_cm;
|
||||
p.state.distanceWalkedCM = std::sqrt((dx*dx) + (dy*dy));
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // MYTRANSITIONSIMPLE_H
|
||||
378
code/plan.svg
378
code/plan.svg
@@ -42,23 +42,23 @@
|
||||
guidetolerance="10"
|
||||
inkscape:pageopacity="0"
|
||||
inkscape:pageshadow="2"
|
||||
inkscape:window-width="1366"
|
||||
inkscape:window-height="672"
|
||||
inkscape:window-width="1600"
|
||||
inkscape:window-height="845"
|
||||
id="namedview3234"
|
||||
showgrid="true"
|
||||
inkscape:zoom="2.4027176"
|
||||
inkscape:cx="422.22336"
|
||||
inkscape:cy="1231.7432"
|
||||
inkscape:zoom="0.84948895"
|
||||
inkscape:cx="272.89941"
|
||||
inkscape:cy="1411.2257"
|
||||
inkscape:window-x="0"
|
||||
inkscape:window-y="33"
|
||||
inkscape:window-y="0"
|
||||
inkscape:window-maximized="1"
|
||||
inkscape:current-layer="layer11"
|
||||
inkscape:current-layer="layer8"
|
||||
inkscape:object-nodes="true"
|
||||
units="px"
|
||||
showborder="true"
|
||||
inkscape:snap-to-guides="true"
|
||||
inkscape:snap-text-baseline="true"
|
||||
showguides="true"
|
||||
showguides="false"
|
||||
inkscape:guide-bbox="true"
|
||||
inkscape:snap-nodes="true"
|
||||
inkscape:snap-smooth-nodes="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"
|
||||
@@ -238,7 +238,7 @@
|
||||
inkscape:groupmode="layer"
|
||||
id="layer2"
|
||||
inkscape:label="roomnames_2"
|
||||
style="display:inline">
|
||||
style="display:none">
|
||||
<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"
|
||||
@@ -1840,12 +1840,6 @@
|
||||
d="m 623.62205,279.92122 0,-212.598421"
|
||||
id="path4544"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 538.58268,198.42516 0,81.49606 m 85.03937,-81.49606 -173.62205,0 0,81.49606"
|
||||
id="path4546"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 577.55906,279.92122 -49.6063,0"
|
||||
@@ -1907,9 +1901,10 @@
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 655.51181,1853.1496 0,-375.5906"
|
||||
d="m 655.51181,1853.1496 0,-432.2835"
|
||||
id="path4570"
|
||||
inkscape:connector-curvature="0" />
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 655.51181,1420.8661 0,-683.85826"
|
||||
@@ -1949,9 +1944,10 @@
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 584.64567,1477.559 0,106.2992"
|
||||
d="m 595.27559,1477.559 0,106.2992"
|
||||
id="path4586"
|
||||
inkscape:connector-curvature="0" />
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 446.45669,1350 0,-35.4331"
|
||||
@@ -2198,14 +2194,16 @@
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 368.50394,992.12595 0,800.78745"
|
||||
d="m 368.50394,992.12595 0,797.24405"
|
||||
id="path4684"
|
||||
inkscape:connector-curvature="0" />
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 368.50394,1828.3464 0,24.8032"
|
||||
d="m 368.50394,1831.8897 0,21.2599"
|
||||
id="path4686"
|
||||
inkscape:connector-curvature="0" />
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 446.45669,1332.2834 209.05512,0"
|
||||
@@ -2221,11 +2219,6 @@
|
||||
d="m 99.212598,403.93697 -49.606299,0"
|
||||
id="path4692"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 446.45669,67.322799 0,67.322831"
|
||||
id="path4694"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 549.2126,134.64563 -102.75591,0"
|
||||
@@ -2233,14 +2226,63 @@
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2402.3622,77.95272 0,56.69291 -92.126,0"
|
||||
d="m 2402.3622,134.64563 -92.126,0"
|
||||
id="path4698"
|
||||
inkscape:connector-curvature="0" />
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 49.606299,1683.0708 318.897641,0"
|
||||
id="path4404"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2785.0394,386.22044 0,-106.29922"
|
||||
id="path5825"
|
||||
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.42516 0,-131.102361"
|
||||
id="path5829"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="display:inline;fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 450,279.9212 0,-81.49606"
|
||||
id="path5851"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="display:inline;fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 450,198.42514 173.62205,0"
|
||||
id="path5853"
|
||||
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 538.58268,279.92122 0,-81.49606"
|
||||
id="path5873"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2381.1024,198.42516 0,-63.77953"
|
||||
id="path5875"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2381.1024,77.95272 0,56.69291"
|
||||
id="path5877"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 531.49606,1491.7322 63.77953,0"
|
||||
id="path5899"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 595.27559,1491.7322 60.23622,0"
|
||||
id="path5901"
|
||||
inkscape:connector-curvature="0" />
|
||||
</g>
|
||||
<g
|
||||
inkscape:groupmode="layer"
|
||||
@@ -2677,7 +2719,7 @@
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 584.64567,1477.559 0,106.2992"
|
||||
d="m 595.27559,1477.559 0,106.2992"
|
||||
id="path12844"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
@@ -2867,21 +2909,6 @@
|
||||
d="m 2462.5984,460.62989 0,276.37795"
|
||||
id="path12918"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2398.8189,134.64563 0,-67.322831"
|
||||
id="path13024"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 446.45669,134.64563 0,-67.322831"
|
||||
id="path13061"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 584.64567,1477.559 70.86614,0"
|
||||
id="path13065"
|
||||
inkscape:connector-curvature="0" />
|
||||
<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.61414 0,67.32283 389.76378,0 0,-67.32283 -389.76378,0"
|
||||
@@ -2893,6 +2920,36 @@
|
||||
d="m 2207.4803,279.92122 17.7166,0"
|
||||
id="path3498"
|
||||
inkscape:connector-curvature="0" />
|
||||
<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.42516 0,-63.77953"
|
||||
id="path5831"
|
||||
inkscape:connector-curvature="0" />
|
||||
<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,134.64563 0,-67.322831"
|
||||
id="path5845"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2381.1024,198.42516 0,-63.77953"
|
||||
id="path5879"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2381.1024,67.322799 0,67.322831"
|
||||
id="path5881"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 531.49606,1491.7322 63.77953,0"
|
||||
id="path5903"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 595.27559,1491.7322 60.23622,0"
|
||||
id="path5905"
|
||||
inkscape:connector-curvature="0" />
|
||||
</g>
|
||||
<g
|
||||
inkscape:groupmode="layer"
|
||||
@@ -2914,13 +2971,13 @@
|
||||
sodipodi:nodetypes="ccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 368.50394,1619.2913 0,-42.5197"
|
||||
d="m 368.50394,1619.2913 0,-38.9764"
|
||||
id="path11569"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 368.50394,1548.4252 0,-467.7166"
|
||||
d="m 368.50394,1544.8818 0,-460.6299"
|
||||
id="path11571"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
@@ -3055,7 +3112,7 @@
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 584.64567,1477.559 0,113.3858"
|
||||
d="m 595.27559,1477.559 0,113.3858"
|
||||
id="path11625"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
@@ -3545,24 +3602,6 @@
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2398.8189,134.64563 0,-67.322831"
|
||||
id="path13022"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 446.45669,134.64563 0,-67.322831"
|
||||
id="path13059"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 584.64567,1477.559 70.86614,0"
|
||||
id="path13063"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 1729.1339,336.6141 -198.4252,0 0,67.32283 198.4252,0"
|
||||
@@ -3609,6 +3648,36 @@
|
||||
id="path3487"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)" />
|
||||
<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"
|
||||
id="path5833"
|
||||
inkscape:connector-curvature="0" />
|
||||
<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,134.64559 0,-67.322833"
|
||||
id="path5847"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2381.1024,198.42512 0,-63.77953"
|
||||
id="path5883"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2381.1024,67.322757 0,67.322833"
|
||||
id="path5885"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 531.49606,1491.7322 63.77953,0"
|
||||
id="path5907"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 595.27559,1491.7322 60.23622,0"
|
||||
id="path5909"
|
||||
inkscape:connector-curvature="0" />
|
||||
</g>
|
||||
<g
|
||||
inkscape:groupmode="layer"
|
||||
@@ -4317,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" />
|
||||
@@ -4329,13 +4398,13 @@
|
||||
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" />
|
||||
<path
|
||||
style="display:inline;fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 584.64567,1477.559 0,113.3858"
|
||||
d="m 595.27559,1477.559 0,113.3858"
|
||||
id="path3459-9"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
@@ -4353,28 +4422,41 @@
|
||||
transform="translate(0,-4.2364502e-5)"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2398.8189,134.64563 0,-67.322831"
|
||||
id="path13020"
|
||||
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 -60.23622,-10e-6"
|
||||
id="path4163"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 584.64567,1477.559 70.86614,0"
|
||||
id="path13055"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)"
|
||||
sodipodi:nodetypes="cc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 446.45669,134.64563 0,-67.322831"
|
||||
id="path13057"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)" />
|
||||
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"
|
||||
id="path5835"
|
||||
inkscape:connector-curvature="0" />
|
||||
<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"
|
||||
id="path4163"
|
||||
d="m 467.71654,134.64559 0,-67.322833"
|
||||
id="path5849"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2381.1024,198.42512 0,-63.77953"
|
||||
id="path5887"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2381.1024,67.322757 0,67.322833"
|
||||
id="path5889"
|
||||
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 531.49606,1491.7322 63.77953,0"
|
||||
id="path5911"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 595.27559,1491.7322 60.23622,0"
|
||||
id="path5913"
|
||||
inkscape:connector-curvature="0" />
|
||||
</g>
|
||||
<g
|
||||
@@ -4383,73 +4465,34 @@
|
||||
inkscape:label="staircase_0_1"
|
||||
style="display:none">
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 535.03937,1477.559 0,177.1654 116.92913,0 0,-177.1654 -109.84252,0"
|
||||
id="path4702"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 439.37008,570.47241 0,56.69291 63.77953,0 0,-56.69291 -56.69292,0"
|
||||
id="path4704"
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
d="m 2664.5669,655.51178 102.7559,0 0,-155.90552"
|
||||
id="path4988"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
sodipodi:nodetypes="ccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 457.08661,74.409413 0,120.472437 162.99213,0 0,-124.015744 -162.99213,0"
|
||||
id="path4706"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2228.7402,77.95272 0,116.92913 170.0787,0 0,-124.015744 -170.0787,0"
|
||||
id="path4708"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2661.0236,655.51178 0,-159.44882 109.8425,0 0,159.44882 -106.2992,0"
|
||||
id="path4710"
|
||||
inkscape:connector-curvature="0" />
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
d="m 503.14961,457.08658 -46.063,0 0,276.37795"
|
||||
id="path4992"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccc" />
|
||||
</g>
|
||||
<g
|
||||
inkscape:groupmode="layer"
|
||||
id="layer11"
|
||||
inkscape:label="staircase_1_2"
|
||||
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 457.08661,194.88185 162.99213,0"
|
||||
id="path12930"
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
d="m 2125.9843,343.70075 0,53.14961 -226.7717,0"
|
||||
id="path4998"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cc" />
|
||||
sodipodi:nodetypes="ccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 620.07874,194.88185 0,-124.015744 -162.99213,0 0,124.015744"
|
||||
id="path12932"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2125.9843,333.07083 -226.7717,0 0,77.95276 233.8583,0 0,-63.77953"
|
||||
id="path12934"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2398.8189,70.866106 -170.0787,0 0,124.015744 170.0787,0 0,-124.015744"
|
||||
id="path12936"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 535.03937,1477.559 116.92913,0 0,177.1654 -116.92913,0 0,-177.1654"
|
||||
id="path12959"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 446.45669,566.9291 56.69292,0 0,74.40945 -63.77953,0 0,-67.32284"
|
||||
id="path4157"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
d="m 503.14961,460.62989 -56.69292,0 0,276.37795"
|
||||
id="path5000"
|
||||
inkscape:connector-curvature="0" />
|
||||
</g>
|
||||
<g
|
||||
inkscape:groupmode="layer"
|
||||
@@ -4458,43 +4501,16 @@
|
||||
style="display:none"
|
||||
transform="translate(0,4.2364502e-5)">
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 524.40945,1477.559 0,170.0788 127.55905,0 0,-170.0788"
|
||||
id="path11647"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="cccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 457.08661,70.866106 0,124.015744 162.99213,0 0,-124.015744 -162.99213,0"
|
||||
id="path13002"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2228.7402,70.866106 0,124.015744 170.0787,0 0,-124.015744 -170.0787,0"
|
||||
id="path13006"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
<path
|
||||
style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 2331.4961,279.92122 0,-77.95275 63.7795,0 0,77.95275 -63.7795,0"
|
||||
id="path13008"
|
||||
inkscape:connector-curvature="0"
|
||||
transform="translate(0,-4.2364502e-5)"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 1697.2441,283.46449 0,173.62205 17.7165,0 0,-173.62205 -14.1732,0"
|
||||
id="path4161"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
d="m 503.14961,641.33851 0,-67.32284 -56.69292,0 0,67.32284 53.14961,0"
|
||||
id="path4165"
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
d="m 503.14961,460.62985 -56.69292,0 0,276.37795"
|
||||
id="path5802"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
d="m 1729.1339,340.1574 0,60.23622 -194.8819,0"
|
||||
id="path5804"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccc" />
|
||||
</g>
|
||||
<g
|
||||
inkscape:groupmode="layer"
|
||||
|
||||
|
Before Width: | Height: | Size: 223 KiB After Width: | Height: | Size: 224 KiB |
4709
code/plan_new.svg
Normal file
4709
code/plan_new.svg
Normal file
File diff suppressed because it is too large
Load Diff
|
After Width: | Height: | Size: 226 KiB |
@@ -2,10 +2,10 @@
|
||||
|
||||
#include "../particles/MyState.h"
|
||||
#include "BarometerObservation.h"
|
||||
#include "barometric.h"
|
||||
#include <KLib/math/distribution/Normal.h>
|
||||
//#include "barometric.h"
|
||||
//#include <KLib/math/distribution/Normal.h>
|
||||
|
||||
double g_BarometerObservation = 0.0;
|
||||
static constexpr double g_BarometerObservation = 0.0;
|
||||
|
||||
class BarometerEvaluation {
|
||||
|
||||
@@ -13,46 +13,50 @@ public:
|
||||
|
||||
double getProbability(const MyState& state, const BarometerObservation* obs) const {
|
||||
|
||||
//rho_z
|
||||
double barometerSigma = 0.3;
|
||||
//return 1;
|
||||
|
||||
//The height of the single floor levels.
|
||||
const static double floor_height[3] = {4.1, 3.4, 3.4};
|
||||
// //rho_z
|
||||
double barometerSigma = 0.09;
|
||||
|
||||
if(USE_BAROMETRIC_FORMULAR){
|
||||
//height the particle has climbed.
|
||||
double h_1 = 0.0;
|
||||
for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
|
||||
h_1 += floor_height[i];
|
||||
}
|
||||
// //The height of the single floor levels.
|
||||
// const static double floor_height[3] = {4.1, 3.4, 3.4};
|
||||
|
||||
if(h_1 != 0.0){
|
||||
// use the barometric formular to calculate the relative pressure
|
||||
// the calculation is done assuming sea level height at every floor.
|
||||
double mslp = BarometricFormular::s_getSeaLevelPressure();
|
||||
double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
|
||||
barometerSigma = std::abs(mslp - pressure);
|
||||
}
|
||||
// if(USE_BAROMETRIC_FORMULAR){
|
||||
// //height the particle has climbed.
|
||||
// double h_1 = 0.0;
|
||||
// for(int i = std::min(state.z_nr_old, state.z_nr); i < std::max(state.z_nr_old, state.z_nr); i++){
|
||||
// h_1 += floor_height[i];
|
||||
// }
|
||||
|
||||
}
|
||||
else {
|
||||
// constant value for sigma if we assume all floors are same in height
|
||||
barometerSigma = 0.30 / 1.0; //hPa
|
||||
}
|
||||
// if(h_1 != 0.0){
|
||||
// // use the barometric formular to calculate the relative pressure
|
||||
// // the calculation is done assuming sea level height at every floor.
|
||||
// double mslp = BarometricFormular::s_getSeaLevelPressure();
|
||||
// double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
|
||||
// barometerSigma = std::abs(mslp - pressure);
|
||||
// }
|
||||
|
||||
// evaluate the current particle with a normal distribution
|
||||
const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma/2, obs->hpa);
|
||||
// }
|
||||
// else {
|
||||
// // constant value for sigma if we assume all floors are same in height
|
||||
// barometerSigma = 0.30 / 1.0; //hPa
|
||||
// }
|
||||
|
||||
//Just for the visualization. i'm a lazy bastard
|
||||
g_BarometerObservation = obs->hpa;
|
||||
// // evaluate the current particle with a normal distribution
|
||||
const double barometerProbability = K::NormalDistribution::getProbability(state.hPa, barometerSigma, obs->hpa);
|
||||
|
||||
assert(barometerProbability == barometerProbability);
|
||||
assert(state.hPa == state.hPa);
|
||||
assert(obs->hpa == obs->hpa);
|
||||
// //Just for the visualization. i'm a lazy bastard
|
||||
//g_BarometerObservation = obs->hpa;
|
||||
|
||||
//std::cout << barometerProbability << std::endl;
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
@@ -2,28 +2,32 @@
|
||||
|
||||
#include "circular.h"
|
||||
#include "BarometerObservation.h"
|
||||
#include "../SensorReader.h"
|
||||
#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
|
||||
|
||||
@@ -131,7 +131,7 @@
|
||||
% not capitalized unless they are the first or last word of the title.
|
||||
% Linebreaks \\ can be used within to get better formatting as desired.
|
||||
% Do not put math or special symbols in the title.
|
||||
\title{Realistic Pathfinding for Multi Sensor 3D Indoor Localisation and Navigation}
|
||||
\title{On Prior Navigation Knowledge in Multi Sensor Indoor Localisation}
|
||||
|
||||
|
||||
% author names and affiliations
|
||||
@@ -180,10 +180,10 @@
|
||||
|
||||
\input{chapters/relatedwork}
|
||||
|
||||
\input{chapters/grid}
|
||||
|
||||
\input{chapters/system}
|
||||
|
||||
\input{chapters/grid}
|
||||
|
||||
\input{chapters/experiments}
|
||||
|
||||
\input{chapters/conclusion}
|
||||
|
||||
@@ -9,7 +9,9 @@
|
||||
\subitem klarstellen was wir anders/besser machen
|
||||
\item graphen-basierte systeme
|
||||
\subitem probability graph / transition
|
||||
\item pathfinding
|
||||
\item pathfinding for humans
|
||||
\subitem computerspiele machen das schon ewig. robotor auch.
|
||||
\subitem auf menschliches verhalten anpassen. gibt es viele theoritsche ansätze und simulationen aber in noch keinem system.
|
||||
\end{itemize}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user