160 lines
3.7 KiB
C++
160 lines
3.7 KiB
C++
#ifndef VIS_H
|
|
#define VIS_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 <Indoor/geo/Length.h>
|
|
#include <Indoor/floorplan/Floor.h>
|
|
|
|
#include "eval/GroundTruthWay.h"
|
|
|
|
class Vis {
|
|
|
|
public:
|
|
|
|
K::Gnuplot gp;
|
|
K::GnuplotSplot splot;
|
|
K::GnuplotSplotElementLines floors;
|
|
K::GnuplotSplotElementColorPoints gridNodes;
|
|
K::GnuplotSplotElementLines gridEdges;
|
|
K::GnuplotSplotElementPoints particles;
|
|
K::GnuplotSplotElementLines groundTruth;
|
|
K::GnuplotSplotElementLines estPath;
|
|
|
|
public:
|
|
|
|
Vis() {
|
|
|
|
gp << "set hidden3d front\n";
|
|
//gp << "set view equal xy\n";
|
|
gp << "set ticslevel 0\n";
|
|
gp << "set cbrange[0.8:2.0]\n";
|
|
|
|
gp << "unset xtics\n";
|
|
gp << "unset ytics\n";
|
|
gp << "unset ztics\n";
|
|
gp << "unset border\n";
|
|
|
|
groundTruth.setLineWidth(2);
|
|
groundTruth.setColorHex("#666666");
|
|
|
|
estPath.setLineWidth(2);
|
|
|
|
// attach all layers
|
|
splot.add(&floors);
|
|
splot.add(&gridNodes);
|
|
splot.add(&gridEdges);
|
|
splot.add(&particles);
|
|
splot.add(&groundTruth);
|
|
splot.add(&estPath);
|
|
|
|
}
|
|
|
|
/** add all obstacles of the given floor to the provided height */
|
|
Vis& addFloor(const Floor& f, const LengthF height) {
|
|
|
|
// add each wall
|
|
for (const Line2& l : f.getObstacles()) {
|
|
const K::GnuplotPoint3 p1(l.p1.x, l.p1.y, height.cm());
|
|
const K::GnuplotPoint3 p2(l.p2.x, l.p2.y, height.cm());
|
|
floors.addSegment(p1, p2);
|
|
}
|
|
|
|
return *this;
|
|
|
|
}
|
|
|
|
/** add the grid to the plot */
|
|
template <typename T> Vis& addGrid(Grid<T>& grid) {
|
|
|
|
float max = 0;
|
|
for (const T& n1 : grid) {
|
|
if (n1.distToTarget > max) {max = n1.distToTarget;}
|
|
}
|
|
gp << "set cbrange[0.0:1.0]\n";
|
|
//gp << "set cbrange[0.8:1.3]\n";
|
|
|
|
for (const T& n1 : grid) {
|
|
const K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm);
|
|
//const float color = n1.imp;
|
|
//const float color = n1.distToTarget/max;
|
|
const float color = 0;
|
|
gridNodes.add(p1, color);
|
|
// for (const T& n2 : grid.neighbors(n1)) {
|
|
// const K::GnuplotPoint3 p2(n2.x_cm, n2.y_cm, n2.z_cm);
|
|
// gridEdges.addSegment(p1, p2);
|
|
// }
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
void addGroundTruth(GroundTruthWay& gtw) {
|
|
groundTruth.clear();
|
|
for (auto it : gtw.getWay()) {
|
|
K::GnuplotPoint3 gp(it.value.x, it.value.y, it.value.z);
|
|
groundTruth.add(gp);
|
|
}
|
|
}
|
|
|
|
void addEstPath(std::vector<Point3>& est) {
|
|
estPath.clear();;
|
|
for (const Point3& p : est) {
|
|
K::GnuplotPoint3 gp(p.x, p.y, p.z);
|
|
estPath.add(gp);
|
|
}
|
|
}
|
|
|
|
void setTimestamp(uint64_t ts) {
|
|
static uint64_t firstTs = ts;
|
|
gp << "set label 1 \"" << ((ts-firstTs)/1000.0f) << "\" at screen 0.02,0.98\n";
|
|
}
|
|
|
|
void removeGrid() {
|
|
gridNodes.clear();;
|
|
}
|
|
|
|
void clearStates() {
|
|
particles.clear();
|
|
}
|
|
|
|
void addObject(const int idx, const Point3& p) {
|
|
gp << "set object " << idx << " polygon ";
|
|
gp << "from " << p.x << "," << p.y << "," << p.z;
|
|
gp << " to " << p.x << "," << p.y << "," << p.z + 200;
|
|
gp << " to " << p.x << "," << p.y << "," << p.z; // close
|
|
gp << " lw 2 ";
|
|
gp << "\n";
|
|
}
|
|
|
|
void setEstAndShould(const Point3& est, const Point3& should) {
|
|
addObject(2,est);
|
|
addObject(3,should);
|
|
}
|
|
|
|
template <typename T> void addState(const GridWalkState<T>& n) {
|
|
particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
|
|
}
|
|
|
|
template <typename T> Vis& showStates(std::vector<GridWalkState<T>>& states) {
|
|
particles.clear();;
|
|
for (const GridWalkState<T>& n : states) {
|
|
particles.add(K::GnuplotPoint3(n.node->x_cm, n.node->y_cm, n.node->z_cm));
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
/** show (plot) the current setup */
|
|
void show() {
|
|
gp.draw(splot);
|
|
gp.flush();
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // VIS_H
|