369 lines
9.4 KiB
C++
369 lines
9.4 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>
|
|
|
|
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;
|
|
|
|
std::string codeFile;
|
|
|
|
struct Settings {
|
|
std::vector<int> floors = {};
|
|
bool stairs = true;
|
|
bool obstacles = true;
|
|
bool outline = true;
|
|
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.setColorHex("#888888"); mapOutlineConcrete.setLineWidth(2);
|
|
mapOutlineDrywall.setColorHex("#888888");
|
|
mapOutlineGlass.setColorHex("#888888"); mapOutlineGlass.setDashType(2);
|
|
splot.add(&mapOutlineConcrete);
|
|
splot.add(&mapOutlineDrywall);
|
|
splot.add(&mapOutlineGlass);
|
|
|
|
splot.add(&particles); particles.setPointSize(0.20); //particles.setColorHex("#777777");
|
|
|
|
splot.add(&pathReal); pathReal.setLineWidth(2); pathReal.setColorHex("#000000");
|
|
splot.add(&pathEst); pathEst.setLineWidth(2); pathEst.setColorHex("#0000ff");
|
|
|
|
splot.add(&points);
|
|
points.setPointType(7);
|
|
points.setPointSize(0.5);
|
|
|
|
splot.add(&cpoints);
|
|
cpoints.setPointSize(2);
|
|
cpoints.setPointType(7);
|
|
|
|
}
|
|
|
|
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";
|
|
}
|
|
|
|
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 addPolygon(const std::vector<Point3>& points, const std::string& color, bool front = false, bool fill = true) {
|
|
|
|
for (const Point3 p : points) {
|
|
if (p.z < settings.minZ) {return;}
|
|
if (p.z > settings.maxZ) {return;}
|
|
}
|
|
|
|
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";
|
|
|
|
}
|
|
|
|
void setZRange(const float min, const float max) {
|
|
gp << "set zrange [" << min << ":" << max << "]\n";
|
|
}
|
|
|
|
|
|
void addFloorRect(const Point3 pos_m, const float size, Color c) {
|
|
|
|
const Point3 p1 = pos_m + Point3(-size, -size, 0);
|
|
const Point3 p2 = pos_m + Point3(+size, -size, 0);
|
|
const Point3 p3 = pos_m + Point3(+size, +size, 0);
|
|
const Point3 p4 = pos_m + Point3(-size, +size, 0);
|
|
|
|
std::vector<Point3> points = {p1,p2,p3,p4,p1};
|
|
|
|
addPolygon(points, c.toHEX());
|
|
|
|
// 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";
|
|
|
|
}
|
|
|
|
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";
|
|
}
|
|
|
|
void setScale(const float x, const float y) {
|
|
gp << "set multiplot layout 1,1 scale " << x << "," << y << "\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";
|
|
}
|
|
|
|
void writeEpsTex(const std::string file, K::GnuplotSize size = K::GnuplotSize(8.5, 5.1)) {
|
|
gp.setTerminal("epslatex", size);
|
|
gp.setOutput(file);
|
|
}
|
|
|
|
void plot() {
|
|
gp.draw(splot);
|
|
gp << "unset multiplot\n"; // scaling
|
|
if (codeFile != "") {
|
|
std::ofstream out(codeFile);
|
|
out << gp.getBuffer();
|
|
out.close();
|
|
}
|
|
gp.flush();
|
|
}
|
|
|
|
|
|
void buildFloorplan() {
|
|
|
|
std::vector<Floorplan::Floor*> floors;
|
|
|
|
// only some floors??
|
|
if (settings.floors.empty()) {
|
|
floors = map->floors;
|
|
} else {
|
|
for (int i : settings.floors) {
|
|
floors.push_back(map->floors[i]);
|
|
}
|
|
}
|
|
|
|
|
|
for (Floorplan::Floor* floor : floors) {
|
|
|
|
// plot obstacles
|
|
if (settings.obstacles) {
|
|
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);
|
|
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;
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
// plot the floor's outline
|
|
if (settings.outline) {
|
|
for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
|
|
gp << "set object polygon from ";
|
|
for (size_t i = 0; i < poly->poly.points.size() + 1; ++i) {
|
|
if (i > 0) {gp << " to ";}
|
|
const Point2 pt = poly->poly.points[i % poly->poly.points.size()]; // ensures closing the polygon
|
|
gp << pt.x << "," << pt.y << "," << floor->atHeight << " ";
|
|
}
|
|
gp << " fs solid fc rgb " << ( poly->outdoor ? "'#bbeebb'" : "'#dddddd'");
|
|
gp << "\n";
|
|
}
|
|
}
|
|
|
|
// 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) {
|
|
addPolygon({q.p1, q.p2, q.p3, q.p4, q.p1}, "#c0c0c0");
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // PLOTTY_H
|