added nav-mesh support via demo

navMesh/filter.h

@@ -0,0 +1,184 @@
+#ifndef NAV_MESH_FILTER_H
+#define NAV_MESH_FILTER_H
+
+#include "mesh.h"
+
+#include <Indoor/geo/Heading.h>
+#include <Indoor/math/Distributions.h>
+
+#include <KLib/math/filter/particles/Particle.h>
+#include <KLib/math/filter/particles/ParticleFilter.h>
+#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
+#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
+
+#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
+#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
+
+struct MyState {
+
+	/** the state's position (within the mesh) */
+	MyNavMeshLocation pos;
+
+	/** the state's heading */
+	Heading heading;
+
+	MyState() : pos(), heading(0) {;}
+
+	MyState& operator += (const MyState& o) {
+		pos.tria = nullptr;	// impossible
+		pos.pos += o.pos.pos;
+		return *this;
+	}
+
+	MyState& operator /= (const double val) {
+		pos.tria = nullptr;	// impossible
+		pos.pos /= val;
+		return *this;
+	}
+
+	MyState operator * (const double val) const {
+		MyState res;
+		res.pos.pos = pos.pos * val;
+		return res;
+	}
+
+};
+
+struct MyControl {
+
+	int numStepsSinceLastEval = 0;
+	float headingChangeSinceLastEval = 0;
+
+	void afterEval() {
+		numStepsSinceLastEval = 0;
+		headingChangeSinceLastEval = 0;
+	}
+
+
+};
+
+struct MyObservation {
+
+
+
+};
+
+class MyPFInitUniform : public K::ParticleFilterInitializer<MyState> {
+
+	const MyNavMesh* mesh;
+
+public:
+
+	MyPFInitUniform(const MyNavMesh* mesh) : mesh(mesh) {
+		;
+	}
+
+	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+
+		/** random position and heading within the mesh */
+		Distribution::Uniform<float> dHead(0, 2*M_PI);
+		MyNavMeshRandom rnd = mesh->getRandom();
+		for (K::Particle<MyState>& p : particles) {
+			p.state.pos = rnd.draw();
+			p.state.heading = dHead.draw();
+			p.weight = 1.0 / particles.size();
+		}
+	}
+
+};
+
+class MyPFInitFixed : public K::ParticleFilterInitializer<MyState> {
+
+	const MyNavMesh* mesh;
+	const Point3 pos;
+
+public:
+
+	MyPFInitFixed(const MyNavMesh* mesh, const Point3 pos) : mesh(mesh), pos(pos) {
+		;
+	}
+
+	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+
+		/** random position and heading within the mesh */
+		Distribution::Uniform<float> dHead(0, 2*M_PI);
+		for (K::Particle<MyState>& p : particles) {
+			p.state.pos = mesh->getLocation(pos);
+			p.state.heading = dHead.draw();
+			p.weight = 1.0 / particles.size();
+		}
+	}
+
+};
+
+
+class MyPFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
+
+	using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
+	MyNavMeshWalk walker;
+
+public:
+
+	MyPFTrans(MyNavMesh& mesh) : walker(mesh) {
+
+		// how to evaluate drawn points
+		//walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
+		//walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
+		walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9));		// 90% for particles moving towards the target
+
+	}
+
+	void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
+
+		Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
+		Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
+		Distribution::Normal<float> dHeading(0.0, 0.10);
+
+
+		for (K::Particle<MyState>& p : particles) {
+
+			// how to walk
+			MyNavMeshWalkParams params;
+			params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw();
+			params.numSteps = control->numStepsSinceLastEval;
+			params.start = p.state.pos;
+			params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
+			params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
+
+			// walk
+			MyNavMeshWalk::ResultEntry res = walker.getOne(params);
+
+			// assign back to particle's state
+			p.weight *= res.probability;
+			p.state.pos = res.location;
+			p.state.heading = res.heading;
+
+		}
+
+		// reset the control (0 steps, 0 delta-heading)
+		//control->afterEval();
+
+	}
+
+
+};
+
+class MyPFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
+
+public:
+
+	virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
+
+		return 1.0;
+
+	}
+
+};
+
+
+using MyFilter = K::ParticleFilter<MyState, MyControl, MyObservation>;
+
+
+#endif

navMesh/main.h

@@ -0,0 +1,94 @@
+#ifndef NAV_MESH_MAIN_H
+#define NAV_MESH_MAIN_H
+
+#include "mesh.h"
+#include "filter.h"
+#include <memory>
+#include <thread>
+#include <Indoor/floorplan/v2/FloorplanReader.h>
+
+void navMeshMain() {
+
+	std::string mapFile = "/apps/paper/diss/data/maps/museum31.xml";
+
+	Floorplan::IndoorMap* map = Floorplan::Reader::readFromFile(mapFile);
+
+	NM::NavMeshSettings set;
+	MyNavMesh mesh;
+	MyNavMeshFactory fac(&mesh, set);
+	fac.build(map);
+
+	const Point3 src(26, 43, 7.5);
+
+	// add shortest-path to destination
+	//const Point3 dst(51, 45, 1.7);
+	const Point3 dst(25, 45, 0);
+	NM::NavMeshDijkstra::stamp<MyNavMeshTriangle>(mesh, dst);
+
+	// debug show
+	NM::NavMeshDebug dbg;
+	dbg.addMesh(mesh);
+	//dbg.addDijkstra(mesh);
+	dbg.draw();
+
+	// particle-filter
+	const int numParticles = 1000;
+	auto init = std::make_unique<MyPFInitFixed>(&mesh, src);	// known position
+	//auto init = std::make_unique<MyPFInitUniform>(&mesh);		// uniform distribution
+	auto eval = std::make_unique<MyPFEval>();
+	auto trans = std::make_unique<MyPFTrans>(mesh);
+	auto resample = std::make_unique<K::ParticleFilterResamplingSimple<MyState>>();
+	auto estimate = std::make_unique<K::ParticleFilterEstimationWeightedAverage<MyState>>();
+
+	// setup
+	MyFilter pf(numParticles, std::move(init));
+	pf.setEvaluation(std::move(eval));
+	pf.setTransition(std::move(trans));
+	pf.setResampling(std::move(resample));
+	pf.setEstimation(std::move(estimate));
+	pf.setNEffThreshold(1);
+
+
+	MyControl ctrl;
+	MyObservation obs;
+
+	//Distribution::Uniform<float> dHead(0, 2*M_PI);
+	Distribution::Normal<float> dHead(0, 0.1);
+
+	for (int i = 0; i < 10000; ++i) {
+
+		ctrl.numStepsSinceLastEval = 1;
+		ctrl.headingChangeSinceLastEval = dHead.draw();
+
+		MyState est = pf.update(&ctrl, obs);
+
+		ctrl.afterEval();
+
+		try {
+			MyNavMeshLocation loc = mesh.getLocationNearestTo(est.pos.pos);
+			auto path = loc.tria->getPathToDestination<MyNavMeshTriangle>(loc.pos);
+			dbg.addDijkstra(path);
+		} catch (...) {;}
+
+		const int d = (i * 1) % 360;
+		dbg.plot.getView().setCamera(60, d);
+		dbg.showParticles(pf.getParticles());
+		dbg.setCurPos(est.pos.pos);
+
+		//dbg.gp.setOutput("/tmp/123/" + std::to_string(i) + ".png");
+		//dbg.gp.setTerminal("pngcairo", K::GnuplotSize(60, 30));
+
+		std::cout << i << std::endl;
+
+		dbg.draw();
+
+
+
+		std::this_thread::sleep_for(std::chrono::milliseconds(5));
+
+	}
+
+
+}
+
+#endif

navMesh/mesh.h

@@ -0,0 +1,32 @@
+#ifndef NAV_MESH_MESH_H
+#define NAV_MESH_MESH_H
+
+
+#include <Indoor/navMesh/NavMesh.h>
+#include <Indoor/navMesh/NavMeshLocation.h>
+#include <Indoor/navMesh/NavMeshRandom.h>
+#include <Indoor/navMesh/NavMeshFactory.h>
+
+#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
+#include <Indoor/navMesh/meta/NavMeshDijkstra.h>
+
+/** the triangle to use with the nav-mesh */
+class MyNavMeshTriangle : public NM::NavMeshTriangle, public NM::NavMeshTriangleDijkstra {
+
+	// add own parameters here
+
+public:
+
+	MyNavMeshTriangle(const Point3 p1, const Point3 p2, const Point3 p3, uint8_t type) : NM::NavMeshTriangle(p1, p2, p3, type) {
+		;
+	}
+
+};
+
+using MyNavMeshFactory = NM::NavMeshFactory<MyNavMeshTriangle>;
+using MyNavMesh = NM::NavMesh<MyNavMeshTriangle>;
+using MyNavMeshLocation = NM::NavMeshLocation<MyNavMeshTriangle>;
+using MyNavMeshRandom = NM::NavMeshRandom<MyNavMeshTriangle>;
+using MyNavMeshWalkParams = NM::NavMeshWalkParams<MyNavMeshTriangle>;
+
+#endif