70 lines
1.2 KiB
C++
70 lines
1.2 KiB
C++
#ifndef PROBVIZ_H
|
|
#define PROBVIZ_H
|
|
|
|
#include <Indoor/floorplan/v2/Floorplan.h>
|
|
#include <Indoor/math/Interpolator.h>
|
|
|
|
#include <KLib/misc/gnuplot/Gnuplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
|
#include <KLib/misc/gnuplot/GnuplotSplotElementHeatMap.h>
|
|
|
|
#include <Indoor/math/Distributions.h>
|
|
#include <vector>
|
|
|
|
class ProbViz {
|
|
|
|
|
|
K::Gnuplot gp;
|
|
K::GnuplotSplot plot;
|
|
|
|
K::GnuplotSplotElementHeatMap heat;
|
|
|
|
public:
|
|
|
|
ProbViz() {
|
|
plot.add(&heat);
|
|
}
|
|
|
|
template <typename T> void show(std::vector<T>& particles) {
|
|
|
|
heat.clear();
|
|
|
|
float x1 = -0;
|
|
float x2 = +20;
|
|
float y1 = -0;
|
|
float y2 = +20;
|
|
float s = 0.5;
|
|
|
|
plot.getAxisX().setRange(x1,x2);
|
|
plot.getAxisY().setRange(y1,y2);
|
|
|
|
Distribution::Normal<double> nd(0.0, 1.0);
|
|
|
|
for (float y = y1; y < y2; y+=s) {
|
|
for (float x = x1; x < x2; x+=s) {
|
|
|
|
const Point2 p1(x,y);
|
|
double prob = 1;
|
|
|
|
for (const T& p : particles) {
|
|
const Point2 p2 = p.pos.xy();
|
|
const float dist = p1.getDistance(p2);
|
|
prob += nd.getProbability(dist);
|
|
}
|
|
prob /= particles.size();
|
|
|
|
K::GnuplotPoint3 p3(x,y,prob);
|
|
heat.add(p3);
|
|
|
|
}
|
|
}
|
|
|
|
gp.draw(plot);
|
|
gp.flush();
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // PROBVIZ_H
|