From 726d5f8b90db54a77951de63892235df470ae373 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 31 Jul 2018 10:50:50 +0200 Subject: [PATCH 1/7] Replaced gcc specific #warning with #pragma message --- floorplan/v2/FloorplanLINT.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/floorplan/v2/FloorplanLINT.h b/floorplan/v2/FloorplanLINT.h index e497a49..86856e1 100644 --- a/floorplan/v2/FloorplanLINT.h +++ b/floorplan/v2/FloorplanLINT.h @@ -146,7 +146,7 @@ namespace Floorplan { res.push_back(Issue(Type::ERR, floor, "' door is too narrow: " + std::to_string(len_m) + " meter from " + door->from.asString() + " to " + door->to.asString())); } -#warning "TODO!" +#pragma message ("TODO!") // try { // Ray3D::ModelFactory fac(map); // fac.getDoorAbove(floor, door); From 32870a62c62eb3ac52fd02ff07094fe5eafd78a4 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 31 Jul 2018 15:38:59 +0200 Subject: [PATCH 2/7] Added some MSVC compiler switches --- CMakeLists.txt | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0aed9cd..01170d3 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,8 +55,22 @@ endif() if(${CMAKE_GENERATOR} MATCHES "Visual Studio") - SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} /D_X86_ /D_USE_MATH_DEFINES") - SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox /D_X86_ /D_USE_MATH_DEFINES") + add_definitions( + -D_USE_MATH_DEFINES + -DUNICODE + -D_UNICODE + -DNOGDI +# -D_X86_ + ) + + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /permissive-") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zc:__cplusplus") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /std:c++17") + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Zc:twoPhase-") # disable two-phase name lookup due to OpenMP + + + #SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ") + SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /Zi /Oi /GL /Ot /Ox") SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /DEBUG") SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG /INCREMENTAL:NO") @@ -105,8 +119,8 @@ ADD_EXECUTABLE( # needed external libraries TARGET_LINK_LIBRARIES( ${PROJECT_NAME} - gtest - pthread +# gtest +# pthread ${EXTRA_LIBS} ) From 4aec7bae264454710ce671204fff0e239b478b35 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Tue, 31 Jul 2018 15:41:25 +0200 Subject: [PATCH 3/7] Added 3D boxKDE --- geo/BBox3.h | 69 ++--- math/boxkde/BoxGaus3D.h | 262 ++++++++++++++++++ math/boxkde/BoxSizes.h | 1 - math/boxkde/Grid3D.h | 182 ++++++++++++ math/boxkde/Image3D.h | 208 ++++++++++++++ .../ParticleFilterEstimationBoxKDE3D.h | 114 ++++++++ 6 files changed, 803 insertions(+), 33 deletions(-) create mode 100644 math/boxkde/BoxGaus3D.h create mode 100644 math/boxkde/Grid3D.h create mode 100644 math/boxkde/Image3D.h create mode 100644 smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h diff --git a/geo/BBox3.h b/geo/BBox3.h index 2f72d35..5270c61 100644 --- a/geo/BBox3.h +++ b/geo/BBox3.h @@ -3,34 +3,37 @@ #include "Point3.h" -class BBox3 { +template +class _BBox3 { private: - - static constexpr float MAX = +99999; - static constexpr float MIN = -99999; - /** minimum */ - Point3 p1; + _Point3 p1; /** maximum */ - Point3 p2; + _Point3 p2; public: /** empty ctor */ - BBox3() : p1(MAX,MAX,MAX), p2(MIN,MIN,MIN) {;} + _BBox3() : p1(std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()), + p2(std::numeric_limits::lowest(), + std::numeric_limits::lowest(), + std::numeric_limits::lowest()) + {;} /** ctor with min and max */ - BBox3(const Point3 min, const Point3 max) : p1(min), p2(max) {;} + _BBox3(const _Point3 min, const _Point3 max) : p1(min), p2(max) {;} /** create a bbox around the given point */ - static BBox3 around(const Point3 center, const Point3 size) { - return BBox3(center-size/2, center+size/2); + static _BBox3 around(const _Point3 center, const _Point3 size) { + return _BBox3(center-size/2, center+size/2); } /** adjust the bounding-box by adding this point */ - void add(const Point3& p) { + void add(const _Point3& p) { if (p.x > p2.x) {p2.x = p.x;} if (p.y > p2.y) {p2.y = p.y;} @@ -43,25 +46,25 @@ public: } /** add the given bounding-box to this one */ - void add(const BBox3& bb) { + void add(const _BBox3& bb) { add(bb.getMin()); add(bb.getMax()); } /** get the bbox's minimum */ - const Point3& getMin() const {return p1;} + const _Point3& getMin() const {return p1;} /** get the bbox's maximum */ - const Point3& getMax() const {return p2;} + const _Point3& getMax() const {return p2;} /** get the bbox's size */ - const Point3 getSize() const {return p2-p1;} + const _Point3 getSize() const {return p2-p1;} /** get the boox's center */ - const Point3 getCenter() const {return (p2+p1)/2;} + const _Point3 getCenter() const {return (p2+p1)/2;} /** equal? */ - bool operator == (const BBox3& o) const { + bool operator == (const _BBox3& o) const { return (p1.x == o.p1.x) && (p1.y == o.p1.y) && (p1.z == o.p1.z) && @@ -71,38 +74,38 @@ public: } /** shrink the bbox in each dimension by the given amount */ - void shrink(const float v) { - shrink(Point3(v,v,v)); + void shrink(const Scalar v) { + shrink(_Point3(v,v,v)); } /** shrink the bbox by the amount given for each dimension */ - void shrink(const Point3& p) { + void shrink(const _Point3& p) { p1 += p; // increase minimum p2 -= p; // decrease maximum } /** grow the bbox by the amount given for each dimension */ - void grow(const float v) { - grow(Point3(v,v,v)); + void grow(const Scalar v) { + grow(_Point3(v,v,v)); } /** grow the bbox by the amount given for each dimension */ - void grow(const Point3& p) { + void grow(const _Point3& p) { p1 -= p; // decrease minimum p2 += p; // increase maximum } /** set both, min/max z to the same value */ - void setZ(const float z) { + void setZ(const Scalar z) { p1.z = z; p2.z = z; } - void setMinZ(const float z) {this->p1.z = z;} - void setMaxZ(const float z) {this->p2.z = z;} + void setMinZ(const Scalar z) {this->p1.z = z;} + void setMaxZ(const Scalar z) {this->p2.z = z;} /** does the bbox contain the given point? */ - bool contains(const Point3& p) const { + bool contains(const _Point3& p) const { if (p.x < p1.x) {return false;} if (p.x > p2.x) {return false;} if (p.y < p1.y) {return false;} @@ -113,12 +116,14 @@ public: } /** combine two bboxes */ - static BBox3 join(const BBox3& bb1, const BBox3& bb2) { - const Point3 min( std::min(bb1.p1.x, bb2.p1.x), std::min(bb1.p1.y, bb2.p1.y), std::min(bb1.p1.z, bb2.p1.z) ); - const Point3 max( std::max(bb1.p2.x, bb2.p2.x), std::max(bb1.p2.y, bb2.p2.y), std::max(bb1.p2.z, bb2.p2.z) ); - return BBox3(min,max); + static _BBox3 join(const _BBox3& bb1, const _BBox3& bb2) { + const _Point3 min( std::min(bb1.p1.x, bb2.p1.x), std::min(bb1.p1.y, bb2.p1.y), std::min(bb1.p1.z, bb2.p1.z) ); + const _Point3 max( std::max(bb1.p2.x, bb2.p2.x), std::max(bb1.p2.y, bb2.p2.y), std::max(bb1.p2.z, bb2.p2.z) ); + return _BBox3(min,max); } }; +using BBox3 = _BBox3; + #endif // BBOX3_H diff --git a/math/boxkde/BoxGaus3D.h b/math/boxkde/BoxGaus3D.h new file mode 100644 index 0000000..cbf3216 --- /dev/null +++ b/math/boxkde/BoxGaus3D.h @@ -0,0 +1,262 @@ +#pragma once + +#include +#include + +#include "BoxSizes.h" +#include "Image3D.h" + +template +struct BoxGaus3D +{ + void boxFilter(std::vector& input, unsigned w, unsigned h, unsigned d, unsigned filterSize) + { + std::vector buffer(input.size()); + + boxFilter(input.data(), buffer.data(), w, h, d, filterSize); + + input.assign(buffer.begin(), buffer.end()); + } + + void boxFilter(T* input, T* output, unsigned w, unsigned h, unsigned d, unsigned filterSize) + { + assertMsg((filterSize % 2 == 1), "filterSize must be odd"); + unsigned radius = filterSize / 2; + + ImageView3D in (w, h, d, input); + ImageView3D out(w, h, d, output); + + boxBlur(in, out, radius, radius, radius); + } + + void approxGaus(Image3D& input, _Point3 sigma, unsigned nFilt) + { + approxGaus(input.data(), input.getWidth(), input.getHeight(), input.getDepth(), sigma, nFilt); + } + + void approxGaus(std::vector& input, unsigned w, unsigned h, unsigned d, _Point3 sigma, unsigned nFilt) + { + std::vector buffer(input.size()); + + approxGaus(input.data(), buffer.data(), w, h, d, sigma, nFilt); + + input.assign(buffer.begin(), buffer.end()); + } + + void approxGaus(T* input, T* output, unsigned w, unsigned h, unsigned d, _Point3 sigma, unsigned nFilt) + { + ImageView3D in (w, h, d, input); + ImageView3D out(w, h, d, output); + + approxGaus(in, out, sigma, nFilt); + } + + void approxGaus(ImageView3D& in, ImageView3D& out, _Point3 sigma, unsigned nFilt) + { + BoxSizes bsX(sigma.x, nFilt); + BoxSizes bsY(sigma.y, nFilt); + BoxSizes bsZ(sigma.z, nFilt); + + assertMsg((nFilt % 2 == 1), "nFilt must be odd"); + + // if equal, we can save some cond's inside the loop + if (bsX.m == bsY.m && bsX.m == bsZ.m) + { + const size_t m = bsX.m; + + for (size_t i = 1; i <= m; i++) + { + if (i % 2) { + boxBlur(in, out, bsX.wl, bsY.wl, bsZ.wl); + } else { + boxBlur(out, in, bsX.wl, bsY.wl, bsZ.wl); + } + } + + for (size_t i = 1; i <= nFilt - m; i++) + { + if (i % 2) { + boxBlur(in, out, bsX.wu, bsY.wu, bsZ.wu); + } else { + boxBlur(out, in, bsX.wu, bsY.wu, bsZ.wu); + } + } + } + else + { + for (size_t i = 1; i <= nFilt; i++) + { + const size_t rX = (i < bsX.m ? bsX.wl : bsX.wu); + const size_t rY = (i < bsY.m ? bsY.wl : bsY.wu); + const size_t rZ = (i < bsZ.m ? bsZ.wl : bsZ.wu); + + if (i % 2) { + boxBlur(in, out, rX, rY, rZ); + } else{ + boxBlur(out, in, rX, rY, rZ); + } + } + } + } + +private: + void boxBlur(ImageView3D& in, ImageView3D& out, size_t rX, size_t rY, size_t rZ) + { + //box(in, out, r); + boxBlurX(in, out, rX); + boxBlurY(out, in, rY); + boxBlurZ(in, out, rZ); + } + + void boxBlurX(const ImageView3D& src, ImageView3D& dst, size_t r) + { + T iarr = (T)1.0 / (r + r + 1); + + for (size_t z = 0; z < src.getDepth(); z++) + { + for (size_t y = 0; y < src.getHeight(); y++) + { + auto srcView = src.getViewYZ(y, z); + auto dstView = dst.getViewYZ(y, z); + + boxBlur1D(srcView, dstView, src.getWidth(), r, iarr); + } + } + } + + void boxBlurY(const ImageView3D& src, ImageView3D& dst, size_t r) + { + T iarr = (T)1.0 / (r + r + 1); + + for (size_t x = 0; x < src.getWidth(); x++) + { + for (size_t z = 0; z < src.getDepth(); z++) + { + auto srcView = src.getViewXZ(x, z); + auto dstView = dst.getViewXZ(x, z); + + boxBlur1D(srcView, dstView, src.getHeight(), r, iarr); + } + } + } + + void boxBlurZ(const ImageView3D& src, ImageView3D& dst, size_t r) + { + T iarr = (T)1.0 / (r + r + 1); + + for (size_t x = 0; x < src.getWidth(); x++) + { + for (size_t y = 0; y < src.getHeight(); y++) + { + auto srcView = src.getViewXY(x, y); + auto dstView = dst.getViewXY(x, y); + + boxBlur1D(srcView, dstView, src.getDepth(), r, iarr); + } + } + } + + template + void boxBlur1D(const ImageView3D::ConstLineView& src, ImageView3D::LineView& dst, size_t len, size_t r, T iarr) + { + size_t li = 0; // left index + size_t ri = r; // right index + + T fv = src(0); + T lv = src(len - 1); + T val = (r + 1)*fv; + + for (size_t i = 0; i < r; i++) + val += src(i); + + // Überhangbereich links vom Bild + for (size_t i = 0; i <= r; i++) + { + val += src(ri++) - fv; + dst(i) = val*iarr; + } + + // Bildbereich + for (size_t i = r + 1; i < len - r; i++) + { + val += src(ri++) - src(li++); + dst(i) = val*iarr; + } + + // Überhangbereich rechts vom Bild + for (size_t i = len - r; i < len; i++) + { + val += lv - src(li++); + dst(i) = val*iarr; + } + } + + void box(const ImageView3D& src, ImageView3D& dst, size_t r) + { + T iarr = (T)1.0 / (r + r + 1); + + ImageView3D buffer(src); + ImageView3D buffer2(dst.getHeight(), dst.getDepth(), dst.getWidth(), dst.val_begin()); + + for (size_t i = 0; i < 3; i++) + { + for (size_t idx2 = 0; idx2 < buffer.getHeight(); idx2++) + { + for (size_t idx3 = 0; idx3 < buffer.getDepth(); idx3++) + { + box1D(buffer, buffer2, buffer.getWidth(), idx2, idx3, r, iarr); + } + } + + // reinteprete buffer sizes after rotation and swap data pointers + buffer = ImageView3D(buffer.getHeight(), buffer.getDepth(), buffer.getWidth(), buffer.val_begin()); + buffer2 = ImageView3D(buffer2.getHeight(), buffer2.getDepth(), buffer2.getWidth(), buffer2.val_begin()); + buffer.swap(buffer2); + } + + buffer.swap(dst); + } + + void box1D(const ImageView3D& src, ImageView3D& dst, size_t len, size_t idx2, size_t idx3, size_t r, T iarr) + { + for (size_t i = 0; i < len; i++) + { + // dst(idx2, idx3, i) = src(i, idx2, idx3); + + size_t li = 0; // left index + size_t ri = r; // right index + + T fv = src(0, idx2, idx3); + T lv = src(len - 1, idx2, idx3); + T val = (r + 1)*fv; + + for (size_t i = 0; i < r; i++) + val += src(i, idx2, idx3); + + // Überhangbereich links vom Bild + for (size_t i = 0; i <= r; i++) + { + val += src(ri++, idx2, idx3) - fv; + dst(idx2, idx3, i) = val*iarr; + } + + // Bildbereich + for (size_t i = r + 1; i < len - r; i++) + { + val += src(ri++, idx2, idx3) - src(li++, idx2, idx3); + dst(idx2, idx3, i) = val*iarr; + } + + // Überhangbereich rechts vom Bild + for (size_t i = len - r; i < len; i++) + { + val += lv - src(li++, idx2, idx3); + dst(idx2, idx3, i) = val*iarr; + } + } + } +}; + + +template struct BoxGaus3D; +template struct BoxGaus3D; diff --git a/math/boxkde/BoxSizes.h b/math/boxkde/BoxSizes.h index ca228da..0edd085 100644 --- a/math/boxkde/BoxSizes.h +++ b/math/boxkde/BoxSizes.h @@ -1,6 +1,5 @@ #pragma once -#include "DataStructures.h" template struct BoxSizes diff --git a/math/boxkde/Grid3D.h b/math/boxkde/Grid3D.h new file mode 100644 index 0000000..2585460 --- /dev/null +++ b/math/boxkde/Grid3D.h @@ -0,0 +1,182 @@ +#pragma once + +//#include "DataStructures.h" +#include "Image3D.h" + +#include "../../geo/Point3.h" +#include "../../geo/BBox3.h" + +template +struct Grid3D +{ + static_assert(std::is_floating_point::value, "Grid3D only supports float values"); + +public: + const _BBox3 bb; + const size_t numBinsX, numBinsY, numBinsZ; + const T binSizeX, binSizeY, binSizeZ; +private: + Image3D data; + bool empty; + +public: + Grid3D(_BBox3 bb, size_t numBinsAll) + : Grid3D(bb, numBinsAll, numBinsAll, numBinsAll) + {} + + Grid3D(_BBox3 bb, size_t numBinsX, size_t numBinsY, size_t numBinsZ) + : bb(bb), + numBinsX(numBinsX), + numBinsY(numBinsY), + numBinsZ(numBinsZ), + binSizeX((bb.getSize().x + 1) / numBinsX), + binSizeY((bb.getSize().y + 1) / numBinsY), + binSizeZ((bb.getSize().z + 1) / numBinsZ), + data(numBinsX, numBinsY, numBinsZ), + empty(false) + { + } + + Image3D& image() { return data; } + const Image3D& image() const { return data; } + + T& operator() (size_t x, size_t y, size_t z) { return data(x, y, z); } + const T& operator() (size_t x, size_t y, size_t z) const { return data(x, y, z); } + + void clear() { data.clear(); } + + void fill(const std::vector<_Point3>& samples) + { + assertMsg(!samples.empty(), "Samples must be non-empty"); + + if (!empty) + data.clear(); + + const T weight = T(1.0) / samples.size(); + for (const auto& pt : samples) + { + add(pt.x, pt.y, pt.z, weight); + } + + empty = false; + } + + void fill(const std::vector<_Point3>& samples, const std::vector& weights) + { + assertMsg(!samples.empty(), "Samples must be non-empty"); + assertMsg(weights.size() == samples.size(), "Weights must have the same size as samples"); + + if (!empty) + data.clear(); + + for (size_t i = 0; i < samples.size(); i++) + { + add(samples[i].x, samples[i].y, samples[i].z, weights[i]); + } + + empty = false; + } + + _Point3 add(_Point3 pt, T w) + { + return add(pt.x, pt.y, pt.z, w); + } + + _Point3 add(T x, T y, T z, T w) + { + if (assertCond(bb.contains(_Point3(x, y, z)))) + { + std::stringstream ss; + ss << "Point " << "(" << x << "; " << y << "; " << z << ")" << " is out of bounds: " + << "(X: " << bb.getMin().x << " - " << bb.getMax().x << ";" + << " Y: " << bb.getMin().y << " - " << bb.getMax().y << ";" + << " Z: " << bb.getMin().z << " - " << bb.getMax().z << ")"; + assertThrow(ss.str()); + } + return add_simple_bin(x, y, z, w); + } + + T fetch(T x, T y, T z) const + { + 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); + + return data(bin_x, bin_y, bin_z); + } + + T maximum(_Point3& pt) const + { + _Point3 gridPt; + + T maxValue = image().maximum(gridPt); + pt = asInputSpace(gridPt); + return maxValue; + } + + + // // Takes a point in input space and converts it to grid space coordinates + //_Point3 asGridSpace(T x, T y, T z) const + // { + // size_t bin_x = (size_t)((x - bb.MinX) / binSizeX); + // size_t bin_y = (size_t)((y - bb.MinY) / binSizeY); + // size_t bin_z = (size_t)((z - bb.MinZ) / binSizeZ); + + // if (bin_x == data.width) bin_x--; + // if (bin_y == data.height) bin_y--; + // if (bin_z == data.depth) bin_z--; + + // return Point3D(bin_x, bin_y, bin_z); + // } + + // // Takes a Size2D in input space and converts it to grid space coordiantes + //_Point3 asGridSpace(Size3D sz) const + // { + // return _Point3(sz.sX / binSizeX, sz.sY / binSizeY, sz.sZ / binSizeZ); + // } + + // // Takes a Range2D in input space and converts it to grid space coordinates + // Range3D asGridSpace(Range3D range) const + // { + // Point3D startPt = asGridSpace(range.X.Start, range.Y.Start, range.Z.Start); + + // size_t lengthX = range.X.Length / binSizeX; + // size_t lengthY = range.Y.Length / binSizeY; + // size_t lengthZ = range.Z.Length / binSizeZ; + + // return Range3D(startPt.X, lengthX, startPt.Y, lengthY, startPt.Z, lengthZ); + // } + + // Takes a point in grid space and converts it to input space coordinates + _Point3 asInputSpace(_Point3 pt) const + { + return asInputSpace(pt.x, pt.y, pt.z); + } + + // Takes a point in grid space and converts it to input space coordinates + _Point3 asInputSpace(size_t x, size_t y, size_t z) const + { + return _Point3(x * binSizeX + bb.getMin().x + T(0.5)*binSizeX, + y * binSizeY + bb.getMin().y + T(0.5)*binSizeY, + z * binSizeZ + bb.getMin().z + T(0.5)*binSizeZ); + } + +private: + _Point3 add_simple_bin(T x, T y, T z, T w) + { + 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); + + //if (bin_x == data.width) bin_x--; + //if (bin_y == data.height) bin_y--; + //if (bin_z == data.depth) bin_z--; + + data(bin_x, bin_y, bin_z) += w; + + return _Point3(bin_x, bin_y, bin_z); + } +}; + +template struct Grid3D; +template struct Grid3D; diff --git a/math/boxkde/Image3D.h b/math/boxkde/Image3D.h new file mode 100644 index 0000000..958c212 --- /dev/null +++ b/math/boxkde/Image3D.h @@ -0,0 +1,208 @@ +#pragma once + +#include +#include +#include + +#include "../../geo/Point3.h" + +enum struct SlicePlane { XY, XZ, YZ }; + +template +struct ImageView3D +{ + static_assert(std::is_arithmetic::value, "ImageView3D only supports integers or floats"); + + // forward declaration + template struct LineView; + template struct ConstLineView; + +private: + size_t width, height, depth, size; // TODO private +protected: + TValue* values; // contains image data row-wise +public: + + ImageView3D() + : width(0), height(0), depth(0), size(0), values(nullptr) + { + } + + ImageView3D(size_t width, size_t height, size_t depth) + : width(width), height(height), depth(depth), size(width*height*depth), values(nullptr) + { + } + + ImageView3D(size_t width, size_t height, size_t depth, TValue* data) + : width(width), height(height), depth(depth), size(width*height*depth), values(data) + { + } + + TValue& operator() (size_t x, size_t y, size_t z) { return values[indexFromCoord(x, y, z)]; } + const TValue& operator() (size_t x, size_t y, size_t z) const { return values[indexFromCoord(x, y, z)]; } + + TValue* val_begin() { return values; } + TValue* val_end () { return values + size; } + + const TValue* val_begin() const { return values; } + const TValue* val_end () const { return values + size; } + + size_t getWidth() const { return width; } + size_t getHeight() const { return height; } + size_t getDepth() const { return depth; } + + size_t indexFromCoord(size_t x, size_t y, size_t z) const + { + assertMsg(x < width && y < height && z < depth, "(x,y,z) out of bounds"); + return z*width*height + y*width + x; + } + + _Point3 coordFromIndex(size_t index) const + { + assertMsg(index < size, "Index out of bounds"); + + size_t z = index / (width*height); + size_t y = (index - z*width*height) / width; + size_t x = index - y*width - z*width*height; + + return _Point3(x, y, z); + } + + _Point3 maximum() const + { + size_t maxValueIndex = std::distance(val_begin(), std::max_element(val_begin(), val_end())); + return coordFromIndex(maxValueIndex); + } + + TValue maximum(_Point3& pt) const + { + size_t maxValueIndex = std::distance(val_begin(), std::max_element(val_begin(), val_end())); + pt = coordFromIndex(maxValueIndex); + return values[maxValueIndex]; + } + + void clear() + { + std::fill(val_begin(), val_end(), TValue(0)); + } + + void assign(const ImageView3D& other) + { + assertMsg(size == other.size, "Other must be of same size as this"); + std::copy(other.val_begin(), other.val_end(), val_begin()); + } + + TValue sum() const + { + return std::accumulate(val_begin(), val_end(), TValue(0)); + } + + void swap(ImageView3D& other) + { + TValue* tmp = other.values; + other.values = this->values; + this->values = tmp; + } + +public: + typedef LineView LineViewXY; + typedef LineView LineViewXZ; + typedef LineView LineViewYZ; + + typedef ConstLineView ConstLineViewXY; + typedef ConstLineView ConstLineViewXZ; + typedef ConstLineView ConstLineViewYZ; + + LineViewXY getViewXY(size_t fixedX, size_t fixedY) { return LineViewXY(*this, fixedX, fixedY); } + LineViewXZ getViewXZ(size_t fixedX, size_t fixedZ) { return LineViewXZ(*this, fixedX, fixedZ); } + LineViewYZ getViewYZ(size_t fixedY, size_t fixedZ) { return LineViewYZ(*this, fixedY, fixedZ); } + + ConstLineViewXY getViewXY(size_t fixedX, size_t fixedY) const { return ConstLineViewXY(*this, fixedX, fixedY); } + ConstLineViewXZ getViewXZ(size_t fixedX, size_t fixedZ) const { return ConstLineViewXZ(*this, fixedX, fixedZ); } + ConstLineViewYZ getViewYZ(size_t fixedY, size_t fixedZ) const { return ConstLineViewYZ(*this, fixedY, fixedZ); } + + template + struct LineView + { + size_t fixIdx1, fixIdx2; + ImageView3D& img; + + LineView(ImageView3D& img, size_t fixedIndex1, size_t fixedIndex2) + : fixIdx1(fixedIndex1), fixIdx2(fixedIndex2), img(img) + {} + + TValue& operator() (size_t idx) + { + switch (S) + { + case SlicePlane::XY: return img(fixIdx1, fixIdx2, idx); + case SlicePlane::XZ: return img(fixIdx1, idx, fixIdx2); + case SlicePlane::YZ: return img(idx, fixIdx1, fixIdx2); + default: assert(false); abort(); + } + } + }; + + template + struct ConstLineView + { + size_t fixIdx1, fixIdx2; + const ImageView3D& img; + + ConstLineView(const ImageView3D& img, size_t fixedIndex1, size_t fixedIndex2) + : fixIdx1(fixedIndex1), fixIdx2(fixedIndex2), img(img) + {} + + const TValue& operator() (size_t idx) + { + switch (S) + { + case SlicePlane::XY: return img(fixIdx1, fixIdx2, idx); + case SlicePlane::XZ: return img(fixIdx1, idx, fixIdx2); + case SlicePlane::YZ: return img(idx, fixIdx1, fixIdx2); + default: assert(false); abort(); + } + } + + const TValue& operator() (size_t idx) const + { + switch (S) + { + case SlicePlane::XY: return img(fixIdx1, fixIdx2, idx); + case SlicePlane::XZ: return img(fixIdx1, idx, fixIdx2); + case SlicePlane::YZ: return img(idx, fixIdx1, fixIdx2); + default: assert(false); abort(); + } + } + }; +}; + + +template +struct Image3D : public ImageView3D +{ + std::vector values_vec; + +public: + Image3D(size_t width, size_t height, size_t depth) + : ImageView3D(width, height, depth), values_vec(width * height * depth) + { + this->values = values_vec.data(); + } + + Image3D(size_t width, size_t height, size_t depth, std::vector& data) + : ImageView3D(width, height, depth), values_vec(data) + { + assertMsg(data.size() == width*height*depth, "Data must be of the same size as this"); + this->values = values_vec.data(); + } + + std::vector& data() { return values_vec; } + const std::vector& data() const { return values_vec; } +}; + +template struct ImageView3D; +//template struct ImageView3D; + +template struct Image3D; +//template struct Image3D; \ No newline at end of file diff --git a/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h new file mode 100644 index 0000000..aec2bbc --- /dev/null +++ b/smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h @@ -0,0 +1,114 @@ +#ifndef PARTICLEFILTERESTIMATIONBOXKDE3D_H +#define PARTICLEFILTERESTIMATIONBOXKDE3D_H + +#include +#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 + class ParticleFilterEstimationBoxKDE3D : public ParticleFilterEstimation { + + private: + /** boundingBox for the boxKDE */ + _BBox3 bb; + + /** histogram/grid holding the particles*/ + std::unique_ptr> 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>(bb, nBinsX, nBinsY, nBinsZ); + } + + template ParticleFilterEstimationBoxKDE3D(const NM::NavMesh* 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>(bb, nBinsX, nBinsY, nBinsZ); + } + + State estimate(const std::vector>& particles) override { + + // compile-time sanity checks + static_assert( HasOperatorPlusEq::value, "your state needs a += operator!" ); + static_assert( HasOperatorDivEq::value, "your state needs a /= operator!" ); + static_assert( HasOperatorMul::value, "your state needs a * operator!" ); + static_assert( std::is_constructible::value, "your state needs a constructor with Point3!"); + //TODO: check for function getX() and getY() + + grid->clear(); + for (Particle p : particles) + { + //grid.add receives position in meter! + grid->add(p.state.getX(), p.state.getY(), p.state.getZ(), p.weight); + } + + _Point3 __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 boxGaus; + boxGaus.approxGaus(grid->image(), Point3(sigmaX, sigmaY, sigmaZ), nFilt); + + _Point3 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 From d912a76246453f6d47f654716996a3126aee98f3 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Wed, 1 Aug 2018 10:18:45 +0200 Subject: [PATCH 4/7] Fixed encoding --- math/boxkde/BoxGaus.h | 4 ++-- math/boxkde/BoxGaus3D.h | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/math/boxkde/BoxGaus.h b/math/boxkde/BoxGaus.h index 729c58f..11fa8a4 100644 --- a/math/boxkde/BoxGaus.h +++ b/math/boxkde/BoxGaus.h @@ -86,7 +86,7 @@ private: val += src[ti + j*w]; // col sum } - // Überhangbereich links vom Bild + // Ãœberhangbereich links vom Bild for (size_t j = 0; j <= r; j++) { val += src[ri] - fv; @@ -107,7 +107,7 @@ private: ti += w; } - // Überhangbereich rechts vom Bild + // Ãœberhangbereich rechts vom Bild for (size_t j = h - r; j < h; j++) { val += lv - src[li]; diff --git a/math/boxkde/BoxGaus3D.h b/math/boxkde/BoxGaus3D.h index cbf3216..09196d9 100644 --- a/math/boxkde/BoxGaus3D.h +++ b/math/boxkde/BoxGaus3D.h @@ -169,7 +169,7 @@ private: for (size_t i = 0; i < r; i++) val += src(i); - // Überhangbereich links vom Bild + // Ãœberhangbereich links vom Bild for (size_t i = 0; i <= r; i++) { val += src(ri++) - fv; @@ -183,7 +183,7 @@ private: dst(i) = val*iarr; } - // Überhangbereich rechts vom Bild + // Ãœberhangbereich rechts vom Bild for (size_t i = len - r; i < len; i++) { val += lv - src(li++); @@ -233,7 +233,7 @@ private: for (size_t i = 0; i < r; i++) val += src(i, idx2, idx3); - // Überhangbereich links vom Bild + // Ãœberhangbereich links vom Bild for (size_t i = 0; i <= r; i++) { val += src(ri++, idx2, idx3) - fv; @@ -247,7 +247,7 @@ private: dst(idx2, idx3, i) = val*iarr; } - // Überhangbereich rechts vom Bild + // Ãœberhangbereich rechts vom Bild for (size_t i = len - r; i < len; i++) { val += lv - src(li++, idx2, idx3); From 0bb22b54e90a1fef3daec01e439ecd78790b0fb4 Mon Sep 17 00:00:00 2001 From: toni Date: Wed, 1 Aug 2018 11:54:10 +0200 Subject: [PATCH 5/7] added KDE3DResampling Method. However BoxKDE3D seems to be buggy --- CMakeLists.txt | 2 +- math/boxkde/BoxGaus3D.h | 5 +- math/boxkde/Grid3D.h | 2 + math/boxkde/Image3D.h | 17 ++++- .../resampling/ParticleFilterResamplingKDE.h | 71 ++++++++----------- ...icleFilterResamplingSimpleImpoverishment.h | 2 +- 6 files changed, 53 insertions(+), 46 deletions(-) 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); From cbba8125588a2dcce08ae8e5bfdd4266bdebebf4 Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Wed, 1 Aug 2018 15:50:37 +0200 Subject: [PATCH 6/7] Replaced sleep --- sensors/radio/setup/WiFiOptimizerPerFloor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensors/radio/setup/WiFiOptimizerPerFloor.h b/sensors/radio/setup/WiFiOptimizerPerFloor.h index 7d9d3c4..c326850 100644 --- a/sensors/radio/setup/WiFiOptimizerPerFloor.h +++ b/sensors/radio/setup/WiFiOptimizerPerFloor.h @@ -149,7 +149,7 @@ namespace WiFiOptimizer { gp.flush(); pts.clear(); lines.clear(); - sleep(1); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); #endif // 5) run the optimizer From 9388e6e725493ed1f91b11d45616b0095f0e98fa Mon Sep 17 00:00:00 2001 From: Markus Bullmann Date: Wed, 1 Aug 2018 16:01:49 +0200 Subject: [PATCH 7/7] Workaround for stupid float errors in boxKDE 3D --- math/boxkde/BoxGaus3D.h | 93 +++++-------------- math/boxkde/Grid3D.h | 3 + math/boxkde/Image3D.h | 6 ++ .../resampling/ParticleFilterResamplingKDE.h | 4 - 4 files changed, 34 insertions(+), 72 deletions(-) diff --git a/math/boxkde/BoxGaus3D.h b/math/boxkde/BoxGaus3D.h index 34f0fac..e915a37 100644 --- a/math/boxkde/BoxGaus3D.h +++ b/math/boxkde/BoxGaus3D.h @@ -173,87 +173,44 @@ private: // Ãœberhangbereich links vom Bild for (size_t i = 0; i <= r; i++) { - val += src(ri++) - fv; + val = val + src(ri) - fv; + + // Fix potential float error + if (val < 0) { + val = 0; + } + dst(i) = val*iarr; + ri++; } // Bildbereich for (size_t i = r + 1; i < len - r; i++) { - val += src(ri++) - src(li++); + val = val + src(ri) - src(li); + + // Fix potential float error + if (val < 0) { + val = 0; + } + dst(i) = val*iarr; + ri++; + li++; } // Ãœberhangbereich rechts vom Bild for (size_t i = len - r; i < len; i++) { - val += lv - src(li++); + val += lv - src(li); + + // Fix potential float error + if (val < 0) { + val = 0; + } + dst(i) = val*iarr; - } - } - - void box(const ImageView3D& src, ImageView3D& dst, size_t r) - { - T iarr = (T)1.0 / (r + r + 1); - - ImageView3D buffer(src); - ImageView3D buffer2(dst.getHeight(), dst.getDepth(), dst.getWidth(), dst.val_begin()); - - for (size_t i = 0; i < 3; i++) - { - for (size_t idx2 = 0; idx2 < buffer.getHeight(); idx2++) - { - for (size_t idx3 = 0; idx3 < buffer.getDepth(); idx3++) - { - box1D(buffer, buffer2, buffer.getWidth(), idx2, idx3, r, iarr); - } - } - - // reinteprete buffer sizes after rotation and swap data pointers - buffer = ImageView3D(buffer.getHeight(), buffer.getDepth(), buffer.getWidth(), buffer.val_begin()); - buffer2 = ImageView3D(buffer2.getHeight(), buffer2.getDepth(), buffer2.getWidth(), buffer2.val_begin()); - buffer.swap(buffer2); - } - - buffer.swap(dst); - } - - void box1D(const ImageView3D& src, ImageView3D& dst, size_t len, size_t idx2, size_t idx3, size_t r, T iarr) - { - for (size_t i = 0; i < len; i++) - { - // dst(idx2, idx3, i) = src(i, idx2, idx3); - - size_t li = 0; // left index - size_t ri = r; // right index - - T fv = src(0, idx2, idx3); - T lv = src(len - 1, idx2, idx3); - T val = (r + 1)*fv; - - for (size_t i = 0; i < r; i++) - val += src(i, idx2, idx3); - - // Ãœberhangbereich links vom Bild - for (size_t i = 0; i <= r; i++) - { - val += src(ri++, idx2, idx3) - fv; - dst(idx2, idx3, i) = val*iarr; - } - - // Bildbereich - for (size_t i = r + 1; i < len - r; i++) - { - val += src(ri++, idx2, idx3) - src(li++, idx2, idx3); - dst(idx2, idx3, i) = val*iarr; - } - - // Ãœberhangbereich rechts vom Bild - for (size_t i = len - r; i < len; i++) - { - val += lv - src(li++, idx2, idx3); - dst(idx2, idx3, i) = val*iarr; - } + li++; } } }; diff --git a/math/boxkde/Grid3D.h b/math/boxkde/Grid3D.h index 8093c4b..824aef4 100644 --- a/math/boxkde/Grid3D.h +++ b/math/boxkde/Grid3D.h @@ -105,6 +105,9 @@ public: return data(bin_x, bin_y, bin_z); } + T sum() const { return data.sum(); } + T minimum() const { return data.minimum(); } + T maximum(_Point3& pt) const { _Point3 gridPt; diff --git a/math/boxkde/Image3D.h b/math/boxkde/Image3D.h index 4180f80..1754fa3 100644 --- a/math/boxkde/Image3D.h +++ b/math/boxkde/Image3D.h @@ -68,6 +68,12 @@ public: return _Point3(x, y, z); } + TValue minimum() const + { + size_t minValueIndex = std::distance(val_begin(), std::min_element(val_begin(), val_end())); + return values[minValueIndex]; + } + _Point3 maximum() const { size_t maxValueIndex = std::distance(val_begin(), std::max_element(val_begin(), val_end())); diff --git a/smc/filtering/resampling/ParticleFilterResamplingKDE.h b/smc/filtering/resampling/ParticleFilterResamplingKDE.h index cd6c09a..349d61f 100644 --- a/smc/filtering/resampling/ParticleFilterResamplingKDE.h +++ b/smc/filtering/resampling/ParticleFilterResamplingKDE.h @@ -95,10 +95,6 @@ namespace SMC { NM::NavMeshLocation tmpPos = mesh->getRandom().draw(); float weight = grid->fetch(tmpPos.pos.x, tmpPos.pos.y, tmpPos.pos.z); - if(weight < 0 ){ - int halloBulli = 666; - } - dl.add(tmpPos.pos, weight); }