714 lines
25 KiB
C++
714 lines
25 KiB
C++
#pragma once
|
|
|
|
#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);
|
|
|
|
splot.getView().setEnabled(true);
|
|
|
|
}
|
|
|
|
void addCircle(int id, const Point2& center, float radius)
|
|
{
|
|
auto c = K::GnuplotCoordinate2(center.x, center.y, K::GnuplotCoordinateSystem::FIRST);
|
|
auto r = K::GnuplotCoordinate1(radius, K::GnuplotCoordinateSystem::FIRST);
|
|
|
|
K::GnuplotFill fill(K::GnuplotFillStyle::EMPTY, K::GnuplotColor::fromRGB(0, 0, 0));
|
|
K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromRGB(255, 0, 0));
|
|
|
|
K::GnuplotObjectCircle* obj = new K::GnuplotObjectCircle(c, r, fill, stroke);
|
|
|
|
splot.getObjects().set(id, obj);
|
|
|
|
}
|
|
|
|
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 setActivity(const int act) {
|
|
|
|
std::string activity = "Unkown";
|
|
if(act == 0){
|
|
activity = "Standing";
|
|
} else if(act == 1) {
|
|
activity = "Walking";
|
|
} else if(act == 2) {
|
|
activity = "Up";
|
|
} else if(act == 3) {
|
|
activity = "Down";
|
|
}
|
|
|
|
gp << "set label 1002 at screen 0.02, 0.94 'Act: " << activity << "'\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 closeStream(){
|
|
gp.close();
|
|
}
|
|
|
|
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 printOverview(const std::string& path) {
|
|
gp << "set terminal png size 2000,1500\n";
|
|
gp << "set output '" << path << "_overview" << ".png'\n";
|
|
gp << "set view 75,60\n";
|
|
gp << "set autoscale xy\n";
|
|
gp << "set autoscale z\n";
|
|
}
|
|
|
|
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);
|
|
|
|
}
|
|
break;
|
|
}
|
|
|
|
// 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);
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
};
|