140 lines
3.9 KiB
C++
140 lines
3.9 KiB
C++
#ifndef PLOTTY_H
|
|
#define PLOTTY_H
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include <Indoor/floorplan/v2/FloorplanHelper.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/GnuplotPlot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotPlotElementHistogram.h>
|
|
|
|
class Plotty {
|
|
|
|
public:
|
|
|
|
Floorplan::IndoorMap* map;
|
|
K::Gnuplot gp;
|
|
K::GnuplotSplot splot;
|
|
K::GnuplotSplotElementPoints points;
|
|
K::GnuplotSplotElementColorPoints cpoints;
|
|
K::GnuplotSplotElementLines pathReal;
|
|
K::GnuplotSplotElementLines pathEst;
|
|
K::GnuplotSplotElementLines mapOutline;
|
|
|
|
public:
|
|
|
|
Plotty(Floorplan::IndoorMap* map) : map(map) {
|
|
|
|
//gp << "set view equal xy\n";
|
|
|
|
gp << "set palette model RGB\n";
|
|
//gp << "set palette defined (0 '#0000ff', 1 '#ff0000')\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";
|
|
|
|
// draw floorplan
|
|
mapOutline.setColorHex("#888888");
|
|
splot.add(&mapOutline);
|
|
for (Floorplan::Floor* floor : map->floors) {
|
|
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
|
|
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
|
if (line) {
|
|
const K::GnuplotPoint3 p1(line->from.x, line->from.y, floor->atHeight);
|
|
const K::GnuplotPoint3 p2(line->to.x, line->to.y, floor->atHeight);
|
|
mapOutline.addSegment(p1, p2);
|
|
}
|
|
}
|
|
for (Floorplan::Stair* s : floor->stairs) {
|
|
for (const Floorplan::StairPart& sp : s->getParts()) {
|
|
const K::GnuplotPoint3 p1(sp.start.x, sp.start.y, sp.start.z + floor->atHeight);
|
|
const K::GnuplotPoint3 p2(sp.end.x, sp.end.y, sp.end.z + floor->atHeight);
|
|
mapOutline.addSegment(p1, p2);
|
|
}
|
|
}
|
|
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
|
gp << "set object polygon from ";
|
|
for (size_t i = 0; i < poly->poly.points.size(); ++i) {
|
|
if (i > 0) {gp << " to ";}
|
|
const Point2 pt = poly->poly.points[i];
|
|
gp << pt.x << "," << pt.y << "," << floor->atHeight << " ";
|
|
}
|
|
gp << " fs solid fc rgb " << ( poly->outdoor ? "'#ddffdd'" : "'#dddddd'");
|
|
gp << "\n";
|
|
}
|
|
}
|
|
|
|
splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#0000ff");
|
|
splot.add(&pathEst);
|
|
|
|
splot.add(&points);
|
|
points.setPointType(7);
|
|
points.setPointSize(0.5);
|
|
|
|
splot.add(&cpoints);
|
|
cpoints.setPointSize(2);
|
|
cpoints.setPointType(7);
|
|
|
|
}
|
|
|
|
|
|
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";
|
|
}
|
|
|
|
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 plot() {
|
|
gp.draw(splot);
|
|
gp.flush();
|
|
}
|
|
|
|
};
|
|
|
|
#endif // PLOTTY_H
|