FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

added KDE3DResampling Method.

Browse Source 
However BoxKDE3D seems to be buggy
This commit is contained in:
toni
2018-08-01 11:54:10 +02:00
parent d912a76246
commit 0bb22b54e9
6 changed files with 53 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -81,7 +81,7 @@ else()
# system specific compiler flags # system specific compiler flags
ADD_DEFINITIONS( ADD_DEFINITIONS(
-std=gnu++11 -std=gnu++17
-Wall -Wall
-Werror=return-type -Werror=return-type

5
math/boxkde/BoxGaus3D.h
View File

@@ -156,8 +156,9 @@ private:
} }
} }
template<SlicePlane S>
void boxBlur1D(const ImageView3D<T>::ConstLineView<S>& src, ImageView3D<T>::LineView<S>& dst, size_t len, size_t r, T iarr) template <SlicePlane S>
void boxBlur1D(const typename ImageView3D<T>::template ConstLineView<S>& src, typename ImageView3D<T>::template LineView<S>& dst, size_t len, size_t r, T iarr)
{ {
size_t li = 0; // left index size_t li = 0; // left index
size_t ri = r; // right index size_t ri = r; // right index

2
math/boxkde/Grid3D.h
View File

@@ -164,6 +164,8 @@ public:
private: private:
_Point3<size_t> add_simple_bin(T x, T y, T z, T w) _Point3<size_t> 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_x = (size_t)((x - bb.getMin().x) / binSizeX);
size_t bin_y = (size_t)((y - bb.getMin().y) / binSizeY); size_t bin_y = (size_t)((y - bb.getMin().y) / binSizeY);
size_t bin_z = (size_t)((z - bb.getMin().z) / binSizeZ); size_t bin_z = (size_t)((z - bb.getMin().z) / binSizeZ);

17
math/boxkde/Image3D.h
View File

@@ -104,6 +104,21 @@ public:
this->values = tmp; 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: public:
typedef LineView<SlicePlane::XY> LineViewXY; typedef LineView<SlicePlane::XY> LineViewXY;
typedef LineView<SlicePlane::XZ> LineViewXZ; typedef LineView<SlicePlane::XZ> LineViewXZ;
@@ -205,4 +220,4 @@ template struct ImageView3D<float>;
//template struct ImageView3D<double>; //template struct ImageView3D<double>;
template struct Image3D<float>; template struct Image3D<float>;
//template struct Image3D<double>; //template struct Image3D<double>;

71
smc/filtering/resampling/ParticleFilterResamplingKDE.h
View File

@@ -9,12 +9,13 @@
#include "../../../math/boxkde/benchmark.h" #include "../../../math/boxkde/benchmark.h"
#include "../../../math/boxkde/DataStructures.h" #include "../../../math/boxkde/DataStructures.h"
#include "../../../math/boxkde/Image2D.h" #include "../../../math/boxkde/Image3D.h"
#include "../../../math/boxkde/BoxGaus.h" #include "../../../math/boxkde/BoxGaus3D.h"
#include "../../../math/boxkde/Grid2D.h" #include "../../../math/boxkde/Grid3D.h"
#include "../../../math/distribution/Normal.h" #include "../../../math/distribution/Normal.h"
#include "../../../navMesh/NavMesh.h" #include "../../../navMesh/NavMesh.h"
#include "../../../floorplan/v2/FloorplanHelper.h"
namespace SMC { namespace SMC {
@@ -33,13 +34,13 @@ namespace SMC {
std::minstd_rand gen; std::minstd_rand gen;
/** boundingBox for the boxKDE */ /** boundingBox for the boxKDE */
BoundingBox<float> bb; _BBox3<float> bb;
/** histogram/grid holding the particles*/ /** histogram/grid holding the particles*/
Grid2D<float> grid; std::unique_ptr<Grid3D<float>> grid;
/** bandwith for KDE */ /** bandwith for KDE */
Point2 bandwith; Point3 bandwith;
/** the current mesh */ /** the current mesh */
const NM::NavMesh<Tria>* mesh; const NM::NavMesh<Tria>* mesh;
@@ -47,19 +48,19 @@ namespace SMC {
public: public:
/** ctor */ /** ctor */
ParticleFilterResamplingKDE(const NM::NavMesh<Tria>* mesh, const float gridsize_m, const Point2 bandwith) { ParticleFilterResamplingKDE(const NM::NavMesh<Tria>* mesh, const Point3 gridsize_m, const Point3 bandwith) {
const Point3 maxBB = mesh->getBBox().getMax(); this->mesh = mesh;
const Point3 minBB = mesh->getBBox().getMin(); this->bandwith = bandwith;
this->bb = BoundingBox<float>(minBB.x - 10, maxBB.x + 10, minBB.y - 10, maxBB.y + 10); this->bb = mesh->getBBox();
this->bb.grow(10);
// Create histogram // Create histogram
size_t nBinsX = static_cast<size_t>((maxBB.x - minBB.x) / gridsize_m); size_t nBinsX = (size_t)((this->bb.getMax().x - this->bb.getMin().x) / gridsize_m.x);
size_t nBinsY = static_cast<size_t>((maxBB.y - minBB.y) / gridsize_m); size_t nBinsY = (size_t)((this->bb.getMax().y - this->bb.getMin().y) / gridsize_m.y);
this->grid = Grid2D<float>(bb, nBinsX, nBinsY); size_t nBinsZ = (size_t)((this->bb.getMax().z - this->bb.getMin().z) / gridsize_m.z);
this->bandwith = bandwith; this->grid = std::make_unique<Grid3D<float>>(bb, nBinsX, nBinsY, nBinsZ);
this->mesh = mesh;
gen.seed(1234); gen.seed(1234);
} }
@@ -73,46 +74,34 @@ namespace SMC {
//static_assert( std::is_constructible<State, Point3>::value, "your state needs a constructor with Point3!"); //static_assert( std::is_constructible<State, Point3>::value, "your state needs a constructor with Point3!");
//todo: static assert for getx, gety, getz, setposition //todo: static assert for getx, gety, getz, setposition
State tmpAVG; grid->clear();
double weightSum = 0;
grid.clear();
for (Particle<State> p : particles){ for (Particle<State> p : particles){
//grid.add receives position in meter! //grid.add receives position in meter!
grid.add(p.state.getX(), p.state.getY(), p.weight); grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight);
//TODO: fixe this hack
//get the z value by using the weighted average z!
tmpAVG += p.state * p.weight;
weightSum += p.weight;
} }
//TODO: fixe this hack with z...
tmpAVG /= weightSum;
int nFilt = 3; int nFilt = 3;
float sigmaX = bandwith.x / grid.binSizeX; float sigmaX = bandwith.x / grid->binSizeX;
float sigmaY = bandwith.y / grid.binSizeY; float sigmaY = bandwith.y / grid->binSizeY;
BoxGaus<float> boxGaus; float sigmaZ = bandwith.z / grid->binSizeZ;
boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt);
BoxGaus3D<float> boxGaus;
boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt);
// fill a drawlist with 10k equal distributed particles // fill a drawlist with 10k equal distributed particles
// assign them a weight based on the KDE density. // assign them a weight based on the KDE density.
DrawList<Point3> dl; DrawList<Point3> dl;
Distribution::Normal<float> zProb(tmpAVG.getZ(), 4.0);
for (int i = 0; i < 10000; ++i){ 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<Tria> tmpPos = mesh->getRandom().draw(); NM::NavMeshLocation<Tria> tmpPos = mesh->getRandom().draw();
double weight = grid.image().get(tmpPos.pos.x, tmpPos.pos.y); float weight = grid->fetch(tmpPos.pos.x, tmpPos.pos.y, tmpPos.pos.z);
//weight *= zProb.getProbability(tmpPos.pos.z);
if(weight < 0 ){
int halloBulli = 666;
}
dl.add(tmpPos.pos, weight); dl.add(tmpPos.pos, weight);
} }
int dummyyy = 0;
// used the same particles to not lose the heading. // 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 // 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){ for (int i = 0; i < particles.size(); ++i){
double tmpWeight = 1; 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; particles[i].weight = tmpWeight;
} }

2
smc/filtering/resampling/ParticleFilterResamplingSimpleImpoverishment.h
View File

@@ -75,7 +75,7 @@ namespace SMC {
for (uint32_t i = 0; i < cnt; ++i) { for (uint32_t i = 0; i < cnt; ++i) {
// slight chance to get a truely random particle in range X m // 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 double radius = 50.0;
const NM::NavMeshSub<Tria> reachable(particlesCopy[i].state.loc, radius); const NM::NavMeshSub<Tria> reachable(particlesCopy[i].state.loc, radius);
particles[i].state.loc = reachable.getRandom().drawWithin(particlesCopy[i].state.loc.pos, radius); particles[i].state.loc = reachable.getRandom().drawWithin(particlesCopy[i].state.loc.pos, radius);