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

version for plotting the figures of kde paper

Browse Source
This commit is contained in:
toni
2018-02-19 18:18:45 +01:00
parent 46abeae148
commit 97ce18f5f1
12 changed files with 1374 additions and 1576 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@@ -59,13 +59,13 @@ ADD_DEFINITIONS(
-fstack-protector-all -fstack-protector-all
-g3 -g3
# -O2 #-O2
-march=native -march=native
-DWITH_TESTS -DWITH_TESTS
-DWITH_ASSERTIONS -DWITH_ASSERTIONS
#-DWITH_DEBUG_LOG #-DWITH_DEBUG_LOG
-DWITH_DEBUG_PLOT #-DWITH_DEBUG_PLOT
#-D_GLIBCXX_DEBUG #-D_GLIBCXX_DEBUG

1
Plotti.h
View File

@@ -1,7 +1,6 @@
#ifndef PLOTTI_H #ifndef PLOTTI_H
#define PLOTTI_H #define PLOTTI_H
#include "filter/Structs.h"
#include "Settings.h" #include "Settings.h"
#include <functional> #include <functional>

672
Plotty.h Executable file
View File

@@ -0,0 +1,672 @@
#ifndef PLOTTY_H
#define PLOTTY_H
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
#include <Indoor/geo/BBoxes3.h>
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPM3D.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementEmpty.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>
#include <KLib/misc/gnuplot/objects/GnuplotObjects.h>
struct Color {
uint8_t r;
uint8_t g;
uint8_t b;
Color() : r(0), g(0), b(0) {
;
}
static Color fromRGB(const uint8_t r, const uint8_t g, const uint8_t b) {
Color c; c.setRGB(r,g,b);
return c;
}
static Color fromHSV(const uint8_t h, const uint8_t s, const uint8_t v) {
Color c; c.setHSV(h,s,v);
return c;
}
void setRGB(const uint8_t r, const uint8_t g, const uint8_t b) {
this->r = r;
this->g = g;
this->b = b;
}
void setHSV(const uint8_t h, const uint8_t s, const uint8_t v) {
uint8_t region, remainder, p, q, t;
region = h / 43;
remainder = (h - (region * 43)) * 6;
p = (v * (255 - s)) >> 8;
q = (v * (255 - ((s * remainder) >> 8))) >> 8;
t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
switch (region) {
case 0:
r = v; g = t; b = p;
break;
case 1:
r = q; g = v; b = p;
break;
case 2:
r = p; g = v; b = t;
break;
case 3:
r = p; g = q; b = v;
break;
case 4:
r = t; g = p; b = v;
break;
default:
r = v; g = p; b = q;
break;
}
}
std::string toHEX() const {
char buf[8];
sprintf(buf, "#%02x%02x%02x", r, g, b);
std::string color(buf);
return color;
}
};
class Plotty {
public:
const Floorplan::IndoorMap* map;
K::Gnuplot gp;
K::GnuplotSplot splot;
K::GnuplotSplotElementPoints points;
K::GnuplotSplotElementColorPoints cpoints;
K::GnuplotSplotElementLines pathReal;
K::GnuplotSplotElementLines pathEst;
K::GnuplotSplotElementColorPoints particles;
K::GnuplotSplotElementLines mapOutlineGlass;
K::GnuplotSplotElementLines mapOutlineDrywall;
K::GnuplotSplotElementLines mapOutlineConcrete;
K::GnuplotSplotElementLines mapBBoxes;
K::GnuplotSplotElementEmpty emptyElem;
K::GnuplotSplotElementPM3D pm3doutline;
std::string codeFile;
struct Settings {
std::vector<int> floors = {};
bool stairs = true;
bool obstacles = true;
bool outline = true;
bool outlineColorCustom = false;
bool skipI1 = false;
K::GnuplotColor outlineColor = K::GnuplotColor::fromRGB(128,128,128);
float minZ = -9999;
float maxZ = +9999;
} settings;
public:
Plotty(const Floorplan::IndoorMap* map) : map(map) {
//gp << "set view equal xy\n";
//gp << "set palette model RGB\n";
//gp << "r(x) = (x < 0) ? 0 : (x/2)\n";
//gp << "g(x) = 0\n";
//gp << "b(x) = (x > 0) ? 0 : (-x/2)\n";
//gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n";
gp << "set ticslevel 0\n";
// how to draw the floorplan
mapOutlineConcrete.getStroke().getColor().setHexStr("#888888"); mapOutlineConcrete.getStroke().setWidth(2);
mapOutlineDrywall.getStroke().getColor().setHexStr("#888888");
mapOutlineGlass.getStroke().getColor().setHexStr("#888888"); mapOutlineGlass.getStroke().setType(K::GnuplotDashtype::DASHED);
mapBBoxes.getStroke().setWidth(2);
splot.add(&emptyElem);
splot.add(&mapOutlineConcrete);
splot.add(&mapOutlineDrywall);
splot.add(&mapOutlineGlass);
splot.add(&mapBBoxes);
splot.add(&particles); particles.setPointSize(0.20); //particles.setColorHex("#777777");
splot.add(&pathReal); pathReal.getStroke().setWidth(2); pathReal.getStroke().getColor().setHexStr("#000000");
splot.add(&pathEst); pathEst.getStroke().setWidth(2); pathEst.getStroke().getColor().setHexStr("#0000ff");
splot.add(&pm3doutline);
splot.add(&points);
points.setPointType(7);
points.setPointSize(0.5);
splot.add(&cpoints);
cpoints.setPointSize(2);
cpoints.setPointType(7);
}
void addBBoxes(const BBoxes3& boxes, const K::GnuplotColor& c) {
for (BBox3 bb : boxes.get()) {
//&&addBBoxPoly(bb, c);
//bb.shrink(0.98);
addBBoxPoly2(bb, c);
}
}
void addBBoxPoly2(const BBox3& bb, const K::GnuplotColor& color) {
K::GnuplotFill filler = K::GnuplotFill(K::GnuplotFillStyle::SOLID, color);
K::GnuplotStroke stroke(K::GnuplotDashtype::NONE, 1, color);
K::GnuplotObjectPolygon* gpol1 = new K::GnuplotObjectPolygon(filler, stroke);
gpol1->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
gpol1->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
gpol1->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
gpol1->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
gpol1->close();
gpol1->setZIndex(bb.getMin().z - 0.1);
splot.getObjects().add(gpol1);
K::GnuplotColor color2 = K::GnuplotColor::fromRGB(128,128,128);
K::GnuplotStroke stroke2(K::GnuplotDashtype::NONE, 1, color2);
K::GnuplotFill noFiller = K::GnuplotFill(K::GnuplotFillStyle::EMPTY_BORDER, color2);
K::GnuplotObjectPolygon* gpol2 = new K::GnuplotObjectPolygon(noFiller, stroke2);
gpol2->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
gpol2->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
gpol2->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
gpol2->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
gpol2->close();
gpol2->setZIndex(bb.getMin().z + 3.1);
splot.getObjects().add(gpol2);
K::GnuplotObjectPolygon* gpol3a = new K::GnuplotObjectPolygon(noFiller, stroke2);
gpol3a->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
gpol3a->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
gpol3a->setZIndex(bb.getMin().z + 1.1);
splot.getObjects().add(gpol3a);
K::GnuplotObjectPolygon* gpol3b = new K::GnuplotObjectPolygon(noFiller, stroke2);
gpol3b->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
gpol3b->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
gpol3b->setZIndex(bb.getMin().z + 1.1);
splot.getObjects().add(gpol3b);
K::GnuplotObjectPolygon* gpol3c = new K::GnuplotObjectPolygon(noFiller, stroke2);
gpol3c->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
gpol3c->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
gpol3c->setZIndex(bb.getMin().z + 1.1);
splot.getObjects().add(gpol3c);
K::GnuplotObjectPolygon* gpol3d = new K::GnuplotObjectPolygon(noFiller, stroke2);
gpol3d->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
gpol3d->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMax().z-2, K::GnuplotCoordinateSystem::FIRST));
gpol3d->setZIndex(bb.getMin().z + 1.1);
splot.getObjects().add(gpol3d);
}
void addBBox(const BBox3& bb) {
// // floor
// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z});
// mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMin().z});
// mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMin().z});
// mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMin().z});
// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMin().z});
// mapBBoxes.splitFace(); mapBBoxes.splitFace();
// // ceil
// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z});
// mapBBoxes.add({bb.getMax().x, bb.getMin().y, bb.getMax().z});
// mapBBoxes.add({bb.getMax().x, bb.getMax().y, bb.getMax().z});
// mapBBoxes.add({bb.getMin().x, bb.getMax().y, bb.getMax().z});
// mapBBoxes.add({bb.getMin().x, bb.getMin().y, bb.getMax().z});
// mapBBoxes.splitFace(); mapBBoxes.splitFace();
// // up
// mapBBoxes.addSegment({bb.getMin().x, bb.getMin().y, bb.getMin().z}, {bb.getMin().x, bb.getMin().y, bb.getMax().z});
// mapBBoxes.addSegment({bb.getMax().x, bb.getMin().y, bb.getMin().z}, {bb.getMax().x, bb.getMin().y, bb.getMax().z});
// mapBBoxes.addSegment({bb.getMin().x, bb.getMax().y, bb.getMin().z}, {bb.getMin().x, bb.getMax().y, bb.getMax().z});
// mapBBoxes.addSegment({bb.getMax().x, bb.getMax().y, bb.getMin().z}, {bb.getMax().x, bb.getMax().y, bb.getMax().z});
}
void addBBoxPoly(const BBox3& bb, K::GnuplotColor c) {
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMin().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
poly->add(K::GnuplotCoordinate3(bb.getMax().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
poly->add(K::GnuplotCoordinate3(bb.getMin().x, bb.getMax().y, bb.getMin().z, K::GnuplotCoordinateSystem::FIRST));
poly->close();
poly->setStroke(K::GnuplotStroke::NONE());
poly->setFill(K::GnuplotFill(K::GnuplotFillStyle::SOLID, c));
splot.getObjects().add(poly);
}
void setGroundTruth(const Point3 pos_m) {
gp << "set arrow 998 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n";
}
void setCurEst(const Point3 pos_m) {
gp << "set arrow 999 from " << pos_m.x << "," << pos_m.y << "," << pos_m.z << " to " << pos_m.x << "," << pos_m.y << "," << pos_m.z+1 << " front \n";
}
void setPaletteRedBlue() {
float max = -9999;
float min = +9999;
for (const auto& e : cpoints.get()) {
if (e.color > max) {max = e.color;}
if (e.color < min) {min = e.color;}
}
setPaletteRedBlue(min, max);
}
void setPaletteRedBlue(const float blueVal, const float redVal) {
// we need to map the range from [blueVal:redVal] to [0:1]
const float min = blueVal;
const float max = redVal;
const float range = (max - min);
const float center01 = (0-min)/range;
// values above 0 dB = red
// values below 0 dB = blue
gp << "set palette model RGB\n";
gp << "cen01 = " << center01 << "\n";
gp << "r(x) = (x < cen01) ? 0 : ((x-cen01) / (1-cen01))\n";
gp << "g(x) = 0\n";
gp << "b(x) = (x > cen01) ? 0 : (1 - (x/cen01))\n";
gp << "set palette model RGB functions r(gray),g(gray),b(gray)\n";
}
void addLabel(const std::string& txt, const Point3 pos) {
//gp << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << "\n";
splot.getCustom() << "set label '" << txt << "' at " << pos.x << "," << pos.y << "," << pos.z << " front\n";
}
void addRectangle(const Point3 p1, const Point3 p2, const Color c, bool front = false, bool fill = true) {
std::vector<Point3> points = {
Point3(p1.x, p1.y, p1.z),
Point3(p2.x, p1.y, p1.z),
Point3(p2.x, p2.y, p1.z),
Point3(p1.x, p2.y, p1.z),
Point3(p1.x, p1.y, p1.z),
};
addPolygon(points, c.toHEX(), front, fill);
}
void addRectangleW(const Point3 p1, const Point3 p2, const K::GnuplotColor c, const float w, bool front = false) {
std::vector<Point3> points = {
Point3(p1.x, p1.y, p1.z),
Point3(p2.x, p1.y, p1.z),
Point3(p2.x, p2.y, p1.z),
Point3(p1.x, p2.y, p1.z),
Point3(p1.x, p1.y, p1.z),
};
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
poly->getStroke().setWidth(w);
poly->getStroke().setColor(c);
poly->setFront(front);
for (const Point3 p : points) {
poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST));
}
splot.getObjects().add(poly);
}
K::GnuplotObjectPolygon* addStartIndicator(const Point3 pt, const std::string& color, const float s = 2) {
// for (const Point3 p : points) {
// if (p.z < settings.minZ) {return nullptr;}
// if (p.z > settings.maxZ) {return nullptr;}
// }
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon();
poly->setFill(K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr(color)));
poly->setStroke(K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromRGB(0,0,0)));
//poly->setStroke(K::GnuplotStroke::NONE());
poly->add(K::GnuplotCoordinate3(pt.x-s, pt.y-s, pt.z, K::GnuplotCoordinateSystem::FIRST));
poly->add(K::GnuplotCoordinate3(pt.x+s, pt.y-s, pt.z, K::GnuplotCoordinateSystem::FIRST));
poly->add(K::GnuplotCoordinate3(pt.x+s, pt.y+s, pt.z, K::GnuplotCoordinateSystem::FIRST));
poly->add(K::GnuplotCoordinate3(pt.x-s, pt.y+s, pt.z, K::GnuplotCoordinateSystem::FIRST));
poly->close();
poly->setFront(true);
splot.getObjects().add(poly);
return poly;
}
K::GnuplotObjectPolygon* addPolygon(const std::vector<Point3>& points, const std::string& color, bool front = false, bool fill = true, const float alpha = 1) {
for (const Point3 p : points) {
if (p.z < settings.minZ) {return nullptr;}
if (p.z > settings.maxZ) {return nullptr;}
}
const K::GnuplotFill pfill = (fill) ? (K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr(color))) : (K::GnuplotFill::NONE());
const K::GnuplotStroke pstroke = (!fill) ? (K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1.0, K::GnuplotColor::fromHexStr(color))) : (K::GnuplotStroke::NONE());
K::GnuplotObjectPolygon* poly = new K::GnuplotObjectPolygon(pfill, pstroke);
for (const Point3 p : points) {
poly->add(K::GnuplotCoordinate3(p.x, p.y, p.z, K::GnuplotCoordinateSystem::FIRST));
poly->setZIndex(p.z); // manual depth ordering
poly->getFill().setAlpha(alpha);
}
poly->setFront(front);
splot.getObjects().add(poly);
// gp << "set object polygon from ";
// for (size_t i = 0; i < points.size(); ++i) {
// const Point3 p = points[i];
// if (i > 0) {gp << " to ";}
// gp << p.x << "," << p.y << "," << p.z << " ";
// }
// gp << (front ? "front" : "");
// if (fill) {gp << " fs solid ";} else {gp << " fs transparent ";}
// gp << " fc rgb " << "'" << color << "'";
// gp << "\n";
return poly;
}
void setZRange(const float min, const float max) {
gp << "set zrange [" << min << ":" << max << "]\n";
}
K::GnuplotObjectPolygon* addFloorRect(const Point3 pos_m, const float size, Color c, float ratio = 1.0) {
const Point3 p1 = pos_m + Point3(-size, -size/ratio, 0);
const Point3 p2 = pos_m + Point3(+size, -size/ratio, 0);
const Point3 p3 = pos_m + Point3(+size, +size/ratio, 0);
const Point3 p4 = pos_m + Point3(-size, +size/ratio, 0);
std::vector<Point3> points = {p1,p2,p3,p4,p1};
return addPolygon(points, c.toHEX(), false, true);
// gp << "set object polygon from ";
// for (size_t i = 0; i < points.size(); ++i) {
// const Point3 p = points[i];
// if (i > 0) {gp << " to ";}
// gp << p.x << "," << p.y << "," << p.z << " ";
// }
// gp << "front fs solid fc rgb " << "'" << c.toHEX() << "'";
// gp << "\n";
}
template <typename T> void showParticles(const std::vector<T>& particles) {
this->particles.clear();
double min = +999;
double max = -999;
for (const T& p : particles) {
const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z);
const double prob = std::pow(p.weight, 0.25);
this->particles.add(p3, prob);
if (prob > max) {max = prob;}
if (prob < min) {min = prob;}
}
splot.getAxisCB().setRange(min, max + 0.000001);
}
// estimated path
void addEstimationNode(const Point3 pos){
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
pathEst.add(est);
}
void setTitle(const std::string& title) {
gp << "set title '" << title << "'\n";
}
void setGroundTruth(const std::vector<int> indices) {
const std::vector<Point3> path = FloorplanHelper::getGroundTruth(map, indices);
pathReal.clear();
for (const Point3& p : path) {
pathReal.add(K::GnuplotPoint3(p.x, p.y, p.z));
}
}
void equalXY() {
gp << "set view equal xy\n";
}
void setView(const float degX, const float degY) {
//gp << "set view " << degX << "," << degY << "\n";
splot.getView().setCamera(degX, degY);
}
void setScale(const float x, const float y, const float ox = 0, const float oy = 0) {
gp << "set multiplot layout 1,1 scale " << x << "," << y << " offset " << ox << "," << oy << "\n";
}
void writeCodeTo(const std::string& file) {
this->codeFile = file;
}
void noFrame() {
gp << "unset border\n";
// gp << "unset xtics\n";
// gp << "unset ytics\n";
// gp << "unset ztics\n";
splot.getAxisX().setTicsVisible(false);
splot.getAxisY().setTicsVisible(false);
splot.getAxisZ().setTicsVisible(false);
}
void writeEpsTex(const std::string file, K::GnuplotSize size = K::GnuplotSize(8.5, 5.1)) {
gp.setTerminal("epslatex", size);
gp.setOutput(file);
}
void plot() {
this->mapOutlineConcrete.getStroke().setColor(settings.outlineColor);
this->mapOutlineDrywall.getStroke().setColor(settings.outlineColor);
this->mapOutlineGlass.getStroke().setColor(settings.outlineColor);
gp.draw(splot);
gp << "unset multiplot\n"; // scaling
if (codeFile != "") {
std::ofstream out(codeFile);
out << gp.getBuffer();
out.close();
}
gp.flush();
}
void saveToFile(std::ofstream& stream){
gp.draw(splot);
stream << "set terminal x11 size 2000,1500\n";
stream << gp.getBuffer();
stream << "pause -1\n";
gp.flush();
}
void buildFloorplan() {
std::vector<Floorplan::Floor*> floors;
BBox3 bbox = FloorplanHelper::getBBox(map);
// only some floors??
if (settings.floors.empty()) {
floors = map->floors;
} else {
for (int i : settings.floors) {
floors.push_back(map->floors[i]);
}
}
// mapOutlineDrywall.addSegment(
// K::GnuplotPoint3(bbox.getMin().x, bbox.getMin().y, bbox.getMin().z),
// K::GnuplotPoint3(bbox.getMax().x, bbox.getMax().y, bbox.getMax().z)
// );
splot.getAxisX().setRange(K::GnuplotAxis::Range(bbox.getMin().x, bbox.getMax().x));
splot.getAxisY().setRange(K::GnuplotAxis::Range(bbox.getMin().y, bbox.getMax().y));
splot.getAxisZ().setRange(K::GnuplotAxis::Range(0, 11));
// process each selected floor
for (Floorplan::Floor* floor : floors) {
const float vo = floor->atHeight * 4.5;
// plot the floor's outline
if (settings.outline) {
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
if (floor->atHeight < settings.minZ) {continue;}
if (floor->atHeight > settings.maxZ) {continue;}
// for toni
if (settings.skipI1) {
if (floor->atHeight == 4) {
//if (poly->poly.points[2].y < 0) {
if (poly->poly.points[0].x > 70 && poly->poly.points[0].y < 50) {
continue;
}
}
}
const float v = 180 + vo;
K::GnuplotColor color = K::GnuplotColor::fromRGB(v,v,v);
if (poly->outdoor) {color = K::GnuplotColor::fromRGB(180, 240, 180);}
if (poly->method == Floorplan::OutlineMethod::REMOVE) {color = K::GnuplotColor::fromRGB(245,245,245);}
K::GnuplotFill filler(K::GnuplotFillStyle::SOLID, color);
K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, K::GnuplotStroke::NONE());
for (Point2 pt : poly->poly.points) {
K::GnuplotCoordinate3 coord(pt.x, pt.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST);
gpol->add(coord);
}
gpol->close();
gpol->setZIndex(floor->atHeight-0.1); // below the lines
//gpol->setFront(true);
splot.getObjects().add(gpol);
}
}
// plot obstacles?
if (settings.obstacles) {
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line) {
if (floor->atHeight < settings.minZ) {continue;}
if (floor->atHeight > settings.maxZ) {continue;}
// const K::GnuplotPoint3 p1(line->from.x, line->from.y, floor->atHeight);
// const K::GnuplotPoint3 p2(line->to.x, line->to.y, floor->atHeight);
// switch(line->material) {
// case Floorplan::Material::CONCRETE: mapOutlineConcrete.addSegment(p1, p2); break;
// case Floorplan::Material::GLASS: mapOutlineGlass.addSegment(p1, p2); break;
// case Floorplan::Material::UNKNOWN:
// case Floorplan::Material::DRYWALL: mapOutlineDrywall.addSegment(p1, p2); break;
// }
// K::GnuplotObjectArrow* arrow = new K::GnuplotObjectArrow(
// K::GnuplotCoordinate3(line->from.x, line->from.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST),
// K::GnuplotCoordinate3(line->to.x, line->to.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST)
// );
// arrow->setHead(K::GnuplotObjectArrow::Head::NONE);
// splot.getObjects().add(arrow);
const float v = 140 + vo;
// drawing outlines as polygon is a hack for correct depth-order in gnuplot
K::GnuplotColor color = (settings.outlineColorCustom) ? (settings.outlineColor) : (K::GnuplotColor::fromRGB(v,v,v));
K::GnuplotFill filler = K::GnuplotFill(K::GnuplotFillStyle::EMPTY_BORDER, color);
K::GnuplotStroke stroke(K::GnuplotDashtype::NONE, 6, color);
//K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(K::GnuplotFill::NONE(), stroke);
K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, stroke);
//K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(K::GnuplotFill::NONE(), K::GnuplotStroke::NONE());
gpol->add(K::GnuplotCoordinate3(line->from.x, line->from.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST));
gpol->add(K::GnuplotCoordinate3(line->to.x, line->to.y, floor->atHeight, K::GnuplotCoordinateSystem::FIRST));
gpol->close();
gpol->setZIndex(floor->atHeight); // above the ground polygon
//gpol->setFront(true);
splot.getObjects().add(gpol);
}
}
}
// plot the stairs as polygon
if (settings.stairs) {
for (Floorplan::Stair* s : floor->stairs) {
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(s->getParts(), floor);
for (const Floorplan::Quad3& q : quads) {
// K::GnuplotObjectPolygon* poly = addPolygon({q.p1, q.p2, q.p3, q.p4, q.p1}, "#c0c0c0");
// if (poly) {
// poly->setZIndex(floor->atHeight+1.5); // above the floor
// }
const float v1 = 180 + q.p1.z * 4.5;
const float v2 = 140 + q.p1.z * 4.5;
const float z = (q.p1.z + q.p2.z + q.p3.z + q.p4.z) / 4.0f;
if (z < settings.minZ) {continue;}
if (z > settings.maxZ) {continue;}
K::GnuplotColor color = K::GnuplotColor::fromRGB(v1,v1,v1);
K::GnuplotColor color2 = K::GnuplotColor::fromRGB(v2,v2,v2);
K::GnuplotFill filler(K::GnuplotFillStyle::SOLID, color);
K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, color2);
K::GnuplotObjectPolygon* gpol = new K::GnuplotObjectPolygon(filler, stroke);
gpol->add(K::GnuplotCoordinate3(q.p1.x, q.p1.y, q.p1.z, K::GnuplotCoordinateSystem::FIRST));
gpol->add(K::GnuplotCoordinate3(q.p2.x, q.p2.y, q.p2.z, K::GnuplotCoordinateSystem::FIRST));
gpol->add(K::GnuplotCoordinate3(q.p3.x, q.p3.y, q.p3.z, K::GnuplotCoordinateSystem::FIRST));
gpol->add(K::GnuplotCoordinate3(q.p4.x, q.p4.y, q.p4.z, K::GnuplotCoordinateSystem::FIRST));
gpol->close();
gpol->setZIndex(z); // above the ground
splot.getObjects().add(gpol);
}
}
}
}
}
};
#endif // PLOTTY_H

106
Settings.h
View File

@@ -9,7 +9,7 @@ namespace Settings {
bool useKLB = false; bool useKLB = false;
const int numParticles = 6000; const int numParticles = 5000;
const int numBSParticles = 50; const int numBSParticles = 50;
namespace IMU { namespace IMU {
@@ -83,16 +83,104 @@ namespace Settings {
constexpr bool useMainThread = false; // perform filtering in the main thread constexpr bool useMainThread = false; // perform filtering in the main thread
} }
namespace Path_DongleTest { const std::string mapDir = "../map/";
const std::vector<int> path1 = {0, 1, 2, 3, 4, 5, 6}; const std::string dataDir = "../measurements/";
const std::vector<int> path2 = {6, 5, 4, 7, 8, 9, 8, 10}; const std::string errorDir = dataDir + "results/";
const std::vector<int> path3 = {10, 8, 7, 4, 11, 12, 13, 14, 6};
const std::vector<int> path4 = {0, 1, 2, 3, 4, 5, 6, 71, 7, 8, 9, 10};
const std::vector<int> path5 = {40, 41, 85, 117, 31, 32, 33, 34, 35, 36, 37, 12, 3, 4, 5, 6, 71};
const std::vector<int> path6 = {28, 27, 26, 25, 24, 23, 22, 21, 20, 1, 2, 3, 4, 6, 5};
} /** describes one dataset (map, training, parameter-estimation, ...) */
struct DataSetup {
std::string map;
std::vector<std::string> training;
std::string fingerprints;
std::string wifiModel;
int numGTPoints;
};
/** all configured datasets */
struct Data {
DataSetup Path0 = {
mapDir + "map42_ap_path0.xml",
{
dataDir + "museum/Nexus/Path0_4113.csv",
dataDir + "museum/Nexus/Path0_6563.csv",
dataDir + "museum/Pixel/Path0_1498.csv",
dataDir + "museum/Pixel/Path0_2529.csv",
dataDir + "museum/Pixel/Path0_7380.csv",
//dataDir + "museum/Samsung/Path0_4208.csv",
//dataDir + "museum/Samsung/Path0_6812.csv",
},
dataDir + "museum/Nexus/fingerprints/wifi_fp.dat",
dataDir + "museum/wifimodel.dat",
15
};
DataSetup Path1 = {
mapDir + "map42_ap_path1_hofzu.xml",
{
dataDir + "museum/Nexus/Path1_1548.csv",
dataDir + "museum/Nexus/Path1_9477.csv",
dataDir + "museum/Pixel/Path1_2468.csv",
dataDir + "museum/Pixel/Path1_5497.csv",
dataDir + "museum/Pixel/Path1_8787.csv",
dataDir + "museum/Samsung/Path1_5745.csv",
dataDir + "museum/Samsung/Path1_9130.csv"
},
dataDir + "museum/Nexus/fingerprints/wifi_fp.dat",
dataDir + "museum/wifimodel.dat",
32
};
DataSetup Path2 = {
mapDir + "map42_ap_path2.xml",
{
dataDir + "museum/Nexus/Path2_0374.csv",
dataDir + "museum/Nexus/Path2_4718.csv",
dataDir + "museum/Nexus/Path2_8766.csv",
dataDir + "museum/Pixel/Path2_4336.csv",
dataDir + "museum/Pixel/Path2_4341.csv",
dataDir + "museum/Pixel/Path2_9038.csv",
//dataDir + "museum/Samsung/Path2_3326.csv",
//dataDir + "museum/Samsung/Path2_5814.csv"
},
dataDir + "museum/Nexus/fingerprints/wifi_fp.dat",
dataDir + "museum/wifimodel.dat",
44
};
DataSetup Path3 = {
mapDir + "map42_ap_path3.xml",
{
dataDir + "museum/Nexus/Path3_4519.csv",
dataDir + "museum/Nexus/Path3_5929.csv",
dataDir + "museum/Pixel/Path3_6307.csv",
dataDir + "museum/Pixel/Path3_5451.csv",
dataDir + "museum/Pixel/Path3_9243.csv",
//dataDir + "museum/Samsung/Path3_7610.csv",
//dataDir + "museum/Samsung/Path3_7819.csv"
},
dataDir + "museum/Nexus/fingerprints/wifi_fp.dat",
dataDir + "museum/wifimodel.dat",
45
};
} data;
} }
#endif // SETTINGS_H #endif // SETTINGS_H

179
filter/KLB.h
View File

@@ -1,179 +0,0 @@
#ifndef KLB_H
#define KLB_H
#include <chrono>
#include <Indoor/math/divergence/KullbackLeibler.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/grid/factory/v2/GridFactory.h>
#include <Indoor/grid/factory/v2/Importance.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/sensors/radio/WiFiGridEstimator.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/math/MovingAVG.h>
#include <Indoor/math/FixedFrequencyInterpolator.h>
#include <Indoor/math/divergence/KullbackLeibler.h>
#include <Indoor/math/divergence/JensenShannon.h>
#include <Indoor/data/Timestamp.h>
//#include <KLib/math/statistics/Statistics.h>
#include <Indoor/smc/Particle.h>
#include <Indoor/smc/filtering/ParticleFilterMixing.h>
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <Indoor/smc/filtering/ParticleFilterHistory.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingDivergence.h>
#include <Indoor/smc/merging/MarkovTransitionProbability.h>
#include <Indoor/smc/merging/mixing/MixingSamplerDivergency.h>
#include <Indoor/smc/merging/estimation/JointEstimationPosteriorOnly.h>
//#include <Indoor/smc/smoothing/BackwardSimulation.h>
//#include <Indoor/smc/CondensationBackwardFilter.h>
//#include <Indoor/smc/smoothing/sampling/ParticleTrajectorieSampler.h>
//#include <Indoor/smc/smoothing/sampling/CumulativeSampler.h>
#include <Indoor/smc/smoothing/BackwardFilterTransition.h>
#include "Structs.h"
#include "../Plotti.h"
#include "Logic.h"
#include "../Settings.h"
static double getKernelDensityProbability(std::vector<SMC::Particle<MyState>>& particles, MyState state, std::vector<SMC::Particle<MyState>>& samplesWifi){
Distribution::KernelDensity<double, MyState> parzen([&](MyState state){
int size = particles.size();
double prob = 0;
#pragma omp parallel for reduction(+:prob) num_threads(6)
for(int i = 0; i < size; ++i){
double distance = particles[i].state.position.getDistanceInCM(state.position);
prob += Distribution::Normal<double>::getProbability(0, 100, distance) * particles[i].weight;
}
return prob;
;});
std::vector<double> probsWifiV;
std::vector<double> probsParticleV;
//just for plottingstuff
std::vector<SMC::Particle<MyState>> samplesParticles;
const int step = 4;
int i = 0;
for(SMC::Particle<MyState> particle : samplesWifi){
if(++i % step != 0){continue;}
MyState state(GridPoint(particle.state.position.x_cm, particle.state.position.y_cm, particle.state.position.z_cm));
double probiParticle = parzen.getProbability(state);
probsParticleV.push_back(probiParticle);
double probiwifi = particle.weight;
probsWifiV.push_back(probiwifi);
//samplesParticles.push_back(SMC::Particle<MyState>(state, probiParticle));
}
//make vectors
Eigen::Map<Eigen::VectorXd> probsWifi(&probsWifiV[0], probsWifiV.size());
Eigen::Map<Eigen::VectorXd> probsParticle(&probsParticleV[0], probsParticleV.size());
//get divergence
double kld = Divergence::KullbackLeibler<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
//double kld = Divergence::JensenShannon<double>::getGeneralFromSamples(probsParticle, probsWifi, Divergence::LOGMODE::NATURALIS);
//plotti
//plot.debugDistribution1(samplesWifi);
//plot.debugDistribution1(samplesParticles);
//estimate the mean
// SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
// const MyState estWifi = estimateWifi.estimate(samplesWifi);
// plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
return kld;
}
static double kldFromMultivariatNormal(std::vector<SMC::Particle<MyState>>& particles, MyState state, std::vector<SMC::Particle<MyState>>& particleWifi){
//kld: particle die resampling hatten nehmen und nv daraus schätzen. vergleiche mit wi-fi
//todo put this in depletionhelper.h
Point3 estPos = state.position.inMeter();
//this is a hack! it is possible that the sigma of z is getting 0 and therefore the rank decreases to 2 and
//no inverse matrix is possible
std::mt19937_64 rng;
// initialize the random number generator with time-dependent seed
uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed>>32)};
rng.seed(ss);
// initialize a uniform distribution between -0.0001 and 0.0001
std::uniform_real_distribution<double> unif(-0.0001, 0.0001);
//create a gauss dist for the current particle approx.
Eigen::MatrixXd m(particles.size(), 3);
for(int i = 0; i < particles.size(); ++i){
m(i,0) = (particles[i].state.position.x_cm / 100.0) + unif(rng);
m(i,1) = (particles[i].state.position.y_cm / 100.0) + unif(rng);
m(i,2) = (particles[i].state.position.z_cm / 100.0) + unif(rng);
}
Eigen::VectorXd mean(3);
mean << estPos.x, estPos.y, estPos.z;
Distribution::NormalDistributionN normParticle = Distribution::NormalDistributionN::getNormalNFromSamplesAndMean(m, mean);
//create a gauss dist for wifi
Eigen::MatrixXd covWifi(3,3);
covWifi << Settings::WiFiModel::sigma, 0, 0,
0, Settings::WiFiModel::sigma, 0,
0, 0, 0.01;
//estimate the mean
SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState> estimateWifi(0.95);
const MyState estWifi = estimateWifi.estimate(particleWifi);
Eigen::VectorXd meanWifi(3);
meanWifi << estWifi.position.x_cm / 100.0, estWifi.position.y_cm / 100.0, estWifi.position.z_cm / 100.0;
Distribution::NormalDistributionN normWifi(meanWifi, covWifi);
//get the kld distance
double kld = Divergence::KullbackLeibler<double>::getMultivariateGauss(normParticle, normWifi);
//plot.debugDistribution1(particleWifi);
//plot.drawNormalN1(normParticle);
//plot.drawNormalN2(normWifi);
//plot.addEstimationNodeSmoothed(estWifi.position.inMeter());
return kld;
}
#endif // KLB_H

561
filter/Logic.h
View File

@@ -1,561 +0,0 @@
#ifndef FLOGIC_H
#define FLOGIC_H
#include <Indoor/grid/Grid.h>
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/GridWalkerMulti.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFollowDestination.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingControl.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeadingVonMises.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleNodeImportance.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
#include <Indoor/sensors/radio/WiFiQualityAnalyzer.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/radio/WiFiProbabilityGrid.h>
#include <Indoor/sensors/beacon/model/BeaconModelLogDistCeiling.h>
#include <Indoor/sensors/beacon/BeaconProbabilityFree.h>
#include <Indoor/sensors/activity/ActivityDetector.h>
#include <Indoor/smc/filtering/ParticleFilterMixing.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/smoothing/BackwardFilterTransition.h>
#include <Indoor/smc/smoothing/BackwardSimulation.h>
#include <Indoor/smc/sampling/CumulativeSampler.h>
#include "Structs.h"
#include <omp.h>
#include "../Settings.h"
/** particle-filter init randomly distributed within the building*/
struct PFInit : public SMC::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
PFInit(Grid<MyNode>& grid) : grid(grid) {;}
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
for (SMC::Particle<MyState>& p : particles) {
int idx = rand() % grid.getNumNodes();
p.state.position = grid[idx]; // random position
p.state.heading.direction = (rand() % 360) / 180.0 * M_PI; // random heading
p.state.heading.error = 0;
p.state.relativePressure = 0; // start with a relative pressure of 0
p.weight = 1.0 / particles.size(); // equal weight
}
}
};
/** particle-filter init with fixed position*/
struct PFInitFixed : public SMC::ParticleFilterInitializer<MyState> {
Grid<MyNode>& grid;
GridPoint startPos;
float headingDeg;
PFInitFixed(Grid<MyNode>& grid, GridPoint startPos, float headingDeg) :
grid(grid), startPos(startPos), headingDeg(headingDeg) {;}
virtual void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
Distribution::Normal<float> norm(0.0f, 1.5f);
for (SMC::Particle<MyState>& p : particles) {
GridPoint pos = startPos + GridPoint(norm.draw(),norm.draw(),0.0f);
GridPoint startPos = grid.getNodeFor(pos);
p.state.position = startPos; // scatter arround the start position
p.state.heading.direction = headingDeg / 180.0 * M_PI; // fixed heading
p.state.heading.error = 0;
p.state.relativePressure = 0; // start with a relative pressure of 0
p.weight = 1.0 / particles.size(); // equal weight
}
}
};
/** very simple transition model, just scatter normal distributed */
struct PFTransSimple : public SMC::ParticleFilterTransition<MyState, MyControl>{
Grid<MyNode>& grid;
// define the noise
Distribution::Normal<float> noise_cm = Distribution::Normal<float>(0.0, Settings::IMU::stepLength * 2.0 * 100.0);
Distribution::Normal<float> height_m = Distribution::Normal<float>(0.0, 6.0);
// draw randomly from a vector
//random_selector<> rand;
// draw from 0 - 1
Distribution::Uniform<float> uniRand = Distribution::Uniform<float>(0,1);
/** ctor */
PFTransSimple(Grid<MyNode>& grid) : grid(grid) {}
virtual void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
//int noNewPositionCounter = 0;
#pragma omp parallel for num_threads(6)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
// update the baromter
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
p.state.relativePressure += deltaZ_cm * 0.105f;
double diffHeight = p.state.position.inMeter().z + height_m.draw();
double newHeight_cm = p.state.position.z_cm;
if(diffHeight > 9.1){
newHeight_cm = 10.8 * 100.0;
} else if (diffHeight < 9.1 && diffHeight > 5.7){
newHeight_cm = 7.4 * 100.0;
} else if (diffHeight < 5.7 && diffHeight > 2.0) {
newHeight_cm = 4.0 * 100.0;
} else {
newHeight_cm = 0.0;
}
GridPoint noisePt(noise_cm.draw(), noise_cm.draw(), 0.0);
GridPoint newPosition = p.state.position + noisePt;
newPosition.z_cm = newHeight_cm;
// p.state.position = grid.getNearestNode(newPosition);
if(grid.hasNodeFor(newPosition)){
p.state.position = newPosition;
}else{
//no new position!
// #pragma omp atomic
// noNewPositionCounter++;
}
}
// std::cout << noNewPositionCounter << std::endl;
}
};
/** particle-filter transition */
struct PFTrans : public SMC::ParticleFilterTransition<MyState, MyControl> {
Grid<MyNode>& grid;
GridWalker<MyNode, MyState> walker;
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
WalkModuleHeadingVonMises<MyNode, MyState, MyControl> modHeadMises;
WalkModuleNodeImportance<MyNode, MyState> modImportance;
WalkModuleSpread<MyNode, MyState> modSpread;
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
//WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
//WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;
std::minstd_rand gen;
PFTrans(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
walker.addModule(&modHead);
//walker.addModule(&modHeadMises); // fürn arsch und geht net
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modActivity);
//walker.addModule(&modHeadUgly);
//walker.addModule(&modImportance);
//walker.addModule(&modFavorZ);
//walker.addModule(&modButterAct);
//walker.addModule(&modWifi);
//walker.addModule(&modPreventVisited);
}
virtual void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
for (SMC::Particle<MyState>& p : particles) {
//this is just for the smoothing transition... quick and dirty
p.state.headingChangeMeasured_rad = control->turnSinceLastTransition_rad;
// save old position
p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
// update steps
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
// update the particle in-place
p.state = walker.getDestination(grid, p.state, dist_m);
// update the baromter
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
p.state.relativePressure += deltaZ_cm * 0.105f;
}
}
};
/**
* particle-filter transition
* Adapting the Sample Size in Particle Filters Through KLD-Sampling - D. Fox
*/
struct PFTransKLDSampling : public SMC::ParticleFilterTransition<MyState, MyControl> {
Grid<MyNode>& grid;
GridWalker<MyNode, MyState> walker;
WalkModuleHeading<MyNode, MyState> modHeadUgly; // stupid
WalkModuleHeadingControl<MyNode, MyState, MyControl> modHead;
WalkModuleHeadingVonMises<MyNode, MyState, MyControl> modHeadMises;
WalkModuleNodeImportance<MyNode, MyState> modImportance;
WalkModuleSpread<MyNode, MyState> modSpread;
WalkModuleFavorZ<MyNode, MyState> modFavorZ;
//WalkModulePreventVisited<MyNode, MyState> modPreventVisited;
//WalkModuleActivityControl<MyNode, MyState, MyControl> modActivity;
std::minstd_rand gen;
/** upper bound epsilon of the kld distance - the particle size is not allowed to exceed epsilon*/
double epsilon;
/** the upper 1 - delta quantil of the normal distribution. something like 0.01 */
double delta;
/** the bins */
Binning<MyState> bins;
/** max particle size */
uint32_t N_max;
PFTransKLDSampling(Grid<MyNode>& grid, MyControl* ctrl) : grid(grid), modHead(ctrl, Settings::IMU::turnSigma), modHeadMises(ctrl, Settings::IMU::turnSigma) {//, modPressure(ctrl, 0.100) {
//walker.addModule(&modHead);
walker.addModule(&modHeadMises);
//walker.addModule(&modSpread); // might help in some situations! keep in mind!
//walker.addModule(&modActivity);
//walker.addModule(&modHeadUgly);
//walker.addModule(&modImportance);
//walker.addModule(&modFavorZ);
//walker.addModule(&modButterAct);
//walker.addModule(&modWifi);
//walker.addModule(&modPreventVisited);
epsilon = 0.15;
delta = 0.01;
N_max = 5000;
bins.setBinSizes({0.01, 0.01, 0.2, 0.3});
bins.setRanges({BinningRange(-1,100), BinningRange(-10,60), BinningRange(-1,15), BinningRange(0, 2 * M_PI)});
}
virtual void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
std::normal_distribution<float> noise(0, Settings::IMU::stepSigma);
Distribution::Uniform<int> getParticle(0, particles.size()-1);
//init stuff
uint32_t n = 0;
uint32_t k = 1;
double N = 0;
//clear the bins
bins.clearUsed();
//create new particle set
std::vector<SMC::Particle<MyState>> particlesNew;
do{
//draw equally from the particle set
int particleIdx = getParticle.draw();
SMC::Particle<MyState>& p = particles[particleIdx];
//sample new particles based on the transition step
// save old position
p.state.positionOld = p.state.position; //GridPoint(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
// update steps
const float dist_m = std::abs(control->numStepsSinceLastTransition * Settings::IMU::stepLength + noise(gen));
// update the particle in-place
p.state = walker.getDestination(grid, p.state, dist_m);
// update the baromter
float deltaZ_cm = p.state.positionOld.inMeter().z - p.state.position.inMeter().z;
p.state.relativePressure += deltaZ_cm * 0.105f;
//if it falls into an empty bin then draw another particle
//is bin free?
if(bins.isFree(p.state)){
k++;
bins.markUsed(p.state);
//calculate the new N
double z_delta = Distribution::NormalCDF<double>::getProbit(1 - delta);
double front = (k - 1) / (2 * epsilon);
double back = 1 - (2 / (9 * (k - 1))) + (std::sqrt(2 / (9 * (k - 1))) * z_delta );
N = front * std::pow(back, 3.0);
}
++n;
//add particle to new particleset
particlesNew.push_back(p);
} while (n < N && n < N_max);
//write new particleset
particles.clear();
particles.swap(particlesNew);
}
};
struct BFTrans : public SMC::BackwardFilterTransition<MyState, MyControl>{
public:
/**
* ctor
* @param choice the choice to use for randomly drawing nodes
* @param fp the underlying floorplan
*/
BFTrans()
{
//nothin
}
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 = this->ts - ts;
this->ts = ts;
}
}
std::vector<SMC::Particle<MyState>> transition(std::vector<SMC::Particle<MyState>> const& toBeSmoothedParticles_qt, std::vector<MyControl> const& controls_1T) override{
Assert::doThrow( "Wrong transition function. Use the other one!");
std::vector<SMC::Particle<MyState>> dummyReturn;
return dummyReturn;
}
/**
* smoothing transition starting at T with t, t-1,...0
* @param particles_qt q_t (Forward Filter) p2
* @param particles_qt1 q_t+1 (Smoothed Particles from Step before) p1
*/
std::vector<std::vector<double>> transition(std::vector<SMC::Particle<MyState>>const& particles_qt,
std::vector<SMC::Particle<MyState>>const& particles_qt1) override {
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
// this means, predict all possible states q_t+1 with all passible states q_t
// e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
// p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
std::vector<std::vector<double>> predictionProbabilities;
omp_set_dynamic(0); // Explicitly disable dynamic teams
omp_set_num_threads(7);
#pragma omp parallel for shared(predictionProbabilities)
for (int i = 0; i < particles_qt1.size(); ++i) {
std::vector<double> innerVector;
auto p1 = &particles_qt1[i];
for(int j = 0; j < particles_qt.size(); ++j){
auto p2 = &particles_qt[j];
const double distance_m = p2->state.position.inMeter().getDistance(p1->state.position.inMeter()) / 100.0;
//TODO Incorporated Activity - see IPIN16 MySmoothingTransitionExperimental
const double distProb = Distribution::Normal<double>::getProbability(Settings::Smoothing::stepLength, Settings::Smoothing::stepSigma, distance_m);
// TODO: FIX THIS CORRECTLY is the heading change similiar to the measurement?
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.heading.direction.getRAD(), p1->state.heading.direction.getRAD());
double diffDeg = Angle::radToDeg(diffRad);
double measurementRad = Angle::makeSafe_2PI(p1->state.headingChangeMeasured_rad);
double measurementDeg = Angle::radToDeg(measurementRad);
const double headingProb = Distribution::Normal<double>::getProbability(measurementDeg, Settings::Smoothing::headingSigma, diffDeg);
// does the angle between two particles positions is similiar to the measurement
//double angleBetweenParticles = Angle::getDEG_360(p2->state.position.x, p2->state.position.y, p1->state.position.x, p1->state.position.y);
//check how near we are to the measurement
double diffZ = (p2->state.position.inMeter().z - p1->state.position.inMeter().z) / 100.0;
const double floorProb = Distribution::Normal<double>::getProbability(Settings::Smoothing::zChange, Settings::Smoothing::zSigma, diffZ);
//combine the probabilities
double prob = distProb;// * floorProb * headingProb;
innerVector.push_back(prob);
}
#pragma omp critical
predictionProbabilities.push_back(innerVector);
}
return predictionProbabilities;
}
};
struct PFEval : public SMC::ParticleFilterEvaluation<MyState, MyObs> {
WiFiModel& wifiModel;
WiFiObserverFree wiFiProbability; // free-calculation
//WiFiObserverGrid<MyNode> wiFiProbability; // grid-calculation
WiFiQualityAnalyzer wqa;
BeaconModelLogDistCeiling& beaconModel;
BeaconObserverFree beaconProbability;
Grid<MyNode>& grid;
PFEval(WiFiModel& wifiModel, BeaconModelLogDistCeiling& beaconModel, Grid<MyNode>& grid) :
wifiModel(wifiModel),
beaconModel(beaconModel),
grid(grid),
wiFiProbability(Settings::WiFiModel::sigma, wifiModel),
beaconProbability(Settings::BeaconModel::sigma, beaconModel){
}
/** probability step-distance */
//TODO: add number of recognized steps
inline double getStepDistanceProb(const Point3 particle1, const Point3 particle2){
double distance = particle1.getDistance(particle2);
return Distribution::Normal<double>::getProbability(Settings::IMU::stepLength, Settings::IMU::stepSigma + 0.4, distance);
}
//TODO: combinied evaluation heading and distance
/** probability for WIFI */
inline double getWIFI(const MyObs& observation, const WiFiMeasurements& vapWifi, const GridPoint& point) const {
const MyNode& node = grid.getNodeFor(point);
return wiFiProbability.getProbability(node, observation.currentTime, vapWifi);
}
/** probability for BEACONS */
inline double getBEACON(const MyObs& observation, const GridPoint& point){
//consider adding the persons height
Point3 p = point.inMeter() + Point3(0,0,1.3);
return beaconProbability.getProbability(p, observation.currentTime, observation.beacons);
}
/** probability for Barometer */
inline double getBaroPressure(const MyObs& observation, const float hPa) const{
return Distribution::Normal<double>::getProbability(static_cast<double>(hPa), 0.10, static_cast<double>(observation.relativePressure));
}
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.65;
const MyNode& gn = grid.getNodeFor(p.state.position);
switch (act) {
case Activity::WALKING:
if (gn.getType() == GridNode::TYPE_FLOOR) {return kappa;}
if (gn.getType() == GridNode::TYPE_DOOR) {return kappa;}
{return 1-kappa;}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (gn.getType() == GridNode::TYPE_STAIR) {return kappa;}
if (gn.getType() == GridNode::TYPE_ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
}
virtual double evaluation(std::vector<SMC::Particle<MyState>>& particles, const MyObs& observation) override {
double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
wqa.add(wifiObs);
float quality = wqa.getQuality();
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
Point3 pos_m = p.state.position.inMeter();
Point3 posOld_m = p.state.positionOld.inMeter();
double pWifi = getWIFI(observation, wifiObs, p.state.position);
const double pStairProb = getStairProb(p, observation.activity);
const double pStepDistance = getStepDistanceProb(pos_m, posOld_m);
const double pBaroPressure = getBaroPressure(observation, p.state.relativePressure);
//const double pBeacon = getBEACON(observation, p.state.position);
//small checks
Assert::isNotNaN(pWifi, "Wifi prob is nan");
Assert::isNot0(pBaroPressure,"pBaroPressure is null");
const bool volatile init = observation.currentTime.sec() < 25;
//double pWiFiMod = (init) ? (std::pow(pWiFi, 0.1)) : (std::pow(pWiFi, 0.5));
//double pWiFiMod = (init) ? (std::pow(pWifi, 0.5)) : (std::pow(pWifi, 0.9));
// bad wifi? -> we have no idea where we are!
if (quality < 0.25 && !init) {
//pWifi = 1;
//p.weight = std::pow(p.weight, 0.5);
}
const double prob = pWifi;// * pStairProb;
p.weight = prob;
#pragma omp atomic
sum += (prob);
}
if(sum == 0.0){
return 1.0;
}
return sum;
}
};
#endif // FLOGIC_H

136
filter/Structs.h
View File

@@ -1,136 +0,0 @@
#ifndef FSTRUCTS_H
#define FSTRUCTS_H
#include <Indoor/grid/Grid.h>
#include <Indoor/sensors/radio/WiFiGridNode.h>
#include <Indoor/math/Distributions.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/beacon/BeaconMeasurements.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
#include <Indoor/grid/factory/v2/GridNodeImportance.h>
#include <Indoor/math/distribution/KernelDensity.h>
#include <Indoor/sensors/activity/Activity.h>
#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleActivityControl.h>
struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ {
static Floorplan::IndoorMap* map;
float relativePressure = 0.0f;
GridPoint positionOld;
float headingChangeMeasured_rad;
MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}
MyState& operator += (const MyState& o) {
this->position += o.position;
return *this;
}
MyState& operator /= (const double d) {
this->position /= d;
return *this;
}
MyState operator * (const double d) const {
return MyState(this->position*d);
}
bool belongsToRegion(const MyState& o) const {
return position.inMeter().getDistance(o.position.inMeter()) < 3.0;
}
float getBinValue(const int dim) const {
switch (dim) {
case 0: return this->position.x_cm / 100.0;
case 1: return this->position.y_cm / 100.0;
case 2: return this->position.z_cm / 100.0;
case 3: return this->heading.direction.getRAD();
}
throw "cant find this value within the bin";
}
};
struct MyControl {
/** turn angle (in radians) since the last transition */
float turnSinceLastTransition_rad = 0;
float sumTurn_rad = 0;
/** number of steps since the last transition */
int numStepsSinceLastTransition = 0;
/** current motion delta angle*/
float motionDeltaAngle_rad = 0;
float sumMotion_rad = 0;
/** reset the control-data after each transition */
void resetAfterTransition() {
turnSinceLastTransition_rad = 0;
numStepsSinceLastTransition = 0;
motionDeltaAngle_rad = 0;
}
};
struct MyObs {
/** relative pressure since t_0 */
float relativePressure = 0;
/** current estimated sigma for pressure sensor */
float sigmaPressure = 0.10f;
/** wifi measurements */
WiFiMeasurements wifi;
/** detected activity */
Activity activity = Activity::STANDING;
/** beacon measurements */
BeaconMeasurements beacons;
/** gps measurements */
//GPSData gps;
/** time of evaluation */
Timestamp currentTime;
};
struct MyNode : public GridPoint, public GridNode, public GridNodeImportance, public WiFiGridNode<20> {
float navImportance;
float getNavImportance() const { return navImportance; }
float walkImportance;
float getWalkImportance() const { return walkImportance; }
/** empty ctor */
MyNode() : GridPoint(-1, -1, -1) {;}
/** ctor */
MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}
static void staticDeserialize(std::istream& inp) {
WiFiGridNode::staticDeserialize(inp);
}
static void staticSerialize(std::ostream& out) {
WiFiGridNode::staticSerialize(out);
}
};
#endif // FSTRUCTS_H

300
main.cpp
View File

@@ -1,8 +1,304 @@
#include "main.h"
#include "navMesh/main.h" #include "navMesh/mesh.h"
#include "navMesh/filter.h"
#include "Settings.h"
#include "navMesh/meshPlotter.h"
#include "Plotty.h"
#include <memory>
#include <thread>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
#include <Indoor/math/stats/Statistics.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h>
#include <sys/stat.h>
Stats::Statistics<float> run(Settings::DataSetup setup, int numFile, std::string folder) {
// reading file
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
Offline::FileReader fr(setup.training[numFile]);
WiFiFingerprints fingerprints(setup.fingerprints);
std::ifstream inp(setup.wifiModel, std::ifstream::binary);
//ground truth
std::vector<int> gtPath;
for(int i = 0; i < setup.numGTPoints; ++i){gtPath.push_back(i);}
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
Stats::Statistics<float> errorStats;
//error file
const long int t = static_cast<long int>(time(NULL));
const std::string evalDir = Settings::errorDir + folder;
struct stat statStruct;
stat(evalDir.c_str(), &statStruct);
if(!S_ISDIR(statStruct.st_mode)){
if(mkdir(evalDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1){
Assert::doThrow("Eval folder couldn't be created!");
}
}
std::ofstream errorFile;
errorFile.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");
// wifi
WiFiModelLogDistCeiling WiFiModel(map);
// with optimization
if(Settings::WiFiModel::optimize){
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp);
}
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
WiFiModel.addAP(ap.mac, entry);
}
WiFiModel.saveXML(setup.wifiModel);
} else {
WiFiModel.loadXML(setup.wifiModel);
}
} else {
// without optimization
WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
}
// mesh
NM::NavMeshSettings set;
MyNavMesh mesh;
MyNavMeshFactory fac(&mesh, set);
fac.build(map);
const Point3 srcPath0(26, 43, 7.5);
const Point3 srcPath1(62, 38, 1.7);
//const Point3 srcPath2(62, 38, 1.8);
//const Point3 srcPath3(62, 38, 1.8);
// add shortest-path to destination
//const Point3 dst(51, 45, 1.7);
//const Point3 dst(25, 45, 0);
//NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);
// debug show
//MeshPlotter dbg;
//dbg.addFloors(map);
//dbg.addOutline(map);
//dbg.addMesh(mesh);
//dbg.addDijkstra(mesh);
//dbg.draw();
Plotty plot(map);
plot.buildFloorplan();
plot.setGroundTruth(gtPath);
// particle-filter
const int numParticles = 5000;
auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
//auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>(WiFiModel);
auto trans = std::make_unique<MyPFTrans>(mesh);
//auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimpleImpoverishment<MyState, MyNavMeshTriangle>>();
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationBoxKDE<MyState>>(map, 0.2, Point2(1,1));
//auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationMax<MyState>>();
// setup
MyFilter pf(numParticles, std::move(init));
pf.setEvaluation(std::move(eval));
pf.setTransition(std::move(trans));
pf.setResampling(std::move(resample));
pf.setEstimation(std::move(estimate));
pf.setNEffThreshold(0.85);
// sensors
MyControl ctrl;
MyObservation obs;
StepDetection sd;
PoseDetection pd;
TurnDetection td(&pd);
RelativePressure relBaro;
ActivityDetector act;
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
Timestamp lastTimestamp = Timestamp::fromMS(0);
// parse each sensor-value within the offline data
for (const Offline::Entry& e : fr.getEntries()) {
const Timestamp ts = Timestamp::fromMS(e.ts);
if (e.type == Offline::Sensor::WIFI) {
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == Offline::Sensor::ACC) {
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
++ctrl.numStepsSinceLastEval;
}
const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
pd.addAccelerometer(ts, _acc.data);
//simpleActivity walking / standing
act.add(ts, fr.getAccelerometer()[e.idx].data);
} else if (e.type == Offline::Sensor::GYRO) {
const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
const float delta_gyro = td.addGyroscope(ts, _gyr.data);
ctrl.headingChangeSinceLastEval += delta_gyro;
} else if (e.type == Offline::Sensor::BARO) {
relBaro.add(ts, fr.getBarometer()[e.idx].data);
obs.relativePressure = relBaro.getPressureRealtiveToStart();
obs.sigmaPressure = relBaro.getSigma();
//simpleActivity stairs up / down
act.add(ts, fr.getBarometer()[e.idx].data);
obs.activity = act.get();
}
if (ctrl.numStepsSinceLastEval > 0) {
obs.currentTime = ts;
// if(ctrl.numStepsSinceLastEval > 0){
// pf.updateTransitionOnly(&ctrl);
// }
MyState est = pf.update(&ctrl, obs); //pf.updateEvaluationOnly(obs);
ctrl.afterEval();
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms())) + Point3(0,0,0.1);
lastTimestamp = ts;
//plot
//dbg.showParticles(pf.getParticles());
//dbg.setCurPos(est.pos.pos);
//dbg.setGT(gtPos);
//dbg.addEstimationNode(est.pos.pos);
//dbg.addGroundTruthNode(gtPos);
//dbg.setTimeInMinute(static_cast<int>(ts.sec()) / 60, static_cast<int>(static_cast<int>(ts.sec())%60));
//dbg.draw();
plot.showParticles(pf.getParticles());
plot.setCurEst(est.pos.pos);
plot.setGroundTruth(gtPos);
plot.addEstimationNode(est.pos.pos);
plot.plot();
// error calc
float err_m = gtPos.getDistance(est.pos.pos);
errorStats.add(err_m);
errorFile << err_m << "\n";
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
if(ts.ms() == 13410 || ts.ms() == 20802){
std::ofstream plotFile;
plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot_zwischendrin_" + std::to_string(ts.ms()) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
}
}
}
// get someting on console
std::cout << "Statistical Analysis Filtering: " << std::endl;
std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl;
// save the statistical data in file
errorFile << "========================================================== \n";
errorFile << "Average of all statistical data: \n";
errorFile << "Median: " << errorStats.getMedian() << "\n";
errorFile << "Average: " << errorStats.getAvg() << "\n";
errorFile << "Standard Deviation: " << errorStats.getStdDev() << "\n";
errorFile << "75 Quantil: " << errorStats.getQuantile(0.75) << "\n";
errorFile.close();
//save the .gp buffer into a file
// std::ofstream plotFile;
// plotFile.open(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_plot" + ".gp");
// dbg.saveToFile(plotFile);
// plotFile.close();
//save also a png image, just for a better overview
// dbg.printOverview(evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t));
return errorStats;
}
int main(int argc, char** argv) { int main(int argc, char** argv) {
navMeshMain(); Stats::Statistics<float> statsAVG;
Stats::Statistics<float> statsMedian;
Stats::Statistics<float> statsSTD;
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
Settings::DataSetup set = Settings::data.Path1;
std::string evaluationName = "museum/Path1_Bulli_2D_PlotsPaper";
for(int i = 0; i < 1; ++i){
for(int j = 0; j < 1; ++j){
tmp = run(set, j, evaluationName);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());
statsQuantil.add(tmp.getQuantile(0.75));
}
std::cout << "Iteration " << i << " completed" << std::endl;;
}
std::cout << "==========================================================" << std::endl;
std::cout << "Average of all statistical data: " << std::endl;
std::cout << "Median: " << statsMedian.getAvg() << std::endl;
std::cout << "Average: " << statsAVG.getAvg() << std::endl;
std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl;
std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl;
std::cout << "==========================================================" << std::endl;
//EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS
std::ofstream finalStatisticFile;
finalStatisticFile.open (Settings::errorDir + evaluationName + ".csv", std::ios_base::app);
finalStatisticFile << "========================================================== \n";
finalStatisticFile << "Average of all statistical data: \n";
finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n";
finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n";
finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n";
finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n";
finalStatisticFile << "========================================================== \n";
finalStatisticFile.close();
std::this_thread::sleep_for(std::chrono::seconds(60));
} }

494
mainToni.h
View File

@@ -1,494 +0,0 @@
#ifndef MAINTONI_H
#define MAINTONI_H
#include <iostream>
#include "filter/Structs.h"
#include "filter/KLB.h"
#include "Plotti.h"
#include "filter/Logic.h"
#include "Settings.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <Indoor/sensors/radio/model/WiFiModelFactory.h>
#include <Indoor/sensors/radio/model/WiFiModelFactoryImpl.h>
#include <Indoor/math/stats/Statistics.h>
#include <Indoor/smc/smoothing/ForwardFilterHistory.h>
#include <Indoor/smc/smoothing/FastKDESmoothing.h>
#include "navMesh/main.h"
#define D_TONI 1
#define D_FRANK 2
#define USE_DATA D_FRANK
#if (USE_DATA == D_FRANK)
//const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
//const std::string dataDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/src/data/";
const std::string mapDir = "/apps/museum/maps/";
const std::string dataDir = "/apps/museum/data/";
const std::string errorDir = dataDir + "results/";
#elif (USE_DATA == D_TONI)
const std::string mapDir = "/home/toni/Documents/programme/localization/IndoorMap/maps/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/DynLag/code/data/";
const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/shl/";
//const std::string dataDir = "/home/toni/Documents/programme/localization/museum/measurements/motionAxisTest/";
const std::string errorDir = dataDir + "results/";
#endif
/** describes one dataset (map, training, parameter-estimation, ...) */
struct DataSetup {
std::string map;
std::vector<std::string> training;
std::string wifiParams;
int minWifiOccurences;
VAPGrouper::Mode vapMode;
std::string grid;
};
/** all configured datasets */
struct Data {
DataSetup SecondFloorOnly = {
mapDir + "SHL_Stock_2_01.xml",
{
dataDir + "Path1_1.csv",
dataDir + "Path2_1.csv",
dataDir + "Path3_1.csv",
},
mapDir + "wifi_fp_all.dat",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
mapDir + "grid_Stock_2_01.dat"
};
DataSetup FloorOneToThree = {
mapDir + "SHL_Stock_1-3_03.xml",
{
dataDir + "Path4_0.csv",
dataDir + "Path5_0.csv",
dataDir + "Path6_0.csv",
},
mapDir + "wifi_fp_all.dat",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
mapDir + "grid_Stock_1-3_03.dat"
};
DataSetup MotionAxisTest = {
mapDir + "SHL40_noElevator.xml",
{
dataDir + "Path0_0.csv"
},
mapDir + "wifi_fp_all.dat",
40,
VAPGrouper::Mode::LAST_MAC_DIGIT_TO_ZERO,
mapDir + "grid_SHL40_noElevator.dat"
};
} data;
Floorplan::IndoorMap* MyState::map;
Stats::Statistics<float> run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPath) {
std::vector<double> kld_data;
// load the floorplan
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(setup.map);
MyState::map = map;
WiFiModelLogDistCeiling WiFiModel(map);
WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
//Wi-Fi model new
// WiFiModelFactory factory(map);
// WiFiModel* wifimodel= factory.loadXML("/home/toni/Documents/programme/localization/data/wifi/model/eachOptParPos_multimodel.xml");
// Assert::isFalse(wifimodel->getAllAPs().empty(), "no AccessPoints stored within the map.xml");
BeaconModelLogDistCeiling beaconModel(map);
beaconModel.loadBeaconsFromMap(map, Settings::BeaconModel::TXP, Settings::BeaconModel::EXP, Settings::BeaconModel::WAF);
//Assert::isFalse(beaconModel.getAllBeacons().empty(), "no Beacons stored within the map.xml");
// build the grid
std::ifstream inp(setup.grid, std::ifstream::binary);
Grid<MyNode> grid(Settings::Grid::gridSize_cm);
// grid.dat empty? -> build one and save it
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
std::ofstream onp;
onp.open(setup.grid);
GridFactory<MyNode> factory(grid);
factory.build(map);
// add node-importance
Importance::addImportance(grid);
grid.write(onp);
} else {
grid.read(inp);
}
// stamp WiFi signal-strengths onto the grid
WiFiGridEstimator::estimate(grid, WiFiModel, Settings::smartphoneAboveGround);
// reading file
Offline::FileReader fr(setup.training[numFile]);
//interpolator for ground truth
Interpolator<uint64_t, Point3> gtInterpolator = fr.getGroundTruthPath(map, gtPath);
//gnuplot plot
Plotti plot;
plot.addFloors(map);
plot.addOutline(map);
plot.addStairs(map);
plot.gp << "set autoscale xy\n";
//plot.addGrid(grid);
plot.splot.getView().setEnabled(false);
// init ctrl and observation
MyControl ctrl;
ctrl.resetAfterTransition();
MyObs obs;
//History of all estimated particles. Used for smoothing
SMC::ForwardFilterHistory<MyState, MyControl, MyObs> pfHistory;
//filter init
SMC::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
//SMC::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(55.5f * 100.0, 43.7f * 100.0, 740.0f), 180.0f)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
//pf.setTransition(std::unique_ptr<PFTransKLDSampling>(new PFTransKLDSampling(grid, &ctrl)));
//pf.setTransition(std::unique_ptr<PFTransSimple>(new PFTransSimple(grid)));
pf.setEvaluation(std::unique_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
//resampling
if(Settings::useKLB){
pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingDivergence<MyState>>(new SMC::ParticleFilterResamplingDivergence<MyState>()));
} else {
pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>>(new SMC::ParticleFilterResamplingSimple<MyState>()));
//pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>>(new SMC::ParticleFilterResamplingPercent<MyState>(0.4)));
//pf.setResampling(std::unique_ptr<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
//pf.setResampling(std::unique_ptr<SMC::ParticleFilterResamplingKLD<MyState>>(new SMC::ParticleFilterResamplingKLD<MyState>()));
}
pf.setNEffThreshold(0.95);
//estimation
pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)));
//pf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationKernelDensity<MyState, 3>>(new SMC::ParticleFilterEstimationKernelDensity<MyState, 3>()));
/** Smoothing Init */
SMC::FastKDESmoothing<MyState, MyControl, MyObs> bf(Settings::numParticles, map, Settings::Grid::gridSize_cm, Settings::KDE::bandwidth);
if(Settings::Smoothing::activated){
//create the backward smoothing filter
bf.setSampler( std::unique_ptr<SMC::CumulativeSampler<MyState>>(new SMC::CumulativeSampler<MyState>()));
bool smoothing_resample = false;
//bf->setNEffThreshold(1.0);
if(smoothing_resample)
bf.setResampling( std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>>(new SMC::ParticleFilterResamplingSimple<MyState>()) );
//bf.setTransition(std::unique_ptr<BFTrans>( new BFTrans) );
bf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
//Smoothing estimation
bf.setEstimation(std::unique_ptr<SMC::ParticleFilterEstimationWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
//bf->setEstimation( std::unique_ptr<SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new SMC::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.50f)));
}
Timestamp lastTimestamp = Timestamp::fromMS(0);
StepDetection sd;
PoseDetection pd;
TurnDetection td(&pd);
MotionDetection md;
ActivityButterPressure act;
//ActivityDetector act;
RelativePressure relBaro;
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
Stats::Statistics<float> errorStats;
Stats::Statistics<float> errorStatsSmoothing;
//file writing for error data
const long int t = static_cast<long int>(time(NULL));
const std::string evalDir = errorDir + folder + std::to_string(t);
if(mkdir(evalDir.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH) == -1){
Assert::doThrow("Eval folder couldn't be created!");
}
std::ofstream errorFile;
errorFile.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + ".csv");
std::ofstream errorFileSmoothing;
errorFileSmoothing.open (evalDir + "/" + std::to_string(numFile) + "_" + std::to_string(t) + "_Smoothing.csv");
// parse each sensor-value within the offline data
for (const Offline::Entry& e : fr.getEntries()) {
const Timestamp ts = Timestamp::fromMS(e.ts);
if (e.type == Offline::Sensor::WIFI) {
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == Offline::Sensor::BEACON){
obs.beacons.entries.push_back(fr.getBeacons()[e.idx].data);
// remove to old beacon measurements
obs.beacons.removeOld(ts);
} else if (e.type == Offline::Sensor::ACC) {
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
++ctrl.numStepsSinceLastTransition;
}
const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
pd.addAccelerometer(ts, _acc.data);
} else if (e.type == Offline::Sensor::GYRO) {
const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
const float delta_gyro = td.addGyroscope(ts, _gyr.data);
ctrl.turnSinceLastTransition_rad += delta_gyro;
} else if (e.type == Offline::Sensor::BARO) {
relBaro.add(ts, fr.getBarometer()[e.idx].data);
obs.relativePressure = relBaro.getPressureRealtiveToStart();
obs.sigmaPressure = relBaro.getSigma();
//activity recognition
act.add(ts, fr.getBarometer()[e.idx].data);
obs.activity = act.get();
//activity for transition
} else if (e.type == Offline::Sensor::LIN_ACC) {
md.addLinearAcceleration(ts, fr.getLinearAcceleration()[e.idx].data);
} else if (e.type == Offline::Sensor::GRAVITY) {
md.addGravity(ts, fr.getGravity()[e.idx].data);
Eigen::Vector2f curVec = md.getCurrentMotionAxis();
ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad();
}
if (ts.ms() - lastTimestamp.ms() > 500) {
/** filtering stuff */
obs.currentTime = ts;
MyState est = pf.update(&ctrl, obs);
Point3 estPos = est.position.inMeter();
Point3 gtPos = gtInterpolator.get(static_cast<uint64_t>(ts.ms()));
/** plotting stuff */
plot.pInterest.clear();
plot.setEst(estPos);
plot.setGT(gtPos);
//plot.addEstimationNode(estPos);
//plot.addParticles(pf.getParticles());
/** error calculation stuff */
float err_m = gtPos.getDistance(estPos);
errorStats.add(err_m);
errorFile << err_m << "\n";
/** smoothing stuff */
if(Settings::Smoothing::activated){
// add everything from the forward step to the history
pfHistory.add(ts, pf.getNonResamplingParticles(), ctrl, obs);
//backward filtering
//((BFTrans*)bf.getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
MyState estBF = bf.update(pfHistory, Settings::Smoothing::lag);
// get ground truth position at lag time
Point3 estPosSmoothing = estBF.position.inMeter();
Point3 gtPosSmoothed;
if(pfHistory.size() <= Settings::Smoothing::lag){
gtPosSmoothed = gtInterpolator.get(static_cast<uint64_t>(pfHistory.getFirstTimestamp().ms()));
} else {
gtPosSmoothed = gtInterpolator.get(static_cast<uint64_t>(pfHistory.getTimestamp(Settings::Smoothing::lag).ms()));
}
//plot
plot.addEstimationNodeSmoothed(estPosSmoothing);
plot.addParticles(bf.getbackwardParticles().back());
if(Settings::Smoothing::lag >= pfHistory.size()){
// error between GT and smoothing
float errSmoothing_m = gtPosSmoothed.getDistance(estPosSmoothing);
errorStatsSmoothing.add(errSmoothing_m);
errorFileSmoothing << errSmoothing_m << "\n";
}
}
//plot misc
plot.setTimeInMinute(static_cast<int>(ts.sec()) / 60, static_cast<int>(static_cast<int>(ts.sec())%60));
if(Settings::useKLB){
plot.gp << "set label 1002 at screen 0.04, 0.94 'KLD: " << ":" << kld_data.back() << "'\n";
}
plot.gp << "set label 1002 at screen 0.95, 0.98 'act:" << static_cast<int>(obs.activity) << "'\n";
//draw gyro angle and motion angle
//turn angle plot
static float angleSumTurn = 0; angleSumTurn += ctrl.turnSinceLastTransition_rad;
plot.showAngle(1, angleSumTurn + M_PI, Point2(0.9, 0.9), "Turn: ");
//motion angle plot
static float angleSumMotion = 0; angleSumMotion += ctrl.motionDeltaAngle_rad;
plot.showAngle(2, angleSumMotion + M_PI, Point2(0.9, 0.8), "Motion: ");
/** Draw everything */
plot.show();
usleep(10*10);
lastTimestamp = ts;
// reset control
ctrl.resetAfterTransition();
}
}
errorFile.close();
std::cout << "Statistical Analysis Filtering: " << std::endl;
std::cout << "Median: " << errorStats.getMedian() << " Average: " << errorStats.getAvg() << " Std: " << errorStats.getStdDev() << std::endl;
std::cout << "Statistical Analysis Smoothing: " << std::endl;
std::cout << "Median: " << errorStatsSmoothing.getMedian() << " Average: " << errorStatsSmoothing.getAvg() << " Std: " << errorStatsSmoothing.getStdDev() << std::endl;
//Write the current plotti buffer into file
std::ofstream plotFile;
plotFile.open(evalDir + "/plot_" + std::to_string(numFile) + "_" + std::to_string(t) + ".gp");
plot.saveToFile(plotFile);
plotFile.close();
for(int i = 0; i < map->floors.size(); ++i){
plot.printSingleFloor(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), i);
plot.show();
usleep(10*10);
}
plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 90);
plot.show();
plot.printSideView(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t), 0);
plot.show();
plot.printOverview(evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t));
plot.show();
/** Draw KLB */
K::Gnuplot gp;
K::GnuplotPlot plotkld;
K::GnuplotPlotElementLines lines;
if(Settings::useKLB){
std::string path = evalDir + "/image" + std::to_string(numFile) + "_" + std::to_string(t);
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_shennendistance.png'\n";
for(int i=0; i < kld_data.size()-1; ++i){
K::GnuplotPoint2 p1(i, kld_data[i]);
K::GnuplotPoint2 p2(i+1, kld_data[i+1]);
lines.addSegment(p1, p2);
}
plotkld.add(&lines);
gp.draw(plotkld);
gp.flush();
plot.splot.getView().setEnabled(false);
}
std::cout << "finished" << std::endl;
sleep(1);
return errorStats;
}
int main(int argc, char** argv) {
Stats::Statistics<float> statsAVG;
Stats::Statistics<float> statsMedian;
Stats::Statistics<float> statsSTD;
Stats::Statistics<float> statsQuantil;
Stats::Statistics<float> tmp;
for(int i = 0; i < 10; ++i){
tmp = run(data.SecondFloorOnly, 0, "KDE-Smoothing-Test", Settings::Path_DongleTest::path1);
statsMedian.add(tmp.getMedian());
statsAVG.add(tmp.getAvg());
statsSTD.add(tmp.getStdDev());
statsQuantil.add(tmp.getQuantile(0.75));
// tmp = run(data.MotionAxisTest, 0, "Motion-Axis-Test", Settings::Path_DongleTest::path1);
// statsMedian.add(tmp.getMedian());
// statsAVG.add(tmp.getAvg());
// statsSTD.add(tmp.getStdDev());
// statsQuantil.add(tmp.getQuantile(0.75));
std::cout << "Iteration " << i << " completed" << std::endl;;
}
std::cout << "==========================================================" << std::endl;
std::cout << "Average of all statistical data: " << std::endl;
std::cout << "Median: " << statsMedian.getAvg() << std::endl;
std::cout << "Average: " << statsAVG.getAvg() << std::endl;
std::cout << "Standard Deviation: " << statsSTD.getAvg() << std::endl;
std::cout << "75 Quantil: " << statsQuantil.getAvg() << std::endl;
std::cout << "==========================================================" << std::endl;
//EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS
std::ofstream finalStatisticFile;
finalStatisticFile.open (errorDir + "/tmp.csv");
finalStatisticFile << "Average of all statistical data: \n";
finalStatisticFile << "Median: " << statsMedian.getAvg() << "\n";
finalStatisticFile << "Average: " << statsAVG.getAvg() << "\n";
finalStatisticFile << "Standard Deviation: " << statsSTD.getAvg() << "\n";
finalStatisticFile << "75 Quantil: " << statsQuantil.getAvg() << "\n";
finalStatisticFile.close();
//EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS EDIT THIS
}
#endif // MAINTONI_H

72
navMesh/filter.h
View File

@@ -3,6 +3,7 @@
#include "mesh.h" #include "mesh.h"
#include "../Settings.h" #include "../Settings.h"
#include <omp.h>
#include <Indoor/geo/Heading.h> #include <Indoor/geo/Heading.h>
#include <Indoor/math/Distributions.h> #include <Indoor/math/Distributions.h>
@@ -12,15 +13,15 @@
#include <Indoor/smc/filtering/ParticleFilterInitializer.h> #include <Indoor/smc/filtering/ParticleFilterInitializer.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h> #include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKLD.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationBoxKDE.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h> #include <Indoor/smc/filtering/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <Indoor/smc/filtering/estimation/ParticleFilterEstimationMax.h>
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h> #include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
#include <Indoor/navMesh/walk/NavMeshWalkEval.h> #include <Indoor/navMesh/walk/NavMeshWalkEval.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h> #include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/data/Timestamp.h> #include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/WiFiProbabilityFree.h> #include <Indoor/sensors/radio/WiFiProbabilityFree.h>
#include <Indoor/sensors/activity/ActivityDetector.h>
struct MyState { struct MyState {
@@ -32,6 +33,8 @@ struct MyState {
MyState() : pos(), heading(0) {;} MyState() : pos(), heading(0) {;}
MyState(Point3 p) : pos(p, nullptr), heading(0){;}
MyState& operator += (const MyState& o) { MyState& operator += (const MyState& o) {
pos.tria = nullptr; // impossible pos.tria = nullptr; // impossible
pos.pos += o.pos.pos; pos.pos += o.pos.pos;
@@ -50,6 +53,14 @@ struct MyState {
return res; return res;
} }
float getX(){
return pos.pos.x;
}
float getY() {
return pos.pos.y;
}
float getBinValue(const int dim) const { float getBinValue(const int dim) const {
switch (dim) { switch (dim) {
case 0: return this->pos.pos.x; case 0: return this->pos.pos.x;
@@ -87,6 +98,9 @@ struct MyObservation {
//time //time
Timestamp currentTime; Timestamp currentTime;
//activity
Activity activity;
}; };
class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> { class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
@@ -156,21 +170,28 @@ public:
void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override { void transition(std::vector<SMC::Particle<MyState>>& particles, const MyControl* control) override {
Distribution::Normal<float> dStepSizeFloor(0.70, 0.1); Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
Distribution::Normal<float> dStepSizeStair(0.35, 0.1); Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
Distribution::Normal<float> dHeading(0.0, 0.10); Distribution::Normal<float> dHeading(0.0, 0.1);
#pragma omp parallel for num_threads(3)
for (SMC::Particle<MyState>& p : particles) { for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
// how to walk // how to walk
MyNavMeshWalkParams params; MyNavMeshWalkParams params;
params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw(); params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw();
params.numSteps = control->numStepsSinceLastEval; params.numSteps = control->numStepsSinceLastEval;
params.start = p.state.pos; params.start = p.state.pos;
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw(); params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
params.stepSizes.stepSizeStair_m = dStepSizeStair.draw(); params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
if(params.stepSizes.stepSizeFloor_m < 0.1 || params.stepSizes.stepSizeStair_m < 0.1){
params.stepSizes.stepSizeFloor_m = 0.1;
params.stepSizes.stepSizeStair_m = 0.1;
}
// walk // walk
MyNavMeshWalk::ResultEntry res = walker.getOne(params); MyNavMeshWalk::ResultEntry res = walker.getOne(params);
@@ -194,6 +215,29 @@ class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
WiFiModel& wifiModel; WiFiModel& wifiModel;
WiFiObserverFree wifiProbability; WiFiObserverFree wifiProbability;
//TODO: add this to transition probability
double getStairProb(const SMC::Particle<MyState>& p, const Activity act) {
const float kappa = 0.75;
switch (act) {
case Activity::WALKING:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::FLOOR_INDOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::DOOR) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
{return 1-kappa;}
case Activity::WALKING_UP:
case Activity::WALKING_DOWN:
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_SKEWED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::STAIR_LEVELED) {return kappa;}
if (p.state.pos.tria->getType() == (int) NM::NavMeshType::ELEVATOR) {return kappa;}
{return 1-kappa;}
}
return 1.0;
}
public: public:
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){} MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
@@ -203,13 +247,23 @@ public:
double sum = 0; double sum = 0;
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi); const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(observation.wifi);
for (SMC::Particle<MyState>& p : particles) {
#pragma omp parallel for num_threads(3)
for (int i = 0; i < particles.size(); ++i) {
SMC::Particle<MyState>& p = particles[i];
double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs); double pWifi = wifiProbability.getProbability(p.state.pos.pos, observation.currentTime, wifiObs);
double pStair = getStairProb(p, observation.activity);
const double prob = pWifi; //HACK HACK HACK HACK
double prob = 1.0;
if(observation.currentTime.ms() > 20801){
prob = pWifi * pStair;
}
p.weight *= prob; p.weight *= prob;
#pragma omp atomic
sum += prob; sum += prob;
} }

183
navMesh/main.h
View File

@@ -1,183 +0,0 @@
#ifndef NAV_MESH_MAIN_H
#define NAV_MESH_MAIN_H
#include "mesh.h"
#include "filter.h"
#include "../Settings.h"
#include <memory>
#include <thread>
#include <Indoor/floorplan/v2/FloorplanReader.h>
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/geo/Heading.h>
#include <Indoor/geo/Point2.h>
#include <Indoor/sensors/imu/TurnDetection.h>
#include <Indoor/sensors/imu/StepDetection.h>
#include <Indoor/sensors/imu/MotionDetection.h>
#include <Indoor/sensors/pressure/RelativePressure.h>
#include <Indoor/data/Timestamp.h>
#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
void navMeshMain() {
//std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml";
std::string mapFile = "../map/map42_ap.xml";
// reading file
Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile);
Offline::FileReader fr("../measurements/museum/Pixel/Path1_2468.csv");
WiFiFingerprints fingerprints("../measurements/museum/Nexus/fingerprints/wifi_fp.dat");
const std::string wifiModelFile = "../measurements/museum/wifimodel.dat";
std::ifstream inp(wifiModelFile, std::ifstream::binary);
// wifi
WiFiModelLogDistCeiling WiFiModel(map);
// with optimization
if(Settings::WiFiModel::optimize){
if (!inp.good() || (inp.peek()&&0) || inp.eof()) {
Assert::isFalse(fingerprints.getFingerprints().empty(), "no fingerprints available!");
WiFiOptimizer::LogDistCeiling opt(map, Settings::WiFiModel::vg_calib);
for (const WiFiFingerprint& fp : fingerprints.getFingerprints()) {
opt.addFingerprint(fp);
}
const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(opt.NONE);
for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
WiFiModel.addAP(ap.mac, entry);
}
WiFiModel.saveXML(wifiModelFile);
} else {
WiFiModel.loadXML(wifiModelFile);
}
} else {
// without optimization
WiFiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
Assert::isFalse(WiFiModel.getAllAPs().empty(), "no AccessPoints stored within the map.xml");
}
// mesh
NM::NavMeshSettings set;
MyNavMesh mesh;
MyNavMeshFactory fac(&mesh, set);
fac.build(map);
const Point3 srcPath0(26, 43, 7.5);
const Point3 srcPath1(62, 38, 1.8);
// add shortest-path to destination
//const Point3 dst(51, 45, 1.7);
//const Point3 dst(25, 45, 0);
//NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);
// debug show
NM::NavMeshDebug dbg;
dbg.addMesh(mesh);
//dbg.addDijkstra(mesh);
dbg.draw();
// particle-filter
const int numParticles = 1000;
//auto init = std::make_unique<MyPFInitFixed>(&mesh, srcPath1); // known position
auto init = std::make_unique<MyPFInitUniform>(&mesh); // uniform distribution
auto eval = std::make_unique<MyPFEval>(WiFiModel);
auto trans = std::make_unique<MyPFTrans>(mesh);
auto resample = std::make_unique<SMC::ParticleFilterResamplingSimple<MyState>>();
auto estimate = std::make_unique<SMC::ParticleFilterEstimationWeightedAverage<MyState>>();
// setup
MyFilter pf(numParticles, std::move(init));
pf.setEvaluation(std::move(eval));
pf.setTransition(std::move(trans));
pf.setResampling(std::move(resample));
pf.setEstimation(std::move(estimate));
pf.setNEffThreshold(1);
// sensors
MyControl ctrl;
MyObservation obs;
StepDetection sd;
PoseDetection pd;
TurnDetection td(&pd);
RelativePressure relBaro;
relBaro.setCalibrationTimeframe( Timestamp::fromMS(5000) );
Timestamp lastTimestamp = Timestamp::fromMS(0);
// parse each sensor-value within the offline data
for (const Offline::Entry& e : fr.getEntries()) {
const Timestamp ts = Timestamp::fromMS(e.ts);
if (e.type == Offline::Sensor::WIFI) {
obs.wifi = fr.getWiFiGroupedByTime()[e.idx].data;
} else if (e.type == Offline::Sensor::ACC) {
if (sd.add(ts, fr.getAccelerometer()[e.idx].data)) {
++ctrl.numStepsSinceLastEval;
}
const Offline::TS<AccelerometerData>& _acc = fr.getAccelerometer()[e.idx];
pd.addAccelerometer(ts, _acc.data);
} else if (e.type == Offline::Sensor::GYRO) {
const Offline::TS<GyroscopeData>& _gyr = fr.getGyroscope()[e.idx];
const float delta_gyro = td.addGyroscope(ts, _gyr.data);
ctrl.headingChangeSinceLastEval += delta_gyro;
} else if (e.type == Offline::Sensor::BARO) {
relBaro.add(ts, fr.getBarometer()[e.idx].data);
obs.relativePressure = relBaro.getPressureRealtiveToStart();
obs.sigmaPressure = relBaro.getSigma();
}
if (ts.ms() - lastTimestamp.ms() > 500 && ctrl.numStepsSinceLastEval > 0) {
obs.currentTime = ts;
// if(ctrl.numStepsSinceLastEval > 0){
// pf.updateTransitionOnly(&ctrl);
// ctrl.afterEval();
// }
// MyState est = pf.updateEvaluationOnly(obs);
// lastTimestamp = ts;
MyState est = pf.update(&ctrl, obs);
ctrl.afterEval();
lastTimestamp = ts;
// try {
// MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos);
// auto path = loc.tria->getPathToDestination<MyNavMeshTriangle>(loc.pos);
// dbg.addDijkstra(path);
// } catch (...) {;}
// const int d = (i * 1) % 360;
// dbg.plot.getView().setCamera(60, d);
dbg.showParticles(pf.getParticles());
dbg.setCurPos(est.pos.pos);
//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
// std::cout << i << std::endl;
dbg.draw();
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
}
}
#endif

242
navMesh/meshPlotter.h Normal file
View File

@@ -0,0 +1,242 @@
#ifndef MESHPLOTTER_H
#define MESHPLOTTER_H
#include <KLib/misc/gnuplot/Gnuplot.h>
#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 <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
#include <Indoor/math/Distributions.h>
#include <Indoor/navMesh/NavMesh.h>
#include <Indoor/floorplan/v2/Floorplan.h>
class NavMeshTriangleDijkstra;
/**
* debug plot NavMeshes
*/
class MeshPlotter {
public:
K::Gnuplot gp;
K::GnuplotSplot plot;
K::GnuplotSplotElementLines pFloor;
K::GnuplotSplotElementLines pOutline;
K::GnuplotSplotElementLines lines;
K::GnuplotSplotElementPoints border;
K::GnuplotSplotElementColorPoints particles;
K::GnuplotSplotElementColorPoints distances;
K::GnuplotSplotElementLines pathEstimated;
K::GnuplotSplotElementLines shortestPath;
K::GnuplotSplotElementLines groundtruthPath;
private:
K::GnuplotFill gFill[6] = {
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#0000ff"), 1), // unknown
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"), 1), // indoor
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#44ffee"), 1), // outdoor
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666699"), 1), // door
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#444444"), 1), // stairs_level
K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"), 1) // stairs_skewed
};
public:
MeshPlotter() {
gp << "set view equal xy\n";
plot.add(&lines); lines.setShowPoints(true);
plot.add(&border);
plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2);
plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00");
plot.add(&groundtruthPath); groundtruthPath.getStroke().setWidth(2); groundtruthPath.getStroke().getColor().setHexStr("#000000");
plot.add(&distances); distances.setPointSize(0.75); distances.setPointType(7);
plot.add(&shortestPath); shortestPath.getStroke().setWidth(3);
plot.add(&pFloor);
plot.add(&pOutline); pOutline.getStroke().getColor().setHexStr("#999999");
}
void draw() {
gp.draw(plot);
gp.flush();
}
template <typename T> void showParticles(const std::vector<T>& particles) {
this->particles.clear();
double min = +999;
double max = -999;
for (const T& p : particles) {
const K::GnuplotPoint3 p3(p.state.pos.pos.x, p.state.pos.pos.y, p.state.pos.pos.z);
const double prob = std::pow(p.weight, 0.25);
this->particles.add(p3, prob);
if (prob > max) {max = prob;}
if (prob < min) {min = prob;}
}
plot.getAxisCB().setRange(min, max + 0.000001);
}
template <typename Tria> void addMesh(NM::NavMesh<Tria>& nm) {
K::GnuplotStroke gStroke = K::GnuplotStroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#666600"));
const BBox3 bbox = nm.getBBox();
border.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z));
border.add(K::GnuplotPoint3(bbox.getMax().x,bbox.getMax().y,bbox.getMax().z));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(bbox.getMax().x, 0, 0));
// lines.add(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,bbox.getMax().y,0));
// lines.addSegment(K::GnuplotPoint3(bbox.getMin().x,bbox.getMin().y,bbox.getMin().z), K::GnuplotPoint3(0,0,bbox.getMax().z));
//stairs in eigene group? vlt gehen dann auch die dellen weg?
for (const Tria* tria : nm) {
const uint8_t type = tria->getType();
if (type < 0 || type > 5) {
throw std::runtime_error("out of type-bounds");
}
K::GnuplotObjectPolygon* pol = new K::GnuplotObjectPolygon(gFill[type], gStroke);
pol->add(K::GnuplotCoordinate3(tria->getP1().x, tria->getP1().y, tria->getP1().z, K::GnuplotCoordinateSystem::FIRST));
pol->add(K::GnuplotCoordinate3(tria->getP2().x, tria->getP2().y, tria->getP2().z, K::GnuplotCoordinateSystem::FIRST));
pol->add(K::GnuplotCoordinate3(tria->getP3().x, tria->getP3().y, tria->getP3().z, K::GnuplotCoordinateSystem::FIRST));
pol->close();
pol->setZIndex(tria->getP3().z);
plot.getObjects().add(pol);
//for (int i = 0; i < nm.getNumNeighbors(tria); ++i) {
// const Tria* o = nm.getNeighbor(tria, i);
// const Point3 p1 = tria->getCenter();
// const Point3 p2 = o.getCenter();
// //lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
//}
for (const NM::NavMeshTriangle* o : *tria) {
const Point3 p1 = tria->getCenter();
const Point3 p2 = o->getCenter();
// lines.addSegment(K::GnuplotPoint3(p1.x,p1.y,p1.z+0.1), K::GnuplotPoint3(p2.x,p2.y,p2.z+0.1));
}
}
plot.getObjects().reOrderByZIndex();
}
template <typename Tria> void addDijkstra(NM::NavMesh<Tria>& mesh) {
distances.clear();
// ensure Tria extends NavMeshTriangleDijkstra
StaticAssert::AinheritsB<Tria, NavMeshTriangleDijkstra>();
NM::NavMeshRandom<Tria> rnd = mesh.getRandom();
for (int i = 0; i < 5000; ++i) {
NM::NavMeshLocation<Tria> loc = rnd.draw();
float v = loc.tria->interpolate(loc.pos, loc.tria->spFromP1.distance, loc.tria->spFromP2.distance, loc.tria->spFromP3.distance);
distances.add(K::GnuplotPoint3(loc.pos.x, loc.pos.y, loc.pos.z), v);
}
// Distribution::Uniform<float> dist (-0.5, +0.5);
// for (const Tria* t : mesh) {
// const Point3 posC = t->getCenter();
// distances.add(K::GnuplotPoint3(posC.x+dist.draw(), posC.y+dist.draw(), posC.z), t->distAtCenter);
// const Point3 pos1 = t->getP1();
// distances.add(K::GnuplotPoint3(pos1.x+dist.draw(), pos1.y+dist.draw(), pos1.z), t->distAtP1);
// const Point3 pos2 = t->getP2();
// distances.add(K::GnuplotPoint3(pos2.x+dist.draw(), pos2.y+dist.draw(), pos2.z), t->distAtP2);
// const Point3 pos3 = t->getP3();
// distances.add(K::GnuplotPoint3(pos3.x+dist.draw(), pos3.y+dist.draw(), pos3.z), t->distAtP3);
// }
}
template <typename Tria> void addDijkstra(std::vector<NM::NavMeshLocation<Tria>>& path) {
shortestPath.clear();
for (auto& e : path) {
K::GnuplotPoint3 gp(e.pos.x, e.pos.y, e.pos.z);
shortestPath.add(gp);
}
}
void addGroundTruthNode(const Point3 pos) {
K::GnuplotPoint3 gp(pos.x, pos.y, std::round(pos.z * 10) / 10);
groundtruthPath.add(gp);
}
void addEstimationNode(const Point3 pos){
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
pathEstimated.add(est);
}
void setTimeInMinute(const int minutes, const int seconds) {
gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << seconds << "'\n";
}
void setGT(const Point3 pt) {
gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.4) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n";
}
void setCurPos(const Point3 pt) {
gp << "set arrow 31338 from " << pt.x << "," << pt.y << "," << (pt.z+0.9) << " to " << pt.x << "," << pt.y << "," << pt.z << " lw 2 lc 'green' front \n";
}
void saveToFile(std::ofstream& stream){
gp.draw(plot);
stream << "set terminal x11 size 2000,1500\n";
stream << gp.getBuffer();
stream << "pause -1\n";
gp.flush();
}
void printOverview(const std::string& path) {
gp << "set terminal png size 1280,720\n";
gp << "set output '" << path << "_overview" << ".png'\n";
gp << "set view 75,60\n";
gp << "set autoscale xy\n";
gp << "set autoscale z\n";
draw();
}
//meshless drawing
void addFloors(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::FloorObstacle* obs : f->obstacles) {
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
if (line) {
K::GnuplotPoint3 p1(line->from.x, line->from.y, f->atHeight);
K::GnuplotPoint3 p2(line->to.x, line->to.y, f->atHeight);
pFloor.addSegment(p1, p2);
}
}
}
}
void addOutline(Floorplan::IndoorMap* map) {
for (Floorplan::Floor* f : map->floors) {
for (Floorplan::FloorOutlinePolygon* poly : f->outline) {
const int cnt = poly->poly.points.size();
for (int i = 0; i < cnt; ++i) {
Point2 p1 = poly->poly.points[(i+0)];
Point2 p2 = poly->poly.points[(i+1)%cnt];
K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
pOutline.addSegment(gp1, gp2);
}
}
}
}
};
#endif // MESHPLOTTER_H