diff --git a/CMakeLists.txt b/CMakeLists.txt index 01170d3..acfd892 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,7 +81,7 @@ else() # system specific compiler flags ADD_DEFINITIONS( - -std=gnu++11 + -std=gnu++17 -Wall -Werror=return-type diff --git a/math/boxkde/BoxGaus3D.h b/math/boxkde/BoxGaus3D.h index 09196d9..34f0fac 100644 --- a/math/boxkde/BoxGaus3D.h +++ b/math/boxkde/BoxGaus3D.h @@ -156,8 +156,9 @@ private: } } - template - void boxBlur1D(const ImageView3D::ConstLineView& src, ImageView3D::LineView& dst, size_t len, size_t r, T iarr) + + template + void boxBlur1D(const typename ImageView3D::template ConstLineView& src, typename ImageView3D::template LineView& dst, size_t len, size_t r, T iarr) { size_t li = 0; // left index size_t ri = r; // right index diff --git a/math/boxkde/Grid3D.h b/math/boxkde/Grid3D.h index 2585460..8093c4b 100644 --- a/math/boxkde/Grid3D.h +++ b/math/boxkde/Grid3D.h @@ -164,6 +164,8 @@ public: private: _Point3 add_simple_bin(T x, T y, T z, T w) { + Assert::isTrue(w > 0, "Weight needs to be postive."); + size_t bin_x = (size_t)((x - bb.getMin().x) / binSizeX); size_t bin_y = (size_t)((y - bb.getMin().y) / binSizeY); size_t bin_z = (size_t)((z - bb.getMin().z) / binSizeZ); diff --git a/math/boxkde/Image3D.h b/math/boxkde/Image3D.h index 958c212..4180f80 100644 --- a/math/boxkde/Image3D.h +++ b/math/boxkde/Image3D.h @@ -104,6 +104,21 @@ public: this->values = tmp; } + // Returns the value at (x,y). Throws if out of bounds. + const TValue& at(size_t x, size_t y, size_t z) const + { + return values[indexFromCoord(x, y, z)]; + } + + // Returns the value at (x,y) but falls back to default if out of bounds. + const TValue& get(size_t x, size_t y, size_t z, TValue dflt = 0) const + { + if (x >= width || y >= height || z >= depth) + return dflt; + else + return values[indexFromCoord(x, y, z)]; + } + public: typedef LineView LineViewXY; typedef LineView LineViewXZ; @@ -205,4 +220,4 @@ template struct ImageView3D; //template struct ImageView3D; template struct Image3D; -//template struct Image3D; \ No newline at end of file +//template struct Image3D; diff --git a/smc/filtering/resampling/ParticleFilterResamplingKDE.h b/smc/filtering/resampling/ParticleFilterResamplingKDE.h index f038ab6..cd6c09a 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingKDE.h +++ b/smc/filtering/resampling/ParticleFilterResamplingKDE.h @@ -9,12 +9,13 @@ #include "../../../math/boxkde/benchmark.h" #include "../../../math/boxkde/DataStructures.h" -#include "../../../math/boxkde/Image2D.h" -#include "../../../math/boxkde/BoxGaus.h" -#include "../../../math/boxkde/Grid2D.h" +#include "../../../math/boxkde/Image3D.h" +#include "../../../math/boxkde/BoxGaus3D.h" +#include "../../../math/boxkde/Grid3D.h" #include "../../../math/distribution/Normal.h" #include "../../../navMesh/NavMesh.h" +#include "../../../floorplan/v2/FloorplanHelper.h" namespace SMC { @@ -33,13 +34,13 @@ namespace SMC { std::minstd_rand gen; /** boundingBox for the boxKDE */ - BoundingBox bb; + _BBox3 bb; /** histogram/grid holding the particles*/ - Grid2D grid; + std::unique_ptr> grid; /** bandwith for KDE */ - Point2 bandwith; + Point3 bandwith; /** the current mesh */ const NM::NavMesh* mesh; @@ -47,19 +48,19 @@ namespace SMC { public: /** ctor */ - ParticleFilterResamplingKDE(const NM::NavMesh* mesh, const float gridsize_m, const Point2 bandwith) { + ParticleFilterResamplingKDE(const NM::NavMesh* mesh, const Point3 gridsize_m, const Point3 bandwith) { - const Point3 maxBB = mesh->getBBox().getMax(); - const Point3 minBB = mesh->getBBox().getMin(); - this->bb = BoundingBox(minBB.x - 10, maxBB.x + 10, minBB.y - 10, maxBB.y + 10); + this->mesh = mesh; + this->bandwith = bandwith; + this->bb = mesh->getBBox(); + this->bb.grow(10); // Create histogram - size_t nBinsX = static_cast((maxBB.x - minBB.x) / gridsize_m); - size_t nBinsY = static_cast((maxBB.y - minBB.y) / gridsize_m); - this->grid = Grid2D(bb, nBinsX, nBinsY); + size_t nBinsX = (size_t)((this->bb.getMax().x - this->bb.getMin().x) / gridsize_m.x); + size_t nBinsY = (size_t)((this->bb.getMax().y - this->bb.getMin().y) / gridsize_m.y); + size_t nBinsZ = (size_t)((this->bb.getMax().z - this->bb.getMin().z) / gridsize_m.z); - this->bandwith = bandwith; - this->mesh = mesh; + this->grid = std::make_unique>(bb, nBinsX, nBinsY, nBinsZ); gen.seed(1234); } @@ -73,46 +74,34 @@ namespace SMC { //static_assert( std::is_constructible::value, "your state needs a constructor with Point3!"); //todo: static assert for getx, gety, getz, setposition - State tmpAVG; - double weightSum = 0; - - grid.clear(); + grid->clear(); for (Particle p : particles){ //grid.add receives position in meter! - grid.add(p.state.getX(), p.state.getY(), p.weight); - - //TODO: fixe this hack - //get the z value by using the weighted average z! - tmpAVG += p.state * p.weight; - weightSum += p.weight; + grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight); } - //TODO: fixe this hack with z... - tmpAVG /= weightSum; int nFilt = 3; - float sigmaX = bandwith.x / grid.binSizeX; - float sigmaY = bandwith.y / grid.binSizeY; - BoxGaus boxGaus; - boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt); + float sigmaX = bandwith.x / grid->binSizeX; + float sigmaY = bandwith.y / grid->binSizeY; + float sigmaZ = bandwith.z / grid->binSizeZ; + + BoxGaus3D boxGaus; + boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt); // fill a drawlist with 10k equal distributed particles // assign them a weight based on the KDE density. DrawList dl; - Distribution::Normal zProb(tmpAVG.getZ(), 4.0); for (int i = 0; i < 10000; ++i){ - - //todo: wir ziehen natürlich hier aus dem ganzen gebäude, bekommen - // aber ein gewicht nur basierend auf 2D. deshalb kleiner hack und z - // mit einer normalverteilung gewichtet, wobei mean = avg_z der partikel - // menge ist NM::NavMeshLocation tmpPos = mesh->getRandom().draw(); - double weight = grid.image().get(tmpPos.pos.x, tmpPos.pos.y); - //weight *= zProb.getProbability(tmpPos.pos.z); + float weight = grid->fetch(tmpPos.pos.x, tmpPos.pos.y, tmpPos.pos.z); + + if(weight < 0 ){ + int halloBulli = 666; + } dl.add(tmpPos.pos, weight); } - int dummyyy = 0; // used the same particles to not lose the heading. // TODO: Summe aller Partikel wird relativ schnell 0! Ich vermute da am Anfang ein einzelner Partikel stark gewichtet ist und alleine @@ -122,7 +111,7 @@ namespace SMC { for (int i = 0; i < particles.size(); ++i){ double tmpWeight = 1; - particles[i].state.loc = mesh->getLocation(dl.get(tmpWeight)); + particles[i].state.pos = mesh->getLocation(dl.get(tmpWeight)); particles[i].weight = tmpWeight; } diff --git a/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h index 5144135..cfd89c1 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h +++ b/smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h @@ -75,7 +75,7 @@ namespace SMC { for (uint32_t i = 0; i < cnt; ++i) { // slight chance to get a truely random particle in range X m - if (distNewOne(gen) < 0.001) { + if (distNewOne(gen) < 0.005) { const double radius = 50.0; const NM::NavMeshSub reachable(particlesCopy[i].state.loc, radius); particles[i].state.loc = reachable.getRandom().drawWithin(particlesCopy[i].state.loc.pos, radius);