version for plotting the figures of kde paper
This commit is contained in:
toni
@@ -59,13 +59,13 @@ ADD_DEFINITIONS(
|
||||
-fstack-protector-all
|
||||
|
||||
-g3
|
||||
# -O2
|
||||
#-O2
|
||||
-march=native
|
||||
|
||||
-DWITH_TESTS
|
||||
-DWITH_ASSERTIONS
|
||||
#-DWITH_DEBUG_LOG
|
||||
-DWITH_DEBUG_PLOT
|
||||
#-DWITH_DEBUG_PLOT
|
||||
#-D_GLIBCXX_DEBUG
|
||||
|
||||
|
||||
|
||||
1
Plotti.h
1
Plotti.h
@@ -1,7 +1,6 @@
|
||||
#ifndef PLOTTI_H
|
||||
#define PLOTTI_H
|
||||
|
||||
#include "filter/Structs.h"
|
||||
#include "Settings.h"
|
||||
|
||||
#include <functional>
|
||||
|
||||
672
Plotty.h
Executable file
672
Plotty.h
Executable 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
106
Settings.h
@@ -9,7 +9,7 @@ namespace Settings {
|
||||
|
||||
bool useKLB = false;
|
||||
|
||||
const int numParticles = 6000;
|
||||
const int numParticles = 5000;
|
||||
const int numBSParticles = 50;
|
||||
|
||||
namespace IMU {
|
||||
@@ -83,16 +83,104 @@ namespace Settings {
|
||||
constexpr bool useMainThread = false; // perform filtering in the main thread
|
||||
}
|
||||
|
||||
namespace Path_DongleTest {
|
||||
const std::vector<int> path1 = {0, 1, 2, 3, 4, 5, 6};
|
||||
const std::vector<int> path2 = {6, 5, 4, 7, 8, 9, 8, 10};
|
||||
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};
|
||||
const std::string mapDir = "../map/";
|
||||
const std::string dataDir = "../measurements/";
|
||||
const std::string errorDir = dataDir + "results/";
|
||||
|
||||
}
|
||||
/** 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
|
||||
|
||||
179
filter/KLB.h
179
filter/KLB.h
@@ -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
561
filter/Logic.h
@@ -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
136
filter/Structs.h
@@ -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
300
main.cpp
@@ -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) {
|
||||
|
||||
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
494
mainToni.h
@@ -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
|
||||
@@ -3,6 +3,7 @@
|
||||
|
||||
#include "mesh.h"
|
||||
#include "../Settings.h"
|
||||
#include <omp.h>
|
||||
|
||||
#include <Indoor/geo/Heading.h>
|
||||
#include <Indoor/math/Distributions.h>
|
||||
@@ -12,15 +13,15 @@
|
||||
#include <Indoor/smc/filtering/ParticleFilterInitializer.h>
|
||||
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingSimple.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/ParticleFilterEstimationMax.h>
|
||||
#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
|
||||
#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
|
||||
|
||||
#include <Indoor/sensors/radio/WiFiMeasurements.h>
|
||||
#include <Indoor/data/Timestamp.h>
|
||||
|
||||
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
||||
#include <Indoor/sensors/activity/ActivityDetector.h>
|
||||
|
||||
struct MyState {
|
||||
|
||||
@@ -32,6 +33,8 @@ struct MyState {
|
||||
|
||||
MyState() : pos(), heading(0) {;}
|
||||
|
||||
MyState(Point3 p) : pos(p, nullptr), heading(0){;}
|
||||
|
||||
MyState& operator += (const MyState& o) {
|
||||
pos.tria = nullptr; // impossible
|
||||
pos.pos += o.pos.pos;
|
||||
@@ -50,6 +53,14 @@ struct MyState {
|
||||
return res;
|
||||
}
|
||||
|
||||
float getX(){
|
||||
return pos.pos.x;
|
||||
}
|
||||
|
||||
float getY() {
|
||||
return pos.pos.y;
|
||||
}
|
||||
|
||||
float getBinValue(const int dim) const {
|
||||
switch (dim) {
|
||||
case 0: return this->pos.pos.x;
|
||||
@@ -87,6 +98,9 @@ struct MyObservation {
|
||||
//time
|
||||
Timestamp currentTime;
|
||||
|
||||
//activity
|
||||
Activity activity;
|
||||
|
||||
};
|
||||
|
||||
class MyPFInitUniform : public SMC::ParticleFilterInitializer<MyState> {
|
||||
@@ -156,21 +170,28 @@ public:
|
||||
|
||||
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> dHeading(0.0, 0.10);
|
||||
Distribution::Normal<float> dHeading(0.0, 0.1);
|
||||
|
||||
|
||||
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];
|
||||
|
||||
// how to walk
|
||||
MyNavMeshWalkParams params;
|
||||
params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw();
|
||||
params.numSteps = control->numStepsSinceLastEval;
|
||||
params.start = p.state.pos;
|
||||
|
||||
params.stepSizes.stepSizeFloor_m = dStepSizeFloor.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
|
||||
MyNavMeshWalk::ResultEntry res = walker.getOne(params);
|
||||
|
||||
@@ -194,6 +215,29 @@ class MyPFEval : public SMC::ParticleFilterEvaluation<MyState, MyObservation> {
|
||||
WiFiModel& wifiModel;
|
||||
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:
|
||||
|
||||
MyPFEval(WiFiModel& wifiModel) : wifiModel(wifiModel), wifiProbability(Settings::WiFiModel::sigma, wifiModel){}
|
||||
@@ -203,13 +247,23 @@ public:
|
||||
double sum = 0;
|
||||
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 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;
|
||||
|
||||
#pragma omp atomic
|
||||
sum += prob;
|
||||
}
|
||||
|
||||
|
||||
183
navMesh/main.h
183
navMesh/main.h
@@ -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
242
navMesh/meshPlotter.h
Normal 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
|
||||
Reference in New Issue
Block a user