220 lines
6.6 KiB
C++
220 lines
6.6 KiB
C++
#ifndef WALKVIZ3_H
|
|
#define WALKVIZ3_H
|
|
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include <Indoor/synthetic/SyntheticPath.h>
|
|
#include <Indoor/grid/Grid.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/objects/GnuplotObjectPolygon.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
|
|
|
|
struct WalkViz3 {
|
|
|
|
K::Gnuplot gp;
|
|
K::GnuplotSplot plot;
|
|
|
|
K::GnuplotSplotElementPoints nodes;
|
|
K::GnuplotSplotElementLines edges;
|
|
|
|
K::GnuplotSplotElementColorPoints nodesCol;
|
|
|
|
K::GnuplotSplotElementLines outline;
|
|
K::GnuplotSplotElementLines obstacles;
|
|
K::GnuplotSplotElementColorPoints particles;
|
|
K::GnuplotSplotElementLines path;
|
|
K::GnuplotSplotElementLines pathEstimated;
|
|
K::GnuplotObjectPolygon oPos;
|
|
|
|
WalkViz3() {
|
|
plot.add(&obstacles);
|
|
plot.add(&outline);
|
|
|
|
plot.add(&edges); edges.getStroke().getColor().setHexStr("#ff0000");
|
|
plot.add(&nodes); nodes.setPointType(7); nodes.setPointSize(0.4); nodes.getColor().setHexStr("#999999");
|
|
|
|
plot.add(&nodesCol); nodesCol.setPointType(7); nodesCol.setPointSize(0.4);
|
|
|
|
plot.add(&particles); particles.setPointType(7); particles.setPointSize(0.2);
|
|
plot.add(&path); path.getStroke().setWidth(2); path.setShowPoints(true);
|
|
plot.add(&pathEstimated); pathEstimated.getStroke().setWidth(2); pathEstimated.setShowPoints(false); pathEstimated.getStroke().getColor().setHexStr("#00ff00");
|
|
|
|
plot.getObjects().add(&oPos);
|
|
|
|
//gp << "set size ratio -1\n";
|
|
//gp << "set view equal xy\n";
|
|
//gp << "set view equal xyz\n";
|
|
|
|
plot.getView().setEnabled(false);
|
|
}
|
|
|
|
void add(Floorplan::Floor* floor, Floorplan::FloorOutlinePolygon* poly) {
|
|
Floorplan::Polygon2 pol2 = poly->poly;
|
|
const int num = pol2.points.size();
|
|
for (int i = 0; i < num; ++i) {
|
|
const Point2 pt1 = pol2.points[(i+0)];
|
|
const Point2 pt2 = pol2.points[(i+1)%num];
|
|
K::GnuplotPoint3 gp1(pt1.x, pt1.y, floor->atHeight);
|
|
K::GnuplotPoint3 gp2(pt2.x, pt2.y, floor->atHeight);
|
|
outline.addSegment(gp1, gp2);
|
|
}
|
|
}
|
|
|
|
void addObstacle(Floorplan::Floor* floor, Floorplan::FloorObstacle* obs) {
|
|
|
|
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
|
Floorplan::FloorObstacleCircle* circle = dynamic_cast<Floorplan::FloorObstacleCircle*>(obs);
|
|
|
|
if (line) {
|
|
K::GnuplotPoint3 gp1(line->from.x, line->from.y, floor->atHeight);
|
|
K::GnuplotPoint3 gp2(line->to.x, line->to.y, floor->atHeight);
|
|
obstacles.addSegment(gp1,gp2);
|
|
}
|
|
|
|
}
|
|
|
|
void showPos(const Point3 pos) {
|
|
oPos.clear();
|
|
oPos.add( K::GnuplotCoordinate3(pos.x-1,pos.y-1,pos.z, K::GnuplotCoordinateSystem::FIRST) );
|
|
oPos.add( K::GnuplotCoordinate3(pos.x+1,pos.y-1,pos.z, K::GnuplotCoordinateSystem::FIRST) );
|
|
oPos.add( K::GnuplotCoordinate3(pos.x+0,pos.y+1,pos.z, K::GnuplotCoordinateSystem::FIRST) );
|
|
oPos.close();
|
|
}
|
|
|
|
template <typename T> void showParticlesGrid(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.x, p.state.pos.y, p.state.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 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.x, p.state.pos.y, p.state.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);
|
|
}
|
|
|
|
void addFloor(Floorplan::Floor* floor) {
|
|
for (Floorplan::FloorObstacle* obs : floor->obstacles) {
|
|
addObstacle(floor, obs);
|
|
}
|
|
for (Floorplan::Stair* s : floor->stairs) {
|
|
addStair(floor, s);
|
|
}
|
|
}
|
|
|
|
void addStair(Floorplan::Floor* floor, Floorplan::Stair* s) {
|
|
|
|
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(s->getParts(), floor);
|
|
for (const Floorplan::Quad3& quad : quads) {
|
|
K::GnuplotPoint3 gp1(quad.p1.x, quad.p1.y, quad.p1.z);
|
|
K::GnuplotPoint3 gp2(quad.p2.x, quad.p2.y, quad.p2.z);
|
|
K::GnuplotPoint3 gp3(quad.p3.x, quad.p3.y, quad.p3.z);
|
|
K::GnuplotPoint3 gp4(quad.p4.x, quad.p4.y, quad.p4.z);
|
|
obstacles.addSegment(gp1,gp2);
|
|
obstacles.addSegment(gp2,gp3);
|
|
obstacles.addSegment(gp3,gp4);
|
|
obstacles.addSegment(gp4,gp1);
|
|
}
|
|
|
|
}
|
|
|
|
void addMap(Floorplan::IndoorMap* map) {
|
|
for (Floorplan::Floor* floor : map->floors) {
|
|
addFloor(floor);
|
|
}
|
|
}
|
|
|
|
void setGT(const Point3 pt) {
|
|
gp << "set arrow 31337 from " << pt.x << "," << pt.y << "," << (pt.z+1.7) << " 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+1.7) << " to " << pt.x << "," << pt.y << "," << pt.z << " front \n";
|
|
}
|
|
|
|
|
|
template <typename Node> void addGrid(Grid<Node>& grid, bool addEdges = false) {
|
|
nodes.clear();
|
|
edges.clear();
|
|
for (const Node& n : grid) {
|
|
const K::GnuplotPoint3 gp(n.x_cm/100.0f, n.y_cm/100.0f, n.z_cm/100.0f);
|
|
nodes.add(gp);
|
|
}
|
|
if (addEdges) {
|
|
for (const Node& n : grid) {
|
|
for (const Node& n2 : grid.neighbors(n)) {
|
|
if (n.getIdx() < n2.getIdx()) { // uni-directional.. prevent duplicates
|
|
edges.addSegment(
|
|
K::GnuplotPoint3 (n.x_cm, n.y_cm, n.z_cm) / 100.0f,
|
|
K::GnuplotPoint3 (n2.x_cm, n2.y_cm, n2.z_cm) / 100.0f
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename Node> void addGridWalkImp(Grid<Node>& grid, bool addEdges = false) {
|
|
double pMin = +999;
|
|
double pMax = -999;
|
|
for (const Node& n : grid) {
|
|
const K::GnuplotPoint3 gp(n.x_cm/100.0f, n.y_cm/100.0f, n.z_cm/100.0f);
|
|
const double p = n.getWalkImportance();
|
|
nodesCol.add(gp, p);
|
|
if (p > pMax) {pMax = p;}
|
|
if (p < pMin) {pMin = p;}
|
|
}
|
|
if (addEdges) {
|
|
for (const Node& n : grid) {
|
|
for (const Node& n2 : grid.neighbors(n)) {
|
|
if (n.getIdx() < n2.getIdx()) { // uni-directional.. prevent duplicates
|
|
edges.addSegment(
|
|
K::GnuplotPoint3 (n.x_cm, n.y_cm, n.z_cm) / 100.0f,
|
|
K::GnuplotPoint3 (n2.x_cm, n2.y_cm, n2.z_cm) / 100.0f
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
plot.getAxisCB().setRange(pMin, pMax);
|
|
}
|
|
|
|
void showPath(const SyntheticPath& interpol) {
|
|
for (const auto& entry : interpol.getEntries()) {
|
|
const Point3 p = entry.value;
|
|
K::GnuplotPoint3 gp(p.x, p.y, p.z);
|
|
path.add(gp);
|
|
}
|
|
}
|
|
|
|
void draw() {
|
|
gp.draw(plot);
|
|
gp.flush();
|
|
}
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // WALKVIZ3_H
|