Added 3D boxKDE

smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h

@@ -0,0 +1,114 @@
+#ifndef PARTICLEFILTERESTIMATIONBOXKDE3D_H
+#define PARTICLEFILTERESTIMATIONBOXKDE3D_H
+
+#include <vector>
+#include "../../Particle.h"
+#include "../../ParticleAssertions.h"
+#include "ParticleFilterEstimation.h"
+#include "../../../Assertions.h"
+
+#include "../../../math/boxkde/benchmark.h"
+//#include "../../../math/boxkde/DataStructures.h"
+#include "../../../math/boxkde/Image3D.h"
+#include "../../../math/boxkde/BoxGaus3D.h"
+#include "../../../math/boxkde/Grid3D.h"
+#include "../../../grid/Grid.h"
+#include "../../../floorplan/v2/FloorplanHelper.h"
+
+#include "../../../navMesh/NavMesh.h"
+
+namespace SMC {
+
+    /**
+    * calculate an estimation based on the fast
+    * boxed KDE of Bulli
+    */
+    template <typename State>
+    class ParticleFilterEstimationBoxKDE3D : public ParticleFilterEstimation<State> {
+
+    private:
+        /** boundingBox for the boxKDE */
+		_BBox3<float> bb;
+
+        /** histogram/grid holding the particles*/
+        std::unique_ptr<Grid3D<float>> grid;
+
+        /** bandwith for KDE */
+        Point3 bandwith;
+
+    public:
+
+        ParticleFilterEstimationBoxKDE3D(){
+            //keine boesen woerter
+        }
+
+        ParticleFilterEstimationBoxKDE3D(const Floorplan::IndoorMap* map, const Point3 gridsize_m, const Point3 bandwith) {
+
+			this->bandwith = bandwith;
+			this->bb = FloorplanHelper::getBBox(map);
+			this->bb.grow(10);
+
+            // Create histogram
+			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->grid = std::make_unique<Grid3D<float>>(bb, nBinsX, nBinsY, nBinsZ);
+        }
+
+        template <typename Tria> ParticleFilterEstimationBoxKDE3D(const NM::NavMesh<Tria>* mesh, const Point3 gridsize_m, const Point2 bandwith){
+
+			this->bandwith = bandwith;
+			this->bb = mesh->getBBox();
+			this->bb.grow(10);
+
+            // Create histogram
+			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->grid = std::make_unique<Grid3D<float>>(bb, nBinsX, nBinsY, nBinsZ);
+		}
+
+        State estimate(const std::vector<Particle<State>>& particles) override {
+
+            // compile-time sanity checks
+            static_assert( HasOperatorPlusEq<State>::value, "your state needs a += operator!" );
+            static_assert( HasOperatorDivEq<State>::value, "your state needs a /= operator!" );
+            static_assert( HasOperatorMul<State>::value, "your state needs a * operator!" );
+            static_assert( std::is_constructible<State, Point3>::value, "your state needs a constructor with Point3!");
+            //TODO: check for function getX() and getY()
+
+            grid->clear();
+            for (Particle<State> p : particles)
+            {
+                //grid.add receives position in meter!
+                grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight);
+            }
+
+			_Point3<float> __maxPos;
+			double __weight = grid->maximum(__maxPos);
+
+            int nFilt = 3;
+            float sigmaX = bandwith.x / grid->binSizeX;
+            float sigmaY = bandwith.y / grid->binSizeY;
+			float sigmaZ = bandwith.z / grid->binSizeY;
+
+            BoxGaus3D<float> boxGaus;
+            boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt);
+
+			_Point3<float> maxPos;
+            double weight = grid->maximum(maxPos);
+
+            Assert::isFalse( (isnan(weight)), "the sum of particle weights is NaN!");
+            Assert::isTrue( (weight != 0), "the sum of particle weights is null!");
+
+            //this depends on the given state
+            return State(maxPos);
+        }
+
+    };
+
+}
+
+#endif // PARTICLEFILTERESTIMATIONBOXKDE3D_H