merged

data/File.h

@@ -0,0 +1,115 @@
+#ifndef FS_FILE_H
+#define FS_FILE_H
+
+#include <string>
+#include <vector>
+
+
+
+
+#if __cplusplus > 201103L
+
+	// use new cross-platform std::experimental stuff
+	#include <experimental/filesystem>
+
+namespace FS {
+
+	class File {
+
+		std::experimental::filesystem::path path;
+
+	public:
+
+		File(const std::string& fileOrFolder) : path(fileOrFolder) {
+
+		}
+
+		bool exists() const {
+			return std::experimental::filesystem::exists(path);
+		}
+
+		std::vector<File> listFiles() const {
+			std::vector<File> res;
+			for (std::experimental::filesystem::directory_entry entry : std::experimental::filesystem::directory_iterator(path)) {
+				const std::string abs = entry.path().string();
+				res.push_back(File(abs));
+			}
+			return res;
+		}
+
+		std::string getPath() const {
+			return path.string();
+		}
+
+		std::string getFilename() const {
+			return path.filename().string();
+		}
+
+	};
+
+}
+
+#else
+
+	// use linux-only fallback
+
+	#include <dirent.h>
+	#include <sys/stat.h>
+
+namespace FS {
+
+	class File {
+
+		std::string path;
+
+	public:
+
+		File(const std::string& fileOrFolder) : path(fileOrFolder) {
+
+		}
+
+		bool exists() const {
+			struct stat st;
+			int res = stat(path.c_str(), &st );
+			return res == 0;
+		}
+
+		/** list of all files/folders within the current folder */
+		std::vector<File> listFiles() const {
+
+			std::vector<File> res;
+
+			DIR* dir;
+
+			dir = opendir(path.c_str());
+			if (!dir) {throw Exception("failed to open folder " + path);}
+
+			// fetch all entries
+			while(true) {
+				dirent* entry = readdir(dir);
+				if (!entry) {break;}
+				const std::string abs = path + "/" + entry->d_name;
+				res.push_back(File(abs));
+			}
+
+			return res;
+
+		}
+
+		const std::string& getPath() const {
+			return path;
+		}
+
+		const std::string getFilename() const {
+			const size_t lastSlash = path.find_last_of("/");
+			if (std::string::npos == lastSlash) {return path;}
+			std::string name = path.substr(lastSlash+1);
+			return name;
+		}
+
+	};
+
+}
+#endif
+
+#endif // FS_FILE_H

data/csv.h

@@ -0,0 +1,95 @@
+#ifndef CSV_H
+#define CSV_H
+
+#include <vector>
+#include <string>
+#include <fstream>
+#include "../misc/Debug.h"
+#include "../Exception.h"
+
+class CSV {
+
+	static constexpr const char* NAME = "CSV";
+
+public:
+
+	/** one column within the CSV */
+	struct Col {
+		std::string data;
+		Col(const std::string& data) : data(data) {;}
+		const std::string& asString() const {return data;}
+		int asInt() const {return std::stoi(data);}
+		long asLong() const {return std::stol(data);}
+		double asDouble() const {return std::stod(data);}
+	};
+
+	/** one row within the CSV */
+	struct Row : std::vector<Col> {
+
+	};
+
+	/** one csv document */
+	struct Doc : std::vector<Row> {
+
+	};
+
+
+	class Reader {
+
+	private:
+
+		const char sep;
+
+	public:
+
+		/** ctor */
+		Reader(const char sep) : sep(sep) {
+
+		}
+
+		/** read the given csv file */
+		Doc read(const std::string& file) {
+
+			Log::add(NAME, "reading file: " + file);
+
+			Doc doc;
+
+			std::ifstream inp(file.c_str());
+
+			// sanity check
+			if (!inp) {
+				throw Exception("failed to open file: " + file);
+			}
+
+			int rowCnt = 0;
+			std::string line;
+
+			// read all lines within the CSV
+			while(std::getline(inp, line)) {
+
+				++rowCnt;
+
+				// split
+				Row row;
+				std::stringstream ss(line);
+				std::string tmp;
+				while(getline(ss, tmp, sep)){
+					row.push_back(Col(tmp));
+				}
+
+				// apend row
+				doc.push_back(row);
+
+			}
+
+			Log::add(NAME, "got " + std::to_string(rowCnt) + " rows");
+
+			return doc;
+
+		}
+
+	};
+
+};
+
+#endif // CSV_H

floorplan/v2/Floorplan.h

@@ -349,8 +349,9 @@ namespace Floorplan {
 		Point2 from;
 		Point2 to;
 		float thickness_m;
-		FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m) {;}
+		float height_m = 0;	// 0 = floor's height
-		FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float thickness_m = 0.2f) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m) {;}
+		FloorObstacleLine(const ObstacleType type, const Material material, const Point2 from, const Point2 to, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(from), to(to), thickness_m(thickness_m), height_m(height_m) {;}
+		FloorObstacleLine(const ObstacleType type, const Material material, const float x1, const float y1, const float x2, const float y2, const float thickness_m = 0.2f, const float height_m = 0) : FloorObstacle(material), type(type), from(x1,y1), to(x2,y2), thickness_m(thickness_m), height_m(height_m) {;}
 	};
 
 	/** circle obstacle */
@@ -380,7 +381,8 @@ namespace Floorplan {
 		std::string file;
 		Point3 pos;
 		Point3 rot;
-		FloorObstacleObject(const std::string& file, const Point3 pos, const Point3 rot) : file(file), pos(pos), rot(rot) {;}
+		Point3 scale = Point3(1,1,1);
+		FloorObstacleObject(const std::string& file, const Point3 pos, const Point3 rot, const Point3 scale) : file(file), pos(pos), rot(rot), scale(scale) {;}
 	};
 
 

floorplan/v2/FloorplanReader.h

@@ -403,7 +403,8 @@ namespace Floorplan {
 				parseMaterial(el->Attribute("material")),
 				el->FloatAttribute("x1"), el->FloatAttribute("y1"),
 				el->FloatAttribute("x2"), el->FloatAttribute("y2"),
-				(el->FloatAttribute("thickness") > 0) ? (el->FloatAttribute("thickness")) : (0.15)	// default wall thickness in m
+				(el->FloatAttribute("thickness") > 0) ? (el->FloatAttribute("thickness")) : (0.15),	// default wall thickness in m
+				el->FloatAttribute("height")
 			);
 		}
 
@@ -433,7 +434,8 @@ namespace Floorplan {
 			return new FloorObstacleObject(
 				el->Attribute("file"),
 				Point3(el->FloatAttribute("x"), el->FloatAttribute("y"), el->FloatAttribute("z")),
-				Point3(el->FloatAttribute("rx"), el->FloatAttribute("ry"), el->FloatAttribute("rz"))
+				Point3(el->FloatAttribute("rx", 0), el->FloatAttribute("ry", 0), el->FloatAttribute("rz", 0)),
+				Point3(el->FloatAttribute("sx", 1), el->FloatAttribute("sy", 1), el->FloatAttribute("sz", 1))
 			);
 		}
 

floorplan/v2/FloorplanWriter.h

@@ -334,6 +334,7 @@ namespace Floorplan {
 			obstacle->SetAttribute("x2", line->to.x);
 			obstacle->SetAttribute("y2", line->to.y);
 			obstacle->SetAttribute("thickness", line->thickness_m);
+			if (line->height_m != 0) {obstacle->SetAttribute("height", line->height_m);}
 			obstacles->InsertEndChild(obstacle);
 		}
 
@@ -344,7 +345,7 @@ namespace Floorplan {
 			obstacle->SetAttribute("cx", circle->center.x);
 			obstacle->SetAttribute("cy", circle->center.y);
 			obstacle->SetAttribute("radius", circle->radius);
-			obstacle->SetAttribute("height", circle->height);
+			if (circle->height != 0) {obstacle->SetAttribute("height", circle->height);}
 			obstacles->InsertEndChild(obstacle);
 		}
 
@@ -369,9 +370,12 @@ namespace Floorplan {
 			obstacle->SetAttribute("x", obj->pos.x);
 			obstacle->SetAttribute("y", obj->pos.y);
 			obstacle->SetAttribute("z", obj->pos.z);
-			obstacle->SetAttribute("rx", obj->rot.x);
+			if (obj->rot.x != 0)	{obstacle->SetAttribute("rx", obj->rot.x);}
-			obstacle->SetAttribute("ry", obj->rot.y);
+			if (obj->rot.y != 0)	{obstacle->SetAttribute("ry", obj->rot.y);}
-			obstacle->SetAttribute("rz", obj->rot.z);
+			if (obj->rot.z != 0)	{obstacle->SetAttribute("rz", obj->rot.z);}
+			if (obj->scale.x != 1)	{obstacle->SetAttribute("sx", obj->scale.x);}
+			if (obj->scale.y != 1)	{obstacle->SetAttribute("sy", obj->scale.y);}
+			if (obj->scale.z != 1)	{obstacle->SetAttribute("sz", obj->scale.z);}
 			obstacles->InsertEndChild(obstacle);
 		}
 

geo/BBox2.h

@@ -106,7 +106,7 @@ public:
 
 	}
 
-	BBox2 intersection(const BBox2& o) {
+	BBox2 intersection(const BBox2& o) const {
 		// TODO is this correct?
 		const float x1 = std::max(p1.x, o.p1.x);
 		const float x2 = std::min(p2.x, o.p2.x);

geo/Triangle3.h

@@ -47,6 +47,14 @@ public:
 		return *this;
 	}
 
+	/** scale using 3 individual components */
+	Triangle3& operator *= (const Point3 o) {
+		p1 *= o;
+		p2 *= o;
+		p3 *= o;
+		return *this;
+	}
+
 	Point3 getNormal() const {
 		return cross( p2-p1, p3-p1 ).normalized();
 	}
@@ -128,6 +136,14 @@ public:
 
 	}
 
+	/** perform some sanity checks */
+	bool isValid() const {
+		if (p1 == p2) {return false;}
+		if (p1 == p3) {return false;}
+		if (p2 == p3) {return false;}
+		return true;
+	}
+
 /*
 	int rayIntersectsTriangle(float *p, float *d,
 				float *v0, float *v1, float *v2) {

grid/factory/v2/GridFactory.h

@@ -52,6 +52,7 @@ private:
 	bool _buildStairs = true;
 	bool _removeIsolated = true;
 	bool _addTightToObstacle = false;
+	bool _abortOnError = true;
 
 public:
 
@@ -60,6 +61,8 @@ public:
 
 	}
 
+	void setAbortOnError(const bool abort) {this->_abortOnError = abort;}
+
 	void setAddTightToObstacle(const bool tight) {this->_addTightToObstacle = tight;}
 
 	/** whether or not to build stairs */
@@ -181,7 +184,9 @@ public:
 
 				// belongs to a "add" polygon? -> remember until all polygons were checked
 				// [might still belong to a "remove" polygon]
-				res.outdoor = poly->outdoor;
+				if (poly->outdoor) {
+					res.outdoor = true;		// belonging to an outdoor region overwrites all other belongings
+				}
 
 			}
 		}
@@ -219,7 +224,7 @@ public:
 			if (foo) {
 
 				// get the obstacle
-				const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(foo->file).rotated_deg(foo->rot).translated(foo->pos);
+				const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(foo->file).scaled(foo->scale).rotated_deg(foo->rot).translated(foo->pos);
 
 				// construct its 2D convex hull (in centimter)
 				HelperPoly poly;
@@ -295,6 +300,8 @@ public:
 
 	void buildStairs(const Floorplan::IndoorMap* map, const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
 
+		stairs.setAbortOnError(_abortOnError);
+
 		const int total = floor->stairs.size();
 		int cur = 0;
 

grid/factory/v2/Stairs2.h

@@ -32,6 +32,7 @@ private:
 	std::vector<T*> toDelete;
 
 	bool tryImproveStairConnections = true;
+	bool abortOnError = true;
 
 private:
 
@@ -47,7 +48,6 @@ private:
 
 		StairNode(const int x_cm, const int y_cm, const int quadIdx) : x_cm(x_cm), y_cm(y_cm), belongsToQuadIdx(quadIdx) {;}
 
-
 	};
 
 
@@ -62,7 +62,10 @@ public:
 		finalize();
 	}
 
-
+	/** whether to abort when errors are detected */
+	void setAbortOnError(const bool abort) {
+		this->abortOnError = abort;
+	}
 
 
 
@@ -184,7 +187,11 @@ public:
 
 			// be sure both are connected to a floor
 			if (!end1OK || !end2OK) {
-				throw Exception("stair's start or end is not directly connectable to a floor");
+				if (abortOnError) {
+					throw Exception("stair's start or end is not directly connectable to a floor");
+				} else{
+					std::cout << "stair's start or end is not directly connectable to a floor" << std::endl;
+				}
 			}
 
 		}

grid/walk/v2/GridWalker.h

@@ -5,7 +5,7 @@
 #include "../../Grid.h"
 #include "../../../math/DrawList.h"
 #include "modules/WalkModule.h"
-#include "../../../math/Distributions.h"
+#include "../../../math/distribution/Normal.h"
 #include "../../../math/Stats.h"
 
 /**

grid/walk/v2/modules/WalkModuleActivityControl.h

@@ -4,7 +4,7 @@
 #include "WalkModule.h"
 
 #include "../../../../geo/Heading.h"
-#include "../../../../math/Distributions.h"
+#include "../../../../math/distribution/Normal.h"
 
 #include "../../../../sensors/activity/ActivityButterPressure.h"
 

grid/walk/v2/modules/WalkModuleFavorZ.h

@@ -5,7 +5,7 @@
 #include "WalkStateHeading.h"
 
 #include "../../../../geo/Heading.h"
-#include "../../../../math/Distributions.h"
+#include "../../../../math/distribution/Normal.h"
 
 #include "../../../../Assertions.h"
 

grid/walk/v2/modules/WalkModuleFollowDestination.h

@@ -7,7 +7,7 @@
 #include "../../../../nav/dijkstra/Dijkstra.h"
 #include "../../../../nav/dijkstra/DijkstraPath.h"
 
-#include "../../../../math/Distributions.h"
+#include "../../../../math/distribution/Normal.h"
 #include "../../../../Assertions.h"
 
 

grid/walk/v2/modules/WalkModuleHeadingControl.h

@@ -5,8 +5,9 @@
 #include "WalkStateHeading.h"
 
 #include "../../../../geo/Heading.h"
-#include "../../../../math/Distributions.h"
+#include "../../../../math/distribution/Normal.h"
-
+#include "../../../../math/distribution/LUT.h"
+#include "../../../../math/distribution/VonMises.h"
 
 /** keep the state's heading */
 template <typename Node, typename WalkState, typename Control> class WalkModuleHeadingControl : public WalkModule<Node, WalkState> {

math/dsp/fir/ComplexFactory.h

@@ -0,0 +1,31 @@
+#ifndef FIRFACTORY_H
+#define FIRFACTORY_H
+
+#include <vector>
+#include <complex>
+
+
+class FIRFactory {
+
+	float sRate_hz;
+
+public:
+
+	/** ctor */
+	FIRFactory(float sRate_hz) : sRate_hz(sRate_hz) {
+		;
+	}
+
+
+
+//	static std::vector<float> getRealBandbass(float center_hz, float width_hz, float sRate_hz, int size) {
+
+//	}
+
+//	static std::vector<std::complex<float>> getComplexBandbass(float center_hz, float width_hz, float sRate_hz, int size) {
+
+//	}
+
+}
+
+#endif // FIRFACTORY_H

math/dsp/fir/Real.h

@@ -0,0 +1,104 @@
+#ifndef FIRREAL_H
+#define FIRREAL_H
+
+#include <vector>
+#include <cmath>
+#include <string>
+#include "../../../Assertions.h"
+
+namespace FIR {
+
+	namespace Real {
+
+		using Kernel = std::vector<float>;
+		using DataVec = std::vector<float>;
+
+		class Filter {
+
+			Kernel kernel;
+			DataVec data;
+
+		public:
+
+			/** ctor */
+			Filter(const Kernel& kernel) : kernel(kernel) {
+				;
+			}
+
+			/** empty ctor */
+			Filter() : kernel() {
+				;
+			}
+
+			/** set the filter-kernel */
+			void setKernel(const Kernel& kernel) {
+				this->kernel = kernel;
+			}
+
+			/** filter the given incoming real data */
+			DataVec append(const DataVec& newData) {
+				// append to local buffer (as we need some history)
+				data.insert(data.end(), newData.begin(), newData.end());
+				return processLocalBuffer();
+			}
+
+			/** filter the given incoming real value */
+			float append(const float val) {
+				data.push_back(val);
+				auto tmp = processLocalBuffer();
+				if (tmp.size() == 0) {return NAN;}
+				if (tmp.size() == 1) {return tmp[0];}
+				throw Exception("FIR::Real::Filter detected invalid result");
+			}
+
+		private:
+
+			DataVec processLocalBuffer() {
+
+				// sanity check
+				Assert::isNot0(kernel.size(), "FIRComplex:: kernel not yet configured!");
+
+				// number of processable elements (due to filter size)
+				const int processable = data.size() - kernel.size() + 1 - kernel.size()/2;
+
+				// nothing to-do?
+				if (processable <= 0) {return DataVec();}
+
+				// result-vector
+				DataVec res;
+				res.resize(processable);
+
+				// fire
+				convolve(data.data(), res.data(), processable);
+
+				// drop processed elements from the local buffer
+				data.erase(data.begin(), data.begin() + processable);
+
+				// done
+				return res;
+
+			}
+
+			template <typename T> void convolve(const float* src, T* dst, const size_t cnt) {
+
+				const size_t ks = kernel.size();
+
+				for (size_t i = 0; i < cnt; ++i) {
+					T t = T();
+					for (size_t j = 0; j < ks; ++j) {
+						t += src[j+i] * kernel[j];
+					}
+					if (t != t) {throw std::runtime_error("detected NaN");}
+					dst[i] = t;
+				}
+
+			}
+
+
+		};
+
+	}
+
+}
+
+#endif // FIRREAL_H

math/dsp/fir/RealFactory.h

@@ -0,0 +1,133 @@
+#ifndef FIRREALFACTORY_H
+#define FIRREALFACTORY_H
+
+#include "Real.h"
+
+namespace FIR {
+
+	namespace Real {
+
+		class Factory {
+
+			Kernel kernel;
+
+			float sRate_hz;
+
+		public:
+
+			/** ctor */
+			Factory(float sRate_hz) : sRate_hz(sRate_hz) {
+				;
+			}
+
+			/** frequency shift the kernel by multiplying with a frequency */
+			void shift(const float shift_hz) {
+				for (size_t i = 0; i < kernel.size(); ++i) {
+					const float t = (float) i / (float) sRate_hz;
+					const float real = std::sin(t * 2 * M_PI * shift_hz);
+					kernel[i] = kernel[i] * real;
+				}
+			}
+
+			/** create a lowpass filte kernel */
+			void lowpass(const float cutOff_hz, const int n = 50) {
+
+				kernel.clear();
+
+				for (int i = -n; i <= +n; ++i) {
+					const double t = (double) i / (double) sRate_hz;
+					const double tmp = 2 * M_PI * cutOff_hz * t;
+					const double val = (tmp == 0) ? (1) : (std::sin(tmp) / tmp);
+					const double res = val;// * 0.5f;	// why 0.5?
+					if (res != res) {throw std::runtime_error("detected NaN");}
+					kernel.push_back(res);
+				}
+
+			}
+
+			/** apply hamming window to the filter */
+			void applyWindowHamming() {
+				const int n = (kernel.size()-1)/2;
+				int i = -n;
+				for (float& f : kernel) {
+					f *= winHamming(i+n, n*2);
+					++i;
+				}
+			}
+
+			// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
+			/** normalize using the DC-part of the kernel */
+			void normalizeDC() {
+				float sum = 0;
+				for (auto f : kernel) {sum += f;}
+				for (auto& f : kernel) {f /= sum;}
+			}
+
+			// https://dsp.stackexchange.com/questions/4693/fir-filter-gain
+			void normalizeAC(const float freq_hz) {
+
+				const int n = (kernel.size()-1)/2;
+				int i = -n;
+				float sum = 0;
+
+				for (float f : kernel) {
+					const double t = (double) i / (double) sRate_hz;
+					const double r = 2 * M_PI * freq_hz * t;
+					const double s = std::sin(r);
+					sum += f * s;
+					++i;
+				}
+
+				for (auto& f : kernel) {f /= sum;}
+
+			}
+
+
+
+
+
+			Kernel getLowpass(const float cutOff_hz, const int n) {
+				lowpass(cutOff_hz, n);
+				applyWindowHamming();
+				normalizeDC();
+				return kernel;
+			}
+
+			Kernel getBandpass(const float width_hz, const float center_hz, const int n) {
+				lowpass(width_hz/2, n);
+				applyWindowHamming();
+				//normalizeDC();
+				shift(center_hz);
+				normalizeAC(center_hz);
+				return kernel;
+			}
+
+			void dumpKernel(const std::string& file, const std::string& varName) {
+
+				std::ofstream out(file);
+				out << "# name: " << varName << "\n";
+				out << "# type: matrix\n";
+				out << "# rows: " << kernel.size() << "\n";
+				out << "# columns: 1\n";
+
+				for (const float f : kernel) {
+					out << f << "\n";
+				}
+				out.close();
+
+			}
+
+		private:
+
+			/** get a value from the hamming window */
+			static double winHamming(const double t, const double size) {
+				return 0.54 - 0.46 * std::cos(2 * M_PI * t / size);
+			}
+
+		};
+
+	}
+
+}
+
+#endif // FIRREALFACTORY_H

misc/Debug.h

@@ -4,6 +4,7 @@
 #include <string>
 #include <iostream>
 #include <iomanip>
+#include <sstream>
 #include "Time.h"
 
 #include "log/LoggerCOUT.h"

navMesh/NavMeshFactory.h

@@ -632,7 +632,7 @@ namespace NM {
 			Floorplan::Polygon2 res;
 
 			// fetch object from pool
-			const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(obj->file).rotated_deg(obj->rot).translated(obj->pos);
+			const Ray3D::Obstacle3D obs = Ray3D::OBJPool::get().getObject(obj->file).scaled(obj->scale).rotated_deg(obj->rot).translated(obj->pos);
 
 			// construct 2D convex hull
 			res.points = ConvexHull2::get(obs.getPoints2D());

sensors/imu/StepDetection.h

@@ -19,7 +19,7 @@
 
 #include "../../Assertions.h"
 #include "../../math/MovingAverageTS.h"
-#include "../../math/dsp/FIRComplex.h"
+//#include "../../math/dsp/fir/RComplex.h"
 
 
 /**

sensors/imu/StepDetection2.h

@@ -22,7 +22,8 @@
 #endif
 
 #include "../../Assertions.h"
-#include "../../math/dsp/FIRComplex.h"
+#include "../../math/dsp/fir/Real.h"
+#include "../../math/dsp/fir/RealFactory.h"
 #include "../../math/FixedFrequencyInterpolator.h"
 #include "../../math/LocalMaxima.h"
 #include "../../math/MovingAverageTS.h"
@@ -41,7 +42,7 @@ class StepDetection2 {
 private:
 
 	FixedFrequencyInterpolator<AccelerometerData> interpol;
-	FIRComplex fir;
+	FIR::Real::Filter fir;
 	LocalMaxima locMax;
 
 	// longterm average to center around zero
@@ -68,12 +69,16 @@ private:
 public:
 
 	/** ctor */
-	StepDetection2() : interpol(Timestamp::fromMS(every_ms)), fir(sRate_hz), locMax(8) {
+	StepDetection2() : interpol(Timestamp::fromMS(every_ms)), locMax(8) {
 
 		//fir.lowPass(0.66, 40);	// allow deviation of +/- 0.66Hz
 		//fir.shiftBy(2.00);			// typical step freq ~2Hz
 
-		fir.lowPass(3.5, 25);		// everything up to 3 HZ
+		//fir.lowPass(3.5, 25);		// everything up to 3 HZ
+
+		FIR::Real::Factory fac(sRate_hz);
+		fir.setKernel(fac.getBandpass(0.66, 2.0, 40));
+
 
 		#ifdef WITH_DEBUG_PLOT
 			gp << "set autoscale xfix\n";
@@ -104,10 +109,13 @@ public:
 			avg.add(ts, mag);
 			const float mag0 = mag - avg.get();
 
-			const std::complex<float> c = fir.append(mag0);
+			//const std::complex<float> c = fir.append(mag0);
-			const float real = c.real();
+			//const float real = c.real();
-			if (real != real) {return;}
+			//if (real != real) {return;}
-			const float fMag = real;
+			//const float fMag = real;
+			const float f = fir.append(mag0);
+			if (f != f) {return;}
+			const float fMag = f;
 
 			const bool isMax = locMax.add(fMag);
 

sensors/radio/WiFiProbabilityFree.h

@@ -3,7 +3,9 @@
 
 #include "WiFiProbability.h"
 #include "model/WiFiModel.h"
-#include "../../math/Distributions.h"
+#include "../../math/distribution/Normal.h"
+#include "../../math/distribution/Region.h"
+#include "../../math/distribution/Exponential.h"
 #include "VAPGrouper.h"
 #include "../../floorplan/v2/Floorplan.h"
 

sensors/radio/WiFiProbabilityGrid.h

@@ -3,7 +3,7 @@
 
 #include <fstream>
 
-#include "../../math/Distributions.h"
+#include "../../math/distribution/Normal.h"
 #include "../../data/Timestamp.h"
 
 #include "WiFiGridNode.h"

sensors/radio/setup/WiFiOptimizerLogDistCeiling.h

@@ -33,6 +33,7 @@ namespace WiFiOptimizer {
 	 */
 	struct LogDistCeiling : public Base {
 
+
 	public:
 
 		/**
@@ -63,6 +64,30 @@ namespace WiFiOptimizer {
 
 		};
 
+		using APFilter = std::function<bool(const Stats& stats, const MACAddress& mac)>;
+
+		static inline bool NONE(const Stats& stats, const MACAddress& mac) {
+			(void) stats; (void) mac;
+			return false;
+		}
+
+		static inline bool  MIN_2_FPS(const Stats& stats, const MACAddress& mac) {
+			(void) mac;
+			return stats.usedFingerprins < 2;
+		}
+
+		static inline bool MIN_5_FPS(const Stats& stats, const MACAddress& mac) {
+			(void) mac;
+			return stats.usedFingerprins < 5;
+		}
+
+		static inline bool MIN_10_FPS(const Stats& stats, const MACAddress& mac) {
+			(void) mac;
+			return stats.usedFingerprins < 10;
+		}
+
+
+
 		/** parameters for one AP when using the LogDistCeiling model */
 		struct APParams {
 
@@ -139,29 +164,6 @@ namespace WiFiOptimizer {
 
 		};
 
-		using APFilter = std::function<bool(const Stats& stats, const MACAddress& mac)>;
-
-		const APFilter NONE = [] (const Stats& stats, const MACAddress& mac) {
-			(void) stats; (void) mac;
-			return false;
-		};
-
-                const APFilter MIN_2_FPS = [] (const Stats& stats, const MACAddress& mac) {
-			(void) mac;
-			return stats.usedFingerprins < 2;
-		};
-
-		const APFilter MIN_5_FPS = [] (const Stats& stats, const MACAddress& mac) {
-			(void) mac;
-			return stats.usedFingerprins < 5;
-		};
-
-		const APFilter MIN_10_FPS = [] (const Stats& stats, const MACAddress& mac) {
-			(void) mac;
-			return stats.usedFingerprins < 10;
-		};
-
-
 	private:
 
 		Floorplan::IndoorMap* map;

sensors/radio/setup/WiFiOptimizerPerFloor.h

@@ -1,6 +1,7 @@
 #ifndef WIFIOPTIMIZERPERFLOOR_H
 #define WIFIOPTIMIZERPERFLOOR_H
 
+#include "WiFiOptimizer.h"
 #include "WiFiOptimizerLogDistCeiling.h"
 #include "../model/WiFiModelPerFloor.h"
 #include "../../../floorplan/v2/FloorplanHelper.h"

tests/ray/TestRayTrace3.cpp

@@ -20,7 +20,7 @@ TEST(RayTrace3, test) {
 
 	ModelFactory fac(map);
 	std::ofstream outOBJ("/tmp/vm/map.obj");
-	outOBJ << fac.getMesh().toOBJ();
+	outOBJ << fac.getMesh().toOBJ("obj").obj;
 	outOBJ.close();
 
 	const int gs_cm = 50;

wifi/estimate/ray3/FloorplanMesh.h

@@ -205,7 +205,7 @@ namespace Ray3D {
 			for (const Obstacle3D& obs : elements) {
 				for (const Triangle3& tria : obs.triangles) {
 					(void) tria;
-					res << "3 " << vidx++ << " " << vidx++ << " " << vidx++ << "\n";
+					res << "3 " << (vidx++) << " " << (vidx++) << " " << (vidx++) << "\n";
 				}
 			}
 

wifi/estimate/ray3/MTLReader.h

@@ -110,6 +110,9 @@ private:
 		} else if ("map_Ka" == token) {
 			const std::string texFile = tokens[1];
 			cur->textureFile = texFile;
+		} else if ("map_Kd" == token) {
+			const std::string texFile = tokens[1];
+			cur->textureFile = texFile;
 		} else if ("Kd" == token) {
 			cur->diffuse.x = std::stof(tokens[1]);
 			cur->diffuse.y = std::stof(tokens[2]);

wifi/estimate/ray3/ModelFactory.h

@@ -282,7 +282,7 @@ namespace Ray3D {
 
 			// attributes
 			const float r = foc->radius;
-			const float h = (foc->height > 0) ? (foc->height) : (fpos.height);
+			const float h = (foc->height > 0) ? (foc->height) : (fpos.height);		// use either floor's height or user height
 			const Point3 pos(foc->center.x, foc->center.y, fpos.z1 + h/2);
 
 			// build
@@ -310,7 +310,8 @@ namespace Ray3D {
 			const float deg = rad * 180 / M_PI;
 
 			// cube's destination center
-			const double height =	(!aboveDoor) ? (fpos.height) :				(fpos.height - aboveDoor->height);
+			const float _height =	(fol->height_m > 0) ? (fol->height_m) :		(fpos.height);	// use either floor's height or user height
+			const double height =	(!aboveDoor) ? (_height) :					(fpos.height - aboveDoor->height);
 			const double cenZ =		(!aboveDoor) ? (fpos.z1 + height/2) :		(fpos.z1 + aboveDoor->height + height/2);// (fpos.z2 - (fpos.height - aboveDoor->height) / 2);
 			const Point3 pos(cen2.x, cen2.y, cenZ);
 
@@ -338,6 +339,14 @@ namespace Ray3D {
 
 			const std::string& name = foo->file;
 			Obstacle3D obs = OBJPool::get().getObject(name);
+
+			// perform sanity checks
+			if (!obs.isValid()) {
+				throw std::runtime_error("invalid obstacle-data detected");
+			}
+
+			// apply scaling/rotation/translation
+			obs = obs.scaled(foo->scale);
 			obs = obs.rotated_deg( Point3(foo->rot.x, foo->rot.y, foo->rot.z) );
 			obs = obs.translated(foo->pos + Point3(0,0,fpos.z1));
 			obs.type = Obstacle3D::Type::OBJECT;

wifi/estimate/ray3/OBJPool.h

@@ -11,7 +11,8 @@
 // LINUX ONLY
 //#include <dirent.h>
 //#include <stdio.h>
-#include <experimental/filesystem>
+#include "../../../data/File.h"
+
 
 #include "../../../misc/Debug.h"
 
@@ -83,17 +84,17 @@ namespace Ray3D {
 		/** scan the given folder for all *.obj files */
 		void scanFolder(const std::string& folder) {
 
-			std::experimental::filesystem::path d(folder);
+			FS::File d(folder);
-			if (!std::experimental::filesystem::exists(d)) {
+			if (!d.exists()) {
 				throw Exception("OBJPool: folder not found: " + folder);
 			}
 
-			for (std::experimental::filesystem::directory_entry entry : std::experimental::filesystem::directory_iterator(d)) {
+			for (const FS::File& f : d.listFiles()) {
-				const std::string absFile = entry.path().string();
+				std::string name = f.getFilename();
-				if (endsWith(absFile, ".obj")) {
+				if (endsWith(name, ".obj")) {
-					std::string name = entry.path().filename().string();
+					//std::string name = entry.path().filename().string();
 					name = name.substr(0, name.length() - 4);	// without extension
-					load(absFile, name);
+					load(f.getPath(), name);
 				}
 			}
 

wifi/estimate/ray3/OBJReader.h

@@ -129,6 +129,7 @@ private:
 		if ("vn" == token)		{parseNormal(tokens);}
 		if ("f"  == token)		{parseFace(tokens);}
 		if ("g"  == token)		{newObject(tokens[1]);}
+		if ("o"  == token)		{newObject(tokens[1]);}
 
 	}
 

wifi/estimate/ray3/Obstacle3.h

@@ -38,6 +38,14 @@ namespace Ray3D {
 		/** ctor */
 		Obstacle3D(Type type, Floorplan::Material mat) : type(type), mat(mat) {;}
 
+		/** scaled copy */
+		Obstacle3D scaled(const Point3 scale) const {
+			Obstacle3D copy = *this;
+			for (Triangle3& tria : copy.triangles) {
+				tria *= scale;
+			}
+			return copy;
+		}
 
 		/** translated copy */
 		Obstacle3D translated(const Point3 pos) const {
@@ -81,6 +89,14 @@ namespace Ray3D {
 			return res;
 		}
 
+		/** perform sanity checks */
+		bool isValid() const {
+			for (const Triangle3& t : triangles) {
+				if (!t.isValid()) {return false;}
+			}
+			return true;
+		}
+
 	};
 
 }