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

Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

Browse Source
This commit is contained in:
k-a-z-u
2018-08-01 18:07:23 +02:00
parent 17f62e8788 9388e6e725
commit 327b580b69
12 changed files with 837 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
CMakeLists.txt
View File

@@ -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")
@@ -67,7 +81,7 @@ else()
# system specific compiler flags
ADD_DEFINITIONS(
-std=gnu++11
-std=gnu++17
-Wall
-Werror=return-type
@@ -105,8 +119,8 @@ ADD_EXECUTABLE(
# needed external libraries
TARGET_LINK_LIBRARIES(
${PROJECT_NAME}
gtest
pthread
# gtest
# pthread
${EXTRA_LIBS}
)

2
floorplan/v2/FloorplanLINT.h
View File

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

69
geo/BBox3.h
View File

@@ -3,34 +3,37 @@
#include "Point3.h"
class BBox3 {
template <typename Scalar>
class _BBox3 {
private:
static constexpr float MAX = +99999;
static constexpr float MIN = -99999;
/** minimum */
Point3 p1;
_Point3<Scalar> p1;
/** maximum */
Point3 p2;
_Point3<Scalar> p2;
public:
/** 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 */
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 */
static BBox3 around(const Point3 center, const Point3 size) {
return BBox3(center-size/2, center+size/2);
static _BBox3 around(const _Point3<Scalar> center, const _Point3<Scalar> 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<Scalar>& 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<Scalar>& getMin() const {return p1;}
/** get the bbox's maximum */
const Point3& getMax() const {return p2;}
const _Point3<Scalar>& getMax() const {return p2;}
/** 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 */
const Point3 getCenter() const {return (p2+p1)/2;}
const _Point3<Scalar> 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<Scalar>(v,v,v));
}
/** 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
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<Scalar>(v,v,v));
}
/** 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
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<Scalar>& 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<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<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);
}
};
using BBox3 = _BBox3<float>;
#endif // BBOX3_H

4
math/boxkde/BoxGaus.h
View File

@@ -86,7 +86,7 @@ private:
val += src[ti + j*w]; // col sum
}
// <EFBFBD>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;
}
// <EFBFBD>berhangbereich rechts vom Bild
// Überhangbereich rechts vom Bild
for (size_t j = h - r; j < h; j++)
{
val += lv - src[li];

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

@@ -0,0 +1,220 @@
#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 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 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 = 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 = 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);
// Fix potential float error
if (val < 0) {
val = 0;
}
dst(i) = val*iarr;
li++;
}
}
};
template struct BoxGaus3D<float>;
template struct BoxGaus3D<double>;

1
math/boxkde/BoxSizes.h
View File

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

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

@@ -0,0 +1,187 @@
#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 sum() const { return data.sum(); }
T minimum() const { return data.minimum(); }
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)
{
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);
//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>;

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

@@ -0,0 +1,229 @@
#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);
}
TValue minimum() const
{
size_t minValueIndex = std::distance(val_begin(), std::min_element(val_begin(), val_end()));
return values[minValueIndex];
}
_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;
}
// 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<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>;

2
sensors/radio/setup/WiFiOptimizerPerFloor.h
View File

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

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

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

@@ -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<float> bb;
_BBox3<float> bb;
/** histogram/grid holding the particles*/
Grid2D<float> grid;
std::unique_ptr<Grid3D<float>> grid;
/** bandwith for KDE */
Point2 bandwith;
Point3 bandwith;
/** the current mesh */
const NM::NavMesh<Tria>* mesh;
@@ -47,19 +48,19 @@ namespace SMC {
public:
/** 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();
const Point3 minBB = mesh->getBBox().getMin();
this->bb = BoundingBox<float>(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<size_t>((maxBB.x - minBB.x) / gridsize_m);
size_t nBinsY = static_cast<size_t>((maxBB.y - minBB.y) / gridsize_m);
this->grid = Grid2D<float>(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<Grid3D<float>>(bb, nBinsX, nBinsY, nBinsZ);
gen.seed(1234);
}
@@ -73,46 +74,30 @@ namespace SMC {
//static_assert( std::is_constructible<State, Point3>::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<State> 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<float> 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<float> 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<Point3> dl;
Distribution::Normal<float> 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<Tria> 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);
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 +107,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;
}

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

@@ -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<Tria> reachable(particlesCopy[i].state.loc, radius);
particles[i].state.loc = reachable.getRandom().drawWithin(particlesCopy[i].state.loc.pos, radius);