349 lines
12 KiB
C++
349 lines
12 KiB
C++
#pragma once
|
|
|
|
#include "Settings.h"
|
|
|
|
#include <functional>
|
|
|
|
#include <Indoor/geo/Point2.h>
|
|
#include <Indoor/geo/Point3.h>
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
|
|
#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
|
|
#include <Indoor/sensors/radio/WiFiProbabilityGrid.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/smc/filtering/ParticleFilter.h>
|
|
|
|
struct Plotti {
|
|
|
|
K::Gnuplot gp;
|
|
K::GnuplotSplot splot;
|
|
K::GnuplotSplotElementPoints pGrid;
|
|
K::GnuplotSplotElementLines pFloor;
|
|
K::GnuplotSplotElementLines pOutline;
|
|
K::GnuplotSplotElementLines pStairs;
|
|
K::GnuplotSplotElementPoints pAPs;
|
|
K::GnuplotSplotElementPoints pInterest;
|
|
K::GnuplotSplotElementPoints pParticles;
|
|
K::GnuplotSplotElementPoints pNormal1;
|
|
K::GnuplotSplotElementPoints pNormal2;
|
|
K::GnuplotSplotElementColorPoints pDistributation1;
|
|
K::GnuplotSplotElementColorPoints pDistributation2;
|
|
K::GnuplotSplotElementColorPoints pColorPoints;
|
|
K::GnuplotSplotElementLines gtPath;
|
|
K::GnuplotSplotElementLines estPath;
|
|
K::GnuplotSplotElementLines estPathSmoothed;
|
|
|
|
Plotti() {
|
|
gp << "set xrange[0-50:70+50]\nset yrange[0-50:50+50]\nset ticslevel 0\n";
|
|
splot.add(&pGrid); pGrid.setPointSize(0.25); pGrid.getColor().setHexStr("#888888");
|
|
splot.add(&pAPs); pAPs.setPointSize(0.7);
|
|
splot.add(&pColorPoints); pColorPoints.setPointSize(0.6);
|
|
splot.add(&pDistributation1); pDistributation1.setPointSize(0.6);
|
|
splot.add(&pDistributation2); pDistributation2.setPointSize(0.6);
|
|
splot.add(&pParticles); pParticles.getColor().setHexStr("#0000ff"); pParticles.setPointSize(0.4f);
|
|
splot.add(&pNormal1); pNormal1.getColor().setHexStr("#ff00ff"); pNormal1.setPointSize(0.4f);
|
|
splot.add(&pNormal2); pNormal2.getColor().setHexStr("#00aaff"); pNormal2.setPointSize(0.4f);
|
|
splot.add(&pFloor);
|
|
splot.add(&pOutline); pOutline.getStroke().getColor().setHexStr("#999999");
|
|
splot.add(&pStairs); pStairs.getStroke().getColor().setHexStr("#000000");
|
|
splot.add(&pInterest); pInterest.setPointSize(2); pInterest.getColor().setHexStr("#ff0000");
|
|
splot.add(>Path); gtPath.getStroke().setWidth(2); gtPath.getStroke().getColor().setHexStr("#000000");
|
|
splot.add(&estPath); estPath.getStroke().setWidth(2); estPath.getStroke().getColor().setHexStr("#00ff00");
|
|
splot.add(&estPathSmoothed); estPathSmoothed.getStroke().setWidth(2); estPathSmoothed.getStroke().getColor().setHexStr("#0000ff");
|
|
}
|
|
|
|
void addLabel(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
|
|
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "'\n";
|
|
}
|
|
|
|
void addLabelV(const int idx, const Point3 p, const std::string& str, const int fontSize = 10) {
|
|
gp << "set label " << idx << " at " << p.x << "," << p.y << "," << p.z << "'" << str << "'" << " font '," << fontSize << "' rotate by 90\n";
|
|
}
|
|
|
|
|
|
void showAngle(const int idx, const float rad, const Point2 cen, const std::string& str) {
|
|
|
|
Point2 rot(0, 1);
|
|
Point2 pos = cen + rot.rotated(rad) * 0.05;
|
|
|
|
gp << "set label "<<idx<<" at screen " << cen.x << "," << cen.y << " '" << str << "'"<< "\n";
|
|
gp << "set arrow "<<idx<<" from screen " << cen.x << "," << cen.y << " to screen " << pos.x << "," << pos.y << "\n";
|
|
|
|
}
|
|
|
|
void setEst(const Point3 pos) {
|
|
gp << "set arrow 991 from " << pos.x << "," << pos.y << "," << std::round(pos.z * 10) / 10 << " to " << pos.x << "," << pos.y << "," << (std::round(pos.z * 10) / 10)+1 << " nohead lw 1 front \n";
|
|
}
|
|
|
|
void setGT(const Point3 pos) {
|
|
gp << "set arrow 995 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x << "," << pos.y << "," << pos.z+0.3 << " nohead lw 3 front \n";
|
|
gp << "set arrow 996 from " << pos.x << "," << pos.y << "," << pos.z << " to " << pos.x+0.3 << "," << pos.y << "," << pos.z << " nohead lw 3 front \n";
|
|
}
|
|
|
|
void setTimeInMinute(const int minutes, const int seconds) {
|
|
gp << "set label 1002 at screen 0.02, 0.94 'Time: " << minutes << ":" << seconds << "'\n";
|
|
}
|
|
|
|
void addGroundTruthNode(const Point3 pos) {
|
|
K::GnuplotPoint3 gp(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
|
gtPath.add(gp);
|
|
}
|
|
|
|
// estimated path
|
|
void addEstimationNode(const Point3 pos){
|
|
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
|
estPath.add(est);
|
|
}
|
|
|
|
// estimated path
|
|
void addEstimationNodeSmoothed(const Point3 pos){
|
|
K::GnuplotPoint3 est(pos.x, pos.y, std::round(pos.z * 10) / 10);
|
|
estPathSmoothed.add(est);
|
|
}
|
|
|
|
void debugDistribution1(std::vector<SMC::Particle<MyState>> samples){
|
|
|
|
float min = +9999;
|
|
float max = -9999;
|
|
|
|
pDistributation1.clear();
|
|
for (int i = 0; i < samples.size(); ++i) {
|
|
//if (i % 10 != 0) {continue;}
|
|
|
|
double prob = samples[i].weight;
|
|
if (prob < min) {min = prob;}
|
|
if (prob > max) {max = prob;}
|
|
|
|
K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
|
|
pDistributation1.add(pos, prob);
|
|
}
|
|
|
|
if (min == max) {min -= 1;}
|
|
gp << "set cbrange [" << min << ":" << max << "]\n";
|
|
}
|
|
|
|
void debugDistribution2(std::vector<SMC::Particle<MyState>> samples){
|
|
|
|
float min = +9999;
|
|
float max = -9999;
|
|
|
|
pDistributation2.clear();
|
|
for (int i = 0; i < samples.size(); ++i) {
|
|
if (i % 25 != 0) {continue;}
|
|
|
|
double prob = samples[i].weight;
|
|
if (prob < min) {min = prob;}
|
|
if (prob > max) {max = prob;}
|
|
|
|
K::GnuplotPoint3 pos(samples[i].state.position.x_cm / 100.0f, samples[i].state.position.y_cm / 100.0f, samples[i].state.position.z_cm / 100.0f);
|
|
pDistributation2.add(pos, prob);
|
|
}
|
|
|
|
if (min == max) {min -= 1;}
|
|
gp << "set cbrange [" << min << ":" << max << "]\n";
|
|
}
|
|
|
|
void drawNormalN1(Distribution::NormalDistributionN normParticle){
|
|
|
|
pNormal1.clear();
|
|
for (int i = 0; i < 100000; ++i) {
|
|
if (++i % 25 != 0) {continue;}
|
|
Eigen::VectorXd vec = normParticle.draw();
|
|
K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
|
|
pNormal1.add(pos);
|
|
}
|
|
|
|
}
|
|
|
|
void drawNormalN2(Distribution::NormalDistributionN normParticle){
|
|
|
|
pNormal2.clear();
|
|
for (int i = 0; i < 100000; ++i) {
|
|
if (++i % 25 != 0) {continue;}
|
|
Eigen::VectorXd vec = normParticle.draw();
|
|
K::GnuplotPoint3 pos(vec.x(), vec.y(), vec.z());
|
|
pNormal2.add(pos);
|
|
}
|
|
}
|
|
|
|
void debugWiFi(WiFiModelLogDistCeiling& model, const WiFiMeasurements& scan, const Timestamp curTS, const float z) {
|
|
|
|
WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, model);
|
|
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(scan);
|
|
|
|
float min = +9999;
|
|
float max = -9999;
|
|
|
|
const float step = 2.0f;
|
|
for (float x = 0; x < 80; x += step) {
|
|
for (float y = 0; y < 55; y += step) {
|
|
Point3 pt(x,y,z);
|
|
double prob = wiFiProbability.getProbability(pt + Point3(0,0,1.3), curTS, wifiObs);
|
|
|
|
if (prob < min) {min = prob;}
|
|
if (prob > max) {max = prob;}
|
|
pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
|
|
}
|
|
}
|
|
|
|
if (min == max) {min -= 1;}
|
|
gp << "set cbrange [" << min << ":" << max << "]\n";
|
|
|
|
}
|
|
|
|
template <typename Node> void debugProb(Grid<Node>& grid, std::function<double(const MyObs&, const Point3& pos)> func, const MyObs& obs) {
|
|
|
|
pColorPoints.clear();
|
|
|
|
// const float step = 2.0;
|
|
// float z = 0;
|
|
// for (float x = -20; x < 90; x += step) {
|
|
// for (float y = -10; y < 60; y += step) {
|
|
// const Point3 pos_m(x,y,z);
|
|
// const double prob = func(obs, pos_m);
|
|
// pColorPoints.add(K::GnuplotPoint3(x,y,z), prob);
|
|
// }
|
|
// }
|
|
|
|
std::minstd_rand gen;
|
|
std::uniform_int_distribution<int> dist(0, grid.getNumNodes()-1);
|
|
|
|
float min = +9999;
|
|
float max = -9999;
|
|
|
|
for (int i = 0; i < 10000; ++i) {
|
|
int idx = dist(gen);
|
|
Node& n = grid[idx];
|
|
const Point3 pos_cm(n.x_cm, n.y_cm, n.z_cm);
|
|
const Point3 pos_m = pos_cm / 100.0f;
|
|
const double prob = func(obs, pos_m);
|
|
if (prob < min) {min = prob;}
|
|
if (prob > max) {max = prob;}
|
|
pColorPoints.add(K::GnuplotPoint3(pos_m.x, pos_m.y, pos_m.z), prob);
|
|
}
|
|
|
|
if (min == max) {min -= 1;}
|
|
|
|
gp << "set cbrange [" << min << ":" << max << "]\n";
|
|
|
|
}
|
|
|
|
void addStairs(Floorplan::IndoorMap* map) {
|
|
|
|
for (Floorplan::Floor* f : map->floors) {
|
|
for (Floorplan::Stair* stair : f->stairs) {
|
|
std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(stair->getParts(), f);
|
|
for (const Floorplan::Quad3& quad : quads) {
|
|
for (int i = 0; i < 4; ++i) {
|
|
int idx1 = i;
|
|
int idx2 = (i+1) % 4;
|
|
pStairs.addSegment(
|
|
K::GnuplotPoint3(quad[idx1].x,quad[idx1].y, quad[idx1].z),
|
|
K::GnuplotPoint3(quad[idx2].x,quad[idx2].y, quad[idx2].z)
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void addFloors(Floorplan::IndoorMap* map) {
|
|
|
|
for (Floorplan::Floor* f : map->floors) {
|
|
for (Floorplan::FloorObstacle* obs : f->obstacles) {
|
|
Floorplan::FloorObstacleLine* line = dynamic_cast<Floorplan::FloorObstacleLine*>(obs);
|
|
if (line) {
|
|
K::GnuplotPoint3 p1(line->from.x, line->from.y, f->atHeight);
|
|
K::GnuplotPoint3 p2(line->to.x, line->to.y, f->atHeight);
|
|
pFloor.addSegment(p1, p2);
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void addOutline(Floorplan::IndoorMap* map) {
|
|
|
|
for (Floorplan::Floor* f : map->floors) {
|
|
for (Floorplan::FloorOutlinePolygon* poly : f->outline) {
|
|
const int cnt = poly->poly.points.size();
|
|
for (int i = 0; i < cnt; ++i) {
|
|
Point2 p1 = poly->poly.points[(i+0)];
|
|
Point2 p2 = poly->poly.points[(i+1)%cnt];
|
|
K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
|
|
K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
|
|
pOutline.addSegment(gp1, gp2);
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
template <typename Node> void addGrid(Grid<Node>& grid) {
|
|
pGrid.clear();
|
|
for (const Node& n : grid) {
|
|
K::GnuplotPoint3 p(n.x_cm, n.y_cm, n.z_cm);
|
|
pGrid.add(p/100.0f);
|
|
}
|
|
}
|
|
|
|
template <typename State> void addParticles(const std::vector<SMC::Particle<State>>& particles) {
|
|
pParticles.clear();
|
|
int i = 0;
|
|
for (const SMC::Particle<State>& p : particles) {
|
|
if (++i % 25 != 0) {continue;}
|
|
K::GnuplotPoint3 pos(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
|
|
pParticles.add(pos / 100.0f);
|
|
}
|
|
}
|
|
|
|
void show() {
|
|
gp.draw(splot);
|
|
gp.flush();
|
|
}
|
|
|
|
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 printSingleFloor(const std::string& path, const int floorNum) {
|
|
gp << "set terminal png size 1280,720\n";
|
|
gp << "set output '" << path << "_" << floorNum <<".png'\n";
|
|
gp << "set view 0,0\n";
|
|
gp << "set zrange [" << (floorNum * 4) - 2 << " : " << (floorNum * 4) + 2 << "]\n";
|
|
gp << "set autoscale xy\n";
|
|
}
|
|
|
|
void printSideView(const std::string& path, const int degree) {
|
|
gp << "set terminal png size 1280,720\n";
|
|
gp << "set output '" << path << "_deg" << degree <<".png'\n";
|
|
gp << "set view 90,"<< degree << "\n";
|
|
gp << "set autoscale xy\n";
|
|
gp << "set autoscale z\n";
|
|
}
|
|
|
|
void printOverview(const std::string& path) {
|
|
gp << "set terminal png size 1280,720\n";
|
|
gp << "set output '" << path << "_overview" << ".png'\n";
|
|
gp << "set view 75,60\n";
|
|
gp << "set autoscale xy\n";
|
|
gp << "set autoscale z\n";
|
|
}
|
|
|
|
|
|
};
|