added walky

walky/ProbViz.h

@@ -0,0 +1,69 @@
+#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