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

Added 3D boxKDE

Browse Source
This commit is contained in:
Markus Bullmann
2018-07-31 15:41:25 +02:00
parent 32870a62c6
commit 4aec7bae26
6 changed files with 803 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
geo/BBox3.h
View File

@@ -3,34 +3,37 @@
#include "Point3.h" #include "Point3.h"
class BBox3 { template <typename Scalar>
class _BBox3 {
private: private:
static constexpr float MAX = +99999;
static constexpr float MIN = -99999;
/** minimum */ /** minimum */
Point3 p1; _Point3<Scalar> p1;
/** maximum */ /** maximum */
Point3 p2; _Point3<Scalar> p2;
public: public:
/** empty ctor */ /** empty ctor */
BBox3() : p1(MAX,MAX,MAX), p2(MIN,MIN,MIN) {;} _BBox3() : p1(std::numeric_limits<Scalar>::max(),
std::numeric_limits<Scalar>::max(),
std::numeric_limits<Scalar>::max()),
p2(std::numeric_limits<Scalar>::lowest(),
std::numeric_limits<Scalar>::lowest(),
std::numeric_limits<Scalar>::lowest())
{;}
/** ctor with min and max */ /** ctor with min and max */
BBox3(const Point3 min, const Point3 max) : p1(min), p2(max) {;} _BBox3(const _Point3<Scalar> min, const _Point3<Scalar> max) : p1(min), p2(max) {;}
/** create a bbox around the given point */ /** create a bbox around the given point */
static BBox3 around(const Point3 center, const Point3 size) { static _BBox3 around(const _Point3<Scalar> center, const _Point3<Scalar> size) {
return BBox3(center-size/2, center+size/2); return _BBox3(center-size/2, center+size/2);
} }
/** adjust the bounding-box by adding this point */ /** adjust the bounding-box by adding this point */
void add(const Point3& p) { void add(const _Point3<Scalar>& p) {
if (p.x > p2.x) {p2.x = p.x;} if (p.x > p2.x) {p2.x = p.x;}
if (p.y > p2.y) {p2.y = p.y;} if (p.y > p2.y) {p2.y = p.y;}
@@ -43,25 +46,25 @@ public:
} }
/** add the given bounding-box to this one */ /** add the given bounding-box to this one */
void add(const BBox3& bb) { void add(const _BBox3& bb) {
add(bb.getMin()); add(bb.getMin());
add(bb.getMax()); add(bb.getMax());
} }
/** get the bbox's minimum */ /** get the bbox's minimum */
const Point3& getMin() const {return p1;} const _Point3<Scalar>& getMin() const {return p1;}
/** get the bbox's maximum */ /** get the bbox's maximum */
const Point3& getMax() const {return p2;} const _Point3<Scalar>& getMax() const {return p2;}
/** get the bbox's size */ /** get the bbox's size */
const Point3 getSize() const {return p2-p1;} const _Point3<Scalar> getSize() const {return p2-p1;}
/** get the boox's center */ /** get the boox's center */
const Point3 getCenter() const {return (p2+p1)/2;} const _Point3<Scalar> getCenter() const {return (p2+p1)/2;}
/** equal? */ /** equal? */
bool operator == (const BBox3& o) const { bool operator == (const _BBox3& o) const {
return (p1.x == o.p1.x) && return (p1.x == o.p1.x) &&
(p1.y == o.p1.y) && (p1.y == o.p1.y) &&
(p1.z == o.p1.z) && (p1.z == o.p1.z) &&
@@ -71,38 +74,38 @@ public:
} }
/** shrink the bbox in each dimension by the given amount */ /** shrink the bbox in each dimension by the given amount */
void shrink(const float v) { void shrink(const Scalar v) {
shrink(Point3(v,v,v)); shrink(_Point3<Scalar>(v,v,v));
} }
/** shrink the bbox by the amount given for each dimension */ /** shrink the bbox by the amount given for each dimension */
void shrink(const Point3& p) { void shrink(const _Point3<Scalar>& p) {
p1 += p; // increase minimum p1 += p; // increase minimum
p2 -= p; // decrease maximum p2 -= p; // decrease maximum
} }
/** grow the bbox by the amount given for each dimension */ /** grow the bbox by the amount given for each dimension */
void grow(const float v) { void grow(const Scalar v) {
grow(Point3(v,v,v)); grow(_Point3<Scalar>(v,v,v));
} }
/** grow the bbox by the amount given for each dimension */ /** grow the bbox by the amount given for each dimension */
void grow(const Point3& p) { void grow(const _Point3<Scalar>& p) {
p1 -= p; // decrease minimum p1 -= p; // decrease minimum
p2 += p; // increase maximum p2 += p; // increase maximum
} }
/** set both, min/max z to the same value */ /** set both, min/max z to the same value */
void setZ(const float z) { void setZ(const Scalar z) {
p1.z = z; p1.z = z;
p2.z = z; p2.z = z;
} }
void setMinZ(const float z) {this->p1.z = z;} void setMinZ(const Scalar z) {this->p1.z = z;}
void setMaxZ(const float z) {this->p2.z = z;} void setMaxZ(const Scalar z) {this->p2.z = z;}
/** does the bbox contain the given point? */ /** does the bbox contain the given point? */
bool contains(const Point3& p) const { bool contains(const _Point3<Scalar>& p) const {
if (p.x < p1.x) {return false;} if (p.x < p1.x) {return false;}
if (p.x > p2.x) {return false;} if (p.x > p2.x) {return false;}
if (p.y < p1.y) {return false;} if (p.y < p1.y) {return false;}
@@ -113,12 +116,14 @@ public:
} }
/** combine two bboxes */ /** combine two bboxes */
static BBox3 join(const BBox3& bb1, const BBox3& bb2) { 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<Scalar> 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) ); const _Point3<Scalar> 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); return _BBox3(min,max);
} }
}; };
using BBox3 = _BBox3<float>;
#endif // BBOX3_H #endif // BBOX3_H

262
math/boxkde/BoxGaus3D.h Normal file
View File

@@ -0,0 +1,262 @@
#pragma once
#include <cmath>
#include <vector>
#include "BoxSizes.h"
#include "Image3D.h"
template <class T>
struct BoxGaus3D
{
void boxFilter(std::vector<T>& input, unsigned w, unsigned h, unsigned d, unsigned filterSize)
{
std::vector<T> 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<T> in (w, h, d, input);
ImageView3D<T> out(w, h, d, output);
boxBlur(in, out, radius, radius, radius);
}
void approxGaus(Image3D<T>& input, _Point3<T> sigma, unsigned nFilt)
{
approxGaus(input.data(), input.getWidth(), input.getHeight(), input.getDepth(), sigma, nFilt);
}
void approxGaus(std::vector<T>& input, unsigned w, unsigned h, unsigned d, _Point3<T> sigma, unsigned nFilt)
{
std::vector<T> 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<T> sigma, unsigned nFilt)
{
ImageView3D<T> in (w, h, d, input);
ImageView3D<T> out(w, h, d, output);
approxGaus(in, out, sigma, nFilt);
}
void approxGaus(ImageView3D<T>& in, ImageView3D<T>& out, _Point3<T> sigma, unsigned nFilt)
{
BoxSizes<T> bsX(sigma.x, nFilt);
BoxSizes<T> bsY(sigma.y, nFilt);
BoxSizes<T> 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<T>& in, ImageView3D<T>& 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<T>& src, ImageView3D<T>& 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<SlicePlane::YZ>(srcView, dstView, src.getWidth(), r, iarr);
}
}
}
void boxBlurY(const ImageView3D<T>& src, ImageView3D<T>& 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<SlicePlane::XZ>(srcView, dstView, src.getHeight(), r, iarr);
}
}
}
void boxBlurZ(const ImageView3D<T>& src, ImageView3D<T>& 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<SlicePlane::XY>(srcView, dstView, src.getDepth(), r, iarr);
}
}
}
template<SlicePlane S>
void boxBlur1D(const ImageView3D<T>::ConstLineView<S>& src, ImageView3D<T>::LineView<S>& 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);
// <20>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;
}
// <20>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<T>& src, ImageView3D<T>& dst, size_t r)
{
T iarr = (T)1.0 / (r + r + 1);
ImageView3D<T> buffer(src);
ImageView3D<T> 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<T>(buffer.getHeight(), buffer.getDepth(), buffer.getWidth(), buffer.val_begin());
buffer2 = ImageView3D<T>(buffer2.getHeight(), buffer2.getDepth(), buffer2.getWidth(), buffer2.val_begin());
buffer.swap(buffer2);
}
buffer.swap(dst);
}
void box1D(const ImageView3D<T>& src, ImageView3D<T>& 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);
// <20>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;
}
// <20>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<float>;
template struct BoxGaus3D<double>;

1
math/boxkde/BoxSizes.h
View File

@@ -1,6 +1,5 @@
#pragma once #pragma once
#include "DataStructures.h"
template<typename T> template<typename T>
struct BoxSizes struct BoxSizes

182
math/boxkde/Grid3D.h Normal file
View File

@@ -0,0 +1,182 @@
#pragma once
//#include "DataStructures.h"
#include "Image3D.h"
#include "../../geo/Point3.h"
#include "../../geo/BBox3.h"
template<class T>
struct Grid3D
{
static_assert(std::is_floating_point<T>::value, "Grid3D only supports float values");
public:
const _BBox3<T> bb;
const size_t numBinsX, numBinsY, numBinsZ;
const T binSizeX, binSizeY, binSizeZ;
private:
Image3D<T> data;
bool empty;
public:
Grid3D(_BBox3<T> bb, size_t numBinsAll)
: Grid3D(bb, numBinsAll, numBinsAll, numBinsAll)
{}
Grid3D(_BBox3<T> 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<T>& image() { return data; }
const Image3D<T>& 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<T>>& 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<T>>& samples, const std::vector<T>& 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<size_t> add(_Point3<T> pt, T w)
{
return add(pt.x, pt.y, pt.z, w);
}
_Point3<size_t> add(T x, T y, T z, T w)
{
if (assertCond(bb.contains(_Point3<T>(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<T>& pt) const
{
_Point3<size_t> 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<size_t> 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<size_t>(bin_x, bin_y, bin_z);
// }
// // Takes a Size2D in input space and converts it to grid space coordiantes
//_Point3<size_t> asGridSpace(Size3D<T> sz) const
// {
// return _Point3<size_t>(sz.sX / binSizeX, sz.sY / binSizeY, sz.sZ / binSizeZ);
// }
// // Takes a Range2D in input space and converts it to grid space coordinates
// Range3D<size_t> asGridSpace(Range3D<T> range) const
// {
// Point3D<size_t> 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<size_t>(startPt.X, lengthX, startPt.Y, lengthY, startPt.Z, lengthZ);
// }
// Takes a point in grid space and converts it to input space coordinates
_Point3<T> asInputSpace(_Point3<size_t> pt) const
{
return asInputSpace(pt.x, pt.y, pt.z);
}
// Takes a point in grid space and converts it to input space coordinates
_Point3<T> asInputSpace(size_t x, size_t y, size_t z) const
{
return _Point3<T>(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<size_t> 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<size_t>(bin_x, bin_y, bin_z);
}
};
template struct Grid3D<float>;
template struct Grid3D<double>;

208
math/boxkde/Image3D.h Normal file
View File

@@ -0,0 +1,208 @@
#pragma once
#include <cassert>
#include <numeric>
#include <vector>
#include "../../geo/Point3.h"
enum struct SlicePlane { XY, XZ, YZ };
template <class TValue>
struct ImageView3D
{
static_assert(std::is_arithmetic<TValue>::value, "ImageView3D only supports integers or floats");
// forward declaration
template<SlicePlane S> struct LineView;
template<SlicePlane S> 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<size_t> 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<size_t>(x, y, z);
}
_Point3<size_t> maximum() const
{
size_t maxValueIndex = std::distance(val_begin(), std::max_element(val_begin(), val_end()));
return coordFromIndex(maxValueIndex);
}
TValue maximum(_Point3<size_t>& 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<TValue>& 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<TValue>& other)
{
TValue* tmp = other.values;
other.values = this->values;
this->values = tmp;
}
public:
typedef LineView<SlicePlane::XY> LineViewXY;
typedef LineView<SlicePlane::XZ> LineViewXZ;
typedef LineView<SlicePlane::YZ> LineViewYZ;
typedef ConstLineView<SlicePlane::XY> ConstLineViewXY;
typedef ConstLineView<SlicePlane::XZ> ConstLineViewXZ;
typedef ConstLineView<SlicePlane::YZ> 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<SlicePlane S>
struct LineView
{
size_t fixIdx1, fixIdx2;
ImageView3D<TValue>& img;
LineView(ImageView3D<TValue>& 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<SlicePlane S>
struct ConstLineView
{
size_t fixIdx1, fixIdx2;
const ImageView3D<TValue>& img;
ConstLineView(const ImageView3D<TValue>& 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 <class TValue>
struct Image3D : public ImageView3D<TValue>
{
std::vector<TValue> values_vec;
public:
Image3D(size_t width, size_t height, size_t depth)
: ImageView3D<TValue>(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<TValue>& data)
: ImageView3D<TValue>(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<TValue>& data() { return values_vec; }
const std::vector<TValue>& data() const { return values_vec; }
};
template struct ImageView3D<float>;
//template struct ImageView3D<double>;
template struct Image3D<float>;
//template struct Image3D<double>;

114
smc/filtering/estimation/ParticleFilterEstimationBoxKDE3D.h Normal file
View File

@@ -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