diff --git a/data/File.h b/data/File.h new file mode 100644 index 0000000..0f6d48f --- /dev/null +++ b/data/File.h @@ -0,0 +1,115 @@ +#ifndef FS_FILE_H +#define FS_FILE_H + +#include +#include + + + + +#if __cplusplus > 201103L + + // use new cross-platform std::experimental stuff + #include + +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 listFiles() const { + std::vector 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 + #include + +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 listFiles() const { + + std::vector 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 diff --git a/data/csv.h b/data/csv.h new file mode 100644 index 0000000..4f65517 --- /dev/null +++ b/data/csv.h @@ -0,0 +1,95 @@ +#ifndef CSV_H +#define CSV_H + +#include +#include +#include +#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 { + + }; + + /** one csv document */ + struct Doc : std::vector { + + }; + + + 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 diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 90edb51..6c82947 100644 --- a/floorplan/v2/Floorplan.h +++ b/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) {;} - 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) {;} + float height_m = 0; // 0 = floor's height + 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) {;} }; diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index 5c18ccf..f7dd458 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/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)) ); } diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index 1619c69..42e6478 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/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); - obstacle->SetAttribute("ry", obj->rot.y); - obstacle->SetAttribute("rz", obj->rot.z); + if (obj->rot.x != 0) {obstacle->SetAttribute("rx", obj->rot.x);} + if (obj->rot.y != 0) {obstacle->SetAttribute("ry", obj->rot.y);} + 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); } diff --git a/geo/BBox2.h b/geo/BBox2.h index 05ee2d1..b24e1c7 100644 --- a/geo/BBox2.h +++ b/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); diff --git a/geo/Triangle3.h b/geo/Triangle3.h index af1bda8..ca09888 100644 --- a/geo/Triangle3.h +++ b/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) { diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index 24f3349..61c0601 100755 --- a/grid/factory/v2/GridFactory.h +++ b/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; diff --git a/grid/factory/v2/Stairs2.h b/grid/factory/v2/Stairs2.h index f16b772..4ce9d4b 100644 --- a/grid/factory/v2/Stairs2.h +++ b/grid/factory/v2/Stairs2.h @@ -32,6 +32,7 @@ private: std::vector 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; + } } } diff --git a/grid/walk/v2/GridWalker.h b/grid/walk/v2/GridWalker.h index 737caa6..b0154fd 100644 --- a/grid/walk/v2/GridWalker.h +++ b/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" /** diff --git a/grid/walk/v2/modules/WalkModuleActivityControl.h b/grid/walk/v2/modules/WalkModuleActivityControl.h index 075f54b..a5645fc 100644 --- a/grid/walk/v2/modules/WalkModuleActivityControl.h +++ b/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" diff --git a/grid/walk/v2/modules/WalkModuleFavorZ.h b/grid/walk/v2/modules/WalkModuleFavorZ.h index eacc910..3893329 100644 --- a/grid/walk/v2/modules/WalkModuleFavorZ.h +++ b/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" diff --git a/grid/walk/v2/modules/WalkModuleFollowDestination.h b/grid/walk/v2/modules/WalkModuleFollowDestination.h index b92be94..9f66641 100644 --- a/grid/walk/v2/modules/WalkModuleFollowDestination.h +++ b/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" diff --git a/grid/walk/v2/modules/WalkModuleHeadingControl.h b/grid/walk/v2/modules/WalkModuleHeadingControl.h index a79cdad..0eb0651 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingControl.h +++ b/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 class WalkModuleHeadingControl : public WalkModule { diff --git a/math/dsp/FIRComplex.h b/math/dsp/fir/Complex.h similarity index 100% rename from math/dsp/FIRComplex.h rename to math/dsp/fir/Complex.h diff --git a/math/dsp/fir/ComplexFactory.h b/math/dsp/fir/ComplexFactory.h new file mode 100644 index 0000000..1532ebd --- /dev/null +++ b/math/dsp/fir/ComplexFactory.h @@ -0,0 +1,31 @@ +#ifndef FIRFACTORY_H +#define FIRFACTORY_H + +#include +#include + + +class FIRFactory { + + float sRate_hz; + +public: + + /** ctor */ + FIRFactory(float sRate_hz) : sRate_hz(sRate_hz) { + ; + } + + + +// static std::vector getRealBandbass(float center_hz, float width_hz, float sRate_hz, int size) { + +// } + +// static std::vector> getComplexBandbass(float center_hz, float width_hz, float sRate_hz, int size) { + +// } + +} + +#endif // FIRFACTORY_H diff --git a/math/dsp/fir/Real.h b/math/dsp/fir/Real.h new file mode 100644 index 0000000..cae949c --- /dev/null +++ b/math/dsp/fir/Real.h @@ -0,0 +1,104 @@ +#ifndef FIRREAL_H +#define FIRREAL_H + +#include +#include +#include +#include "../../../Assertions.h" + +namespace FIR { + + namespace Real { + + using Kernel = std::vector; + using DataVec = std::vector; + + 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 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 diff --git a/math/dsp/fir/RealFactory.h b/math/dsp/fir/RealFactory.h new file mode 100644 index 0000000..95d92a8 --- /dev/null +++ b/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 diff --git a/misc/Debug.h b/misc/Debug.h index 4f1f1a3..5668a0f 100644 --- a/misc/Debug.h +++ b/misc/Debug.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "Time.h" #include "log/LoggerCOUT.h" diff --git a/navMesh/NavMeshFactory.h b/navMesh/NavMeshFactory.h index 107ac5d..bf8a78c 100644 --- a/navMesh/NavMeshFactory.h +++ b/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()); diff --git a/sensors/imu/StepDetection.h b/sensors/imu/StepDetection.h index c19ee7b..99d067c 100644 --- a/sensors/imu/StepDetection.h +++ b/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" /** diff --git a/sensors/imu/StepDetection2.h b/sensors/imu/StepDetection2.h index eab6e2f..7cee7e6 100644 --- a/sensors/imu/StepDetection2.h +++ b/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 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 c = fir.append(mag0); - const float real = c.real(); - if (real != real) {return;} - const float fMag = real; + //const std::complex c = fir.append(mag0); + //const float real = c.real(); + //if (real != real) {return;} + //const float fMag = real; + const float f = fir.append(mag0); + if (f != f) {return;} + const float fMag = f; const bool isMax = locMax.add(fMag); diff --git a/sensors/radio/WiFiProbabilityFree.h b/sensors/radio/WiFiProbabilityFree.h index 7f4a02a..c7ae6c5 100644 --- a/sensors/radio/WiFiProbabilityFree.h +++ b/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" diff --git a/sensors/radio/WiFiProbabilityGrid.h b/sensors/radio/WiFiProbabilityGrid.h index 840d1dd..d6aff4e 100644 --- a/sensors/radio/WiFiProbabilityGrid.h +++ b/sensors/radio/WiFiProbabilityGrid.h @@ -3,7 +3,7 @@ #include -#include "../../math/Distributions.h" +#include "../../math/distribution/Normal.h" #include "../../data/Timestamp.h" #include "WiFiGridNode.h" diff --git a/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h b/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h index 48b27ce..1f880b1 100644 --- a/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h +++ b/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; + + 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; - - 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; diff --git a/sensors/radio/setup/WiFiOptimizerPerFloor.h b/sensors/radio/setup/WiFiOptimizerPerFloor.h index bd59b6b..7d9d3c4 100644 --- a/sensors/radio/setup/WiFiOptimizerPerFloor.h +++ b/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" diff --git a/tests/ray/TestRayTrace3.cpp b/tests/ray/TestRayTrace3.cpp index 663cf8f..f38f12b 100644 --- a/tests/ray/TestRayTrace3.cpp +++ b/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; diff --git a/wifi/estimate/ray3/FloorplanMesh.h b/wifi/estimate/ray3/FloorplanMesh.h index bf159e2..46ef4ba 100644 --- a/wifi/estimate/ray3/FloorplanMesh.h +++ b/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"; } } diff --git a/wifi/estimate/ray3/MTLReader.h b/wifi/estimate/ray3/MTLReader.h index 3952d14..0007b74 100644 --- a/wifi/estimate/ray3/MTLReader.h +++ b/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]); diff --git a/wifi/estimate/ray3/ModelFactory.h b/wifi/estimate/ray3/ModelFactory.h index fbd039c..adbe864 100644 --- a/wifi/estimate/ray3/ModelFactory.h +++ b/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; diff --git a/wifi/estimate/ray3/OBJPool.h b/wifi/estimate/ray3/OBJPool.h index a10c2d7..037dd5b 100644 --- a/wifi/estimate/ray3/OBJPool.h +++ b/wifi/estimate/ray3/OBJPool.h @@ -11,7 +11,8 @@ // LINUX ONLY //#include //#include -#include +#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); - if (!std::experimental::filesystem::exists(d)) { + FS::File d(folder); + if (!d.exists()) { throw Exception("OBJPool: folder not found: " + folder); } - for (std::experimental::filesystem::directory_entry entry : std::experimental::filesystem::directory_iterator(d)) { - const std::string absFile = entry.path().string(); - if (endsWith(absFile, ".obj")) { - std::string name = entry.path().filename().string(); + for (const FS::File& f : d.listFiles()) { + std::string name = f.getFilename(); + if (endsWith(name, ".obj")) { + //std::string name = entry.path().filename().string(); name = name.substr(0, name.length() - 4); // without extension - load(absFile, name); + load(f.getPath(), name); } } diff --git a/wifi/estimate/ray3/OBJReader.h b/wifi/estimate/ray3/OBJReader.h index f4922d3..f9b6d27 100644 --- a/wifi/estimate/ray3/OBJReader.h +++ b/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]);} } diff --git a/wifi/estimate/ray3/Obstacle3.h b/wifi/estimate/ray3/Obstacle3.h index 8199713..cfbfc71 100644 --- a/wifi/estimate/ray3/Obstacle3.h +++ b/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; + } + }; }