diff --git a/Assertions.h b/Assertions.h index 4c727b8..be6c370 100644 --- a/Assertions.h +++ b/Assertions.h @@ -52,7 +52,7 @@ namespace Assert { } template static inline void isNotNull(const T& v, const STR err) { - if (v == nullptr) {doThrow(err);} + if (v == nullptr) {doThrow(err);} } template static inline void isNotNaN(const T v, const STR err) { diff --git a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h index 653f9e6..e29e3f8 100644 --- a/grid/walk/v2/modules/WalkModuleHeadingVonMises.h +++ b/grid/walk/v2/modules/WalkModuleHeadingVonMises.h @@ -52,7 +52,7 @@ public: state.heading.direction += ctrl->turnSinceLastTransition_rad + var; //set kappa of mises - float kappa = 5 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad)); + float kappa = 600 / std::exp(2 * std::abs(ctrl->motionDeltaAngle_rad)); dist.setKappa(kappa); } diff --git a/math/boxkde/BoxGaus.h b/math/boxkde/BoxGaus.h new file mode 100644 index 0000000..1888cb6 --- /dev/null +++ b/math/boxkde/BoxGaus.h @@ -0,0 +1,127 @@ +#pragma once + +#include +#include + +#include "BoxSizes.h" + +template +struct BoxGaus +{ + void boxfilter(std::vector& input, size_t w, size_t h, unsigned filterSize) + { + assertMsg((filterSize % 2) == 1, "filterSize must be odd"); + + unsigned radius = filterSize / 2; + + std::vector buffer(input.size()); + boxBlur(input, buffer, w, h, radius); + boxBlur(buffer, input, w, h, radius); + } + + void approxGaus(Image2D& input, T sigmaX, T sigmaY, unsigned nFilt) + { + approxGaus(input.data(), input.width, input.height, sigmaX, sigmaY, nFilt); + } + + void approxGaus(std::vector& input, size_t w, size_t h, T sigmaX, T sigmaY, unsigned nFilt) + { + BoxSizes bsX(sigmaX, nFilt); + BoxSizes bsY(sigmaY, nFilt); + std::vector buffer(input.size()); + + assertMsg((2 * bsX.wl + 1 < w) && (2 * bsX.wl + 1 < h), "Box-Filter size in X direction is too big"); + assertMsg((2 * bsX.wu + 1 < w) && (2 * bsX.wu + 1 < h), "Box-Filter size in X direction is too big"); + assertMsg((2 * bsY.wl + 1 < w) && (2 * bsY.wl + 1 < h), "Box-Filter size in Y direction is too big"); + assertMsg((2 * bsY.wu + 1 < w) && (2 * bsY.wu + 1 < h), "Box-Filter size in Y direction is too big"); + + // if equal, we can save some cond's inside the loop + if (bsX.m == bsY.m) + { + const size_t m = bsX.m; + + for (size_t i = 0; i < m; i++) + { + boxBlur(input, buffer, w, h, bsY.wl); + boxBlur(buffer, input, w, h, bsX.wl); + } + + for (size_t i = 0; i < nFilt - m; i++) + { + boxBlur(input, buffer, w, h, bsY.wu); + boxBlur(buffer, input, w, h, bsX.wu); + } + } + else + { + for (size_t i = 0; i < nFilt; i++) + { + boxBlur(input, buffer, w, h, (i < bsY.m ? bsY.wl : bsY.wu) ); + boxBlur(buffer, input, w, h, (i < bsX.m ? bsX.wl : bsX.wu)); + } + } + + } + +private: + void boxBlur(const std::vector &src, std::vector &dst, size_t w, size_t h, size_t r) + { + T iarr = (T)1.0 / (r + r + 1); + + for (size_t i = 0; i < w; i++) + { + // Init indexes + size_t ti = i; + size_t li = ti; + size_t ri = ti + r*w; + + // Init values + T fv = src[ti]; // first values + T lv = src[ti + w*(h - 1)]; // last values + T val = fv * (r + 1); // overhang over image border + + for (size_t j = 0; j < r; j++) + { + val += src[ti + j*w]; // col sum + } + + // Überhangbereich links vom Bild + for (size_t j = 0; j <= r; j++) + { + val += src[ri] - fv; + dst[j + i*w] = val * iarr; + + ri += w; + ti += w; + } + + // Bildbereich + for (size_t j = r + 1; j < h - r; j++) + { + val += src[ri] - src[li]; + dst[j + i*w] = val * iarr; + + li += w; + ri += w; + ti += w; + } + + // Überhangbereich rechts vom Bild + for (size_t j = h - r; j < h; j++) + { + val += lv - src[li]; + dst[j + i*w] = val * iarr; + + li += w; + ti += w; + } + } + } +}; + + + + + + + diff --git a/math/boxkde/BoxSizes.h b/math/boxkde/BoxSizes.h new file mode 100644 index 0000000..ca228da --- /dev/null +++ b/math/boxkde/BoxSizes.h @@ -0,0 +1,85 @@ +#pragma once + +#include "DataStructures.h" + +template +struct BoxSizes +{ + static_assert(std::is_floating_point::value, "This class only works with floats."); + + T sigma, sigmaActual; + T wIdeal, mIdeal; + + unsigned n, m, wl, wu; + + BoxSizes(T sigma, unsigned n) + : sigma(sigma), n(n) + { + assertMsg(sigma >= 0.8, "Sigma values below about 0.8 cannot be represented"); + + wIdeal = sqrt(12 * sigma*sigma / n + 1); // Ideal averaging filter width + + // wl is first odd valued integer less than wIdeal + wl = (unsigned)floor(wIdeal); + if (wl % 2 == 0) + wl = wl - 1; + + // wu is the next odd value > wl + wu = wl + 2; + + // Compute m.Refer to the tech note for derivation of this formula + mIdeal = (12 * sigma*sigma - n*wl*wl - 4 * n*wl - 3 * n) / (-4 * wl - 4); + m = (unsigned)round(mIdeal); + + assertMsg(!(m > n || m < 0), "calculation of m has failed"); + + // Compute actual sigma that will be achieved + sigmaActual = sqrt((m*wl*wl + (n - m)*wu*wu - n) / (T)12.0); + } +}; + + +template +struct ExBoxSizes +{ + static_assert(std::is_floating_point::value, "This class only works with floats."); + + T sigma, sigma_actual; + unsigned n, r; + T alpha, c, c1, c2; + + T r_f; + + // Special case for h == 1 + ExBoxSizes(T sigma, unsigned n) + : sigma(sigma), n(n) + { + T var = sigma*sigma; + r_f = 0.5*sqrt((12 * var) / n + 1) - T(0.5); + r = (unsigned)std::floor(r_f); + + alpha = (2 * r + 1) * ( (r*r + r - 3*var/n) / (6 * (var/n - (r+1)*(r+1))) ); + + c1 = alpha / (2*alpha + 2*r + 1); + c2 = (1 - alpha) / (2 * alpha + 2 * r + 1); + c = c1 + c2; + } + + ExBoxSizes(T sigma, unsigned d, T h) + : sigma(sigma), n(d) + { + T v = sigma*sigma; + r_f = sqrt((12 * v) / n + 1)/(2*h) - T(0.5); // (7) + r = (unsigned)std::floor(std::max(T(0), r_f)); + + alpha = (2 * r + 1) * (((r*r + r) - (v*3*h)/(d*h*h*h)) / (6 * ( v/(d*h*h) - (r+1)*(r+1)) )); // (8) (14) + c1 = alpha / (h*(2 * r + 2 * alpha + 1)); // (8) (13) + c2 = (1 - alpha) / (h*(2 * r + 2 * alpha + 1)); // (8) (13) + + c = c1 + c2; + + T lambda = h*(2*r + 1 + 2*alpha); // (8) + T variance_actual = (d*h*h*h) / (3 * lambda) * (2*r*r*r + 3*r*r + r + 6*alpha*(r+1)*(r+1)); // (14) + sigma_actual = sqrt(variance_actual); + } +}; diff --git a/math/boxkde/DataStructures.h b/math/boxkde/DataStructures.h new file mode 100644 index 0000000..64cb419 --- /dev/null +++ b/math/boxkde/DataStructures.h @@ -0,0 +1,115 @@ +#pragma once + +#include +#include +#include +#include +#include + +template +struct BoundingBox +{ + static_assert(std::is_arithmetic::value, "This class only works with floats or integers."); + + T MinX, MaxX, MinY, MaxY; + + BoundingBox(T MinX = std::numeric_limits::max(), + T MaxX = std::numeric_limits::lowest(), + T MinY = std::numeric_limits::max(), + T MaxY = std::numeric_limits::lowest()) + : MinX(MinX), MaxX(MaxX), MinY(MinY), MaxY(MaxY) + { } + + T width () const { return MaxX - MinX; } + T heigth() const { return MaxY - MinY; } + T area () const { return width()*heigth(); } + + bool isInside(T x, T y) const { return (x >= MinX && x <= MaxX) && (y >= MinY && y <= MaxY); } + + // Expands the size of the BB if the given values are extreme + void expand(T x, T y) + { + if (x < MinX) MinX = x; + if (x > MaxX) MaxX = x; + if (y < MinY) MinY = y; + if (y > MaxY) MaxY = y; + } + + // Enlarges the BB in both direction along an axis. + void inflate(T szX, T szY) + { + MinX -= szX; + MinY -= szY; + + MaxX += szX; + MaxY += szY; + } +}; + + +template +struct Point2D +{ + static_assert(std::is_arithmetic::value, "This class only works with floats and integers."); + + T X, Y; + + Point2D(T x = 0, T y = 0) + : X(x), Y(y) + { } +}; + +template +struct Size2D +{ + static_assert(std::is_arithmetic::value, "This class only works with floats and integers."); + + T sX, sY; + + + Size2D(T all) + : sX(all), sY(all) + { } + + Size2D(T x = 0, T y = 0) + : sX(x), sY(y) + { } +}; + + +#ifdef NDEBUG +#define assertCond(_EXPR_) (false) +#define assertThrow(_arg_) ((void)0) +#define assert_throw(...) ((void)0) +#define assertMsg(...) ((void)0) +#else +// Evaluates the expression. Ifdef NDEBUG returns always false + #define assertCond(_EXPR_) (!(_EXPR_)) +// Throws a excpetion with the argument as message by prepending the current file name and line number + #define assertThrow(_std_string_) assert_throw( (_std_string_), __FILE__, __LINE__) + inline void assert_throw(const std::string& message, const char* file, int line) + { + std::stringstream ss; + ss << file << ":" << line << ": " << message; + throw std::invalid_argument(ss.str()); + } + + #define assertMsg(_EXPR_, _MSG_) if (!(_EXPR_)) assertThrow(_MSG_) +#endif + +// ostream overloads +template +std::ostream& operator<<(std::ostream& os, const Point2D& pt) +{ + return os << "(" << pt.X << "; " << pt.Y << ")"; +} + +template +std::ostream& operator<<(std::ostream& os, const BoundingBox& bb) +{ + return os << "(X: " << bb.MinX << " - " << bb.MaxX << ";" + << " Y: " << bb.MinY << " - " << bb.MaxY << ")"; +} + + + diff --git a/math/boxkde/GausLib.h b/math/boxkde/GausLib.h new file mode 100644 index 0000000..460e3b9 --- /dev/null +++ b/math/boxkde/GausLib.h @@ -0,0 +1,34 @@ +// The following ifdef block is the standard way of creating macros which make exporting +// from a DLL simpler. All files within this DLL are compiled with the GAUSLIB_EXPORTS +// symbol defined on the command line. This symbol should not be defined on any project +// that uses this DLL. This way any other project whose source files include this file see +// GAUSLIB_API functions as being imported from a DLL, whereas this DLL sees symbols +// defined with this macro as being exported. + + typedef struct c_bbox { + double minX; + double maxX; + double minY; + double maxY; + double minZ; + double maxZ; + } c_bbox; + + +#define ALGO_BOX 0 +#define ALGO_EXBOX 1 +#define ALGO_BOX_SIMD 2 +#define ALGO_BOX_CL 3 + + void gausKde2D_simple(float* X, int sizeX, + float* weights, + float hx, float hy, + int nBinsX, int nBinsY, + c_bbox* boundingBox, + int algorithm, + double* out_maxValue, + double* out_maxPosX, double* out_maxPosY, + double* out_runTimeInNS, + float* out_density); + + \ No newline at end of file diff --git a/math/boxkde/Grid2D.h b/math/boxkde/Grid2D.h new file mode 100644 index 0000000..25bd4c9 --- /dev/null +++ b/math/boxkde/Grid2D.h @@ -0,0 +1,211 @@ +#pragma once + +#include + +#include "DataStructures.h" +#include "Image2D.h" + +template +struct Grid2D +{ + static_assert(std::is_floating_point::value, "Grid2D only supports float values"); + + BoundingBox bb; + size_t numBinsX, numBinsY; + T binSizeX, binSizeY; +private: + Image2D data; + +public: + + Grid2D(){ } //TODO: fast hack + + Grid2D(BoundingBox bb, size_t numBins) + : Grid2D(bb, numBins, numBins) + { + } + + Grid2D(BoundingBox bb, size_t numBinsX, size_t numBinsY) + : bb(bb), + numBinsX(numBinsX), + numBinsY(numBinsY), + binSizeX((bb.width()) / (numBinsX-1)), + binSizeY((bb.heigth()) / (numBinsY-1)), + data(numBinsX, numBinsY) + { + } + + Image2D& image() { return data; } + const Image2D& image() const { return data; } + + T& operator() (size_t x, size_t y) { return data(x, y); } + const T& operator() (size_t x, size_t y) const { return data(x, y); } + + void clear() { data.clear(); } + + void fill(const std::vector>& samples) + { + assertMsg(!samples.empty(), "Samples must be non-empty"); + + data.clear(); + + const T weight = T(1.0) / samples.size(); + for (const auto& pt : samples) + { + add(pt.X, pt.Y, weight); + } + } + + void fill(const std::vector>& 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"); + + data.clear(); + + for (size_t i = 0; i < samples.size(); i++) + { + add(samples[i].X, samples[i].Y, weights[i]); + } + } + + Point2D add(T x, T y, T w) + { + if (assertCond(bb.isInside(x,y))) + { + std::stringstream ss; + ss << "Point " << Point2D(x, y) << " is out of bounds. " << bb; + assertThrow(ss.str()); + } + + return add_simple_bin(x, y, w); + //return add_linear_bin(x, y, w); + } + + void add_linear(T x, T y, T w) + { + if (assertCond(bb.isInside(x, y))) + { + std::stringstream ss; + ss << "Point " << Point2D(x, y) << " is out of bounds. " << bb; + assertThrow(ss.str()); + } + + add_linear_bin(x, y, w); + } + + T fetch(T x, T y) const + { + size_t bin_x = (size_t)((x - bb.MinX) / binSizeX); + size_t bin_y = (size_t)((y - bb.MinY) / binSizeY); + + //if (bin_x == data.width) bin_x--; + //if (bin_y == data.height) bin_y--; + + return data(bin_x, bin_y); + } + + // Returns the summation of all bin values + T sum() const + { + return data.sum(); + } + + // Takes a point in input space and converts it to grid space coordinates + Point2D asGridSpace(T x, T y) const + { + size_t bin_x = (size_t)((x - bb.MinX) / binSizeX); + size_t bin_y = (size_t)((y - bb.MinY) / binSizeY); + + if (bin_x == data.width) bin_x--; + if (bin_y == data.height) bin_y--; + + return Point2D(bin_x, bin_y); + } + + // Takes a Size2D in input space and converts it to grid space coordiantes + Size2D asGridSpace(Size2D sz) const + { + return Size2D(sz.sX / binSizeX, sz.sY / binSizeY); + } + + // Takes a point in grid space and converts it to input space coordinates + Point2D asInputSpace(Point2D pt) const + { + return asInputSpace(pt.X, pt.Y); + } + + // Takes a point in grid space and converts it to input space coordinates + Point2D asInputSpace(size_t x, size_t y) const + { + return Point2D(x * binSizeX + bb.MinX + T(0.5)*binSizeX, + y * binSizeY + bb.MinY + T(0.5)*binSizeY); + } + + + T maximum(Point2D& pt) const + { + Point2D gridPt; + + T maxValue = image().maximum(gridPt); + pt = asInputSpace(gridPt); + return maxValue; + } + +private: + Point2D add_simple_bin(T x, T y, T w) + { + size_t bin_x = (size_t)((x - bb.MinX) / binSizeX); + size_t bin_y = (size_t)((y - bb.MinY) / binSizeY); + + if (bin_x == data.width) bin_x--; + if (bin_y == data.height) bin_y--; + + data(bin_x, bin_y) += w; + + return Point2D(bin_x, bin_y); + } + + void add_linear_bin(T x, T y, T w) + { + T xpos1 = (x - bb.MinX) / binSizeX; + T xpos2 = (y - bb.MinY) / binSizeY; + size_t ix1 = (size_t)floor(xpos1); + size_t ix2 = (size_t)floor(xpos2); + T fx1 = xpos1 - ix1; + T fx2 = xpos2 - ix2; + + const size_t ixmin1 = 0; + const size_t ixmax1 = numBinsX - 2; + const size_t ixmin2 = 0; + const size_t ixmax2 = numBinsY - 2; + + if ( ixmin1 <= ix1 && ixmin2 <= ix2 && ix2 <= ixmax2) + { + data(ix1, ix2) += w*(1 - fx1)*(1 - fx2); + data(ix1+1, ix2) += w*fx1*(1 - fx2); + data(ix1, ix2+1) += w*(1 - fx1)*fx2; + data(ix1 + 1, ix2 + 1) += w*fx1*fx2; + } + else if (ix1 == ixmax1 + 1 && ixmin2 <= ix2 && ix2 <= ixmax2) + { + // rechts + data(ix1, ix2) += w*(1 - fx1)*(1 - fx2); + data(ix1, ix2+1) += w*(1 - fx1)*fx2; + } + else if (ixmin1 <= ix1 && ix1 <= ixmax1 && ix2 == ixmax2 + 1) + { + // unten + data(ix1, ix2) += w*(1 - fx1)*(1 - fx2); + data(ix1+1, ix2) += w*fx1*(1 - fx2); + } + else if (ix1 == ixmax1 + 1 && ix2 == ixmax2 + 1) + { + // rechts-unten + data(ix1, ix2) += w*(1 - fx1)*(1 - fx2); + } + } +}; + +template struct Grid2D; +template struct Grid2D; diff --git a/math/boxkde/Image2D.h b/math/boxkde/Image2D.h new file mode 100644 index 0000000..714264d --- /dev/null +++ b/math/boxkde/Image2D.h @@ -0,0 +1,188 @@ +#pragma once + +#include +#include +#include +#include + +#include "DataStructures.h" + +enum struct LineDirection { X, Y }; + +template +struct ImageView2D +{ + static_assert(std::is_arithmetic::value, "Image only supports integers or floats"); + + // forward declaration + //enum struct LineDirection; + template struct LineView; + template struct ConstLineView; + + size_t width, height, size; +protected: + TValue* values; // contains image data row-wise + +public: + ImageView2D() + : width(0), height(0), size(0), values(nullptr) + { } + + ImageView2D(size_t width, size_t height) + : width(width), height(height), size(width*height), values(nullptr) + { } + + ImageView2D(size_t width, size_t height, TValue* data) + : width(width), height(height), size(width*height), values(data) + { } + + inline TValue& operator() (size_t x, size_t y) { return values[indexFromCoord(x, y)]; } + inline const TValue& operator() (size_t x, size_t y) const { return values[indexFromCoord(x, y)]; } + + 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; } + + inline size_t indexFromCoord(size_t x, size_t y) const + { + assertMsg(x < width && y < height, "(x,y) out of bounds"); + return y * width + x; + } + + Point2D coordFromIndex(size_t index) const + { + assertMsg(index < size, "Index out of bounds"); + return Point2D(index % width, index / width); + } + + Point2D maximum() const + { + size_t maxValueIndex = std::distance(val_begin(), std::max_element(val_begin(), val_end())); + return coordFromIndex(maxValueIndex); + } + + TValue maximum(Point2D& pt) const + { + size_t maxValueIndex = std::distance(val_begin(), std::max_element(val_begin(), val_end())); + pt = coordFromIndex(maxValueIndex); + return values[maxValueIndex]; + } + + void clear() + { + fill(TValue(0)); + } + + void fill(TValue value) + { + std::fill(val_begin(), val_end(), value); + } + + void assign(const ImageView2D& other) + { + assertMsg(size == other.size, "Other must be of the 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)); + } + + // Returns a transposed view of this image without copying any data. + ImageView2D transpose() const + { + return ImageView2D(height, width, values); + } + + // Returns the value at (x,y). Throws if out of bounds. + const TValue& at(size_t x, size_t y) const + { + return values[indexFromCoord(x, y)]; + } + + // Returns the value at (x,y) but falls back to default if out of bounds. + const TValue& get(size_t x, size_t y, TValue dflt = 0) const + { + if (x >= width || y >= height) + return dflt; + else + return values[indexFromCoord(x, y)]; + } + + std::vector> findLocalMaxima(int radiusX = 1, int radiusY = 1) const + { + std::vector> result; + findLocalMaxima(result, radiusX, radiusY); + return result; + } + + void findLocalMaxima(std::vector>& result, int radiusX = 1, int radiusY = 1) const + { + assertMsg(radiusX > 0, "Search radius must be greater than 0."); + + for (size_t y = 0; y < height; y++) + { + for (size_t x = 0; x < width; x++) + { + TValue val = at(x, y); + bool lclMax = true; + + for (int ry = -radiusY; ry <= radiusY; ry++) + { + for (int rx = -radiusX; rx <= radiusX; rx++) + { + TValue other = get(x + rx, y + ry); + if (!(rx == 0 && ry == 0) && val <= other) + { + lclMax = false; + } + } + } + + if (lclMax) + { + result.emplace_back(x, y); + } + } + } + } +}; + +template +struct Image2D : public ImageView2D +{ + static_assert(std::is_arithmetic::value, "Image only supports integers or floats"); + + std::vector values_vec; + + Image2D() + { + } + + Image2D(size_t width, size_t height) + : ImageView2D(width, height), values_vec(width * height) + { + this->values = values_vec.data(); + } + + Image2D(size_t width, size_t height, std::vector& data) + : ImageView2D(width, height), values_vec(data) + { + assertMsg(data.size() == width*height, "Sizes must be the same"); + this->values = values_vec.data(); + } + + + std::vector& data() { return values_vec; } + const std::vector& data() const { return values_vec; } +}; + + +template struct ImageView2D; +template struct ImageView2D; + +template struct Image2D; +template struct Image2D; diff --git a/math/boxkde/benchmark.h b/math/boxkde/benchmark.h new file mode 100644 index 0000000..0b8210f --- /dev/null +++ b/math/boxkde/benchmark.h @@ -0,0 +1,105 @@ +#pragma once +#include +#include +#include + +template +static void benchmark(std::string name, size_t count, F&& lambda) +{ + auto start = std::chrono::high_resolution_clock::now(); + + for (size_t i = 0; i < count; i++) + { + lambda(); + } + + auto duration = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); + long long durationAVG = duration.count() / count; + + std::cout << "Function " << name << " took avg. " << durationAVG << "us (" << durationAVG / 1000.0f << "ms) over " << count << " runs." << std::endl; +} + + +struct BenchResult +{ + const long long min, max; + const double mean, median; + + BenchResult() + : min(0), max(0), mean(0), median(0) + {} + + BenchResult(long long min, long long max, double mean, double median) + : min(min), max(max), mean(mean), median(median) + {} +}; + +template +static BenchResult benchmarkEx(std::string name, size_t count, F&& lambda) +{ + std::vector durations(count); + + for (size_t i = 0; i < count; i++) + { + auto start = std::chrono::high_resolution_clock::now(); + lambda(); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(stop - start); + durations[i] = duration.count(); + } + + long long min = *std::min_element(durations.begin(), durations.end()); + long long max = *std::max_element(durations.begin(), durations.end()); + double average = std::accumulate(durations.begin(), durations.end(), 0.0) / durations.size(); + + double median; + std::sort(durations.begin(), durations.end()); + + if (durations.size() % 2 == 0) + median = (double) (durations[durations.size() / 2 - 1] + durations[durations.size() / 2]) / 2; + else + median = (double) durations[durations.size() / 2]; + + std::cout << "Function " << name << " took avg. " << average << "ns (" << average / 1000.0f << "us) over " << count << " runs." << std::endl; + + return BenchResult(min, max, average, median); +} + +struct StopWatch +{ + typedef std::chrono::high_resolution_clock clock; + typedef clock::time_point time_point; + + time_point startTime; // Point in time when start() was called + time_point lapTime; // Point in time when operator() was called + +public: + StopWatch() + { + reset(); + } + + void reset() + { + startTime = clock::now(); + lapTime = startTime; + } + + std::chrono::microseconds stop() + { + auto duration = std::chrono::duration_cast(clock::now() - startTime); + + std::cout << "Total time: " << duration.count() << "us (" << duration.count() / 1000.0f << "ms)" << std::endl; + + return duration; + } + + void operator()(const std::string& str = "") + { + auto duration = std::chrono::duration_cast(clock::now() - lapTime); + + std::cout << str << (str.empty() ? "" : " ") << "took " << duration.count() << "us (" << duration.count() / 1000.0f << "ms)" << std::endl; + + lapTime = clock::now(); + } +}; \ No newline at end of file diff --git a/smc/filtering/ParticleFilterHistory.h b/smc/filtering/ParticleFilterHistory.h index 15cacb2..1cb2f74 100644 --- a/smc/filtering/ParticleFilterHistory.h +++ b/smc/filtering/ParticleFilterHistory.h @@ -80,10 +80,10 @@ namespace SMC { return particles; } - /** access to all non resample particles */ - const std::vector>& getNonResamplingParticles() { - return particlesNoResampling; - } + /** access to all non resample particles */ + const std::vector>& getNonResamplingParticles() { + return particlesNoResampling; + } /** initialize/re-start the particle filter */ void init() { @@ -91,11 +91,11 @@ namespace SMC { initializer->initialize(particles); } - void setAngle(const double angle){ - for(SMC::Particle& p : particles){ - p.state.walkState.heading = angle; - } - } + void setAngle(const double angle){ + for(SMC::Particle& p : particles){ + p.state.walkState.heading = angle; + } + } /** set the resampling method to use */ @@ -148,7 +148,7 @@ namespace SMC { Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); // perform the transition step - transition->transition(particles, control); + transition->transition(particles, control); // perform the evaluation step and calculate the sum of all particle weights const double weightSum = evaluation->evaluation(particles, observation); diff --git a/smc/smoothing/BackwardFilter.h b/smc/smoothing/BackwardFilter.h index 969c360..b83f548 100644 --- a/smc/smoothing/BackwardFilter.h +++ b/smc/smoothing/BackwardFilter.h @@ -6,6 +6,7 @@ #include #include "BackwardFilterTransition.h" +#include "ForwardFilterHistory.h" #include "../sampling/ParticleTrajectorieSampler.h" @@ -16,6 +17,7 @@ #include "../filtering/ParticleFilterEvaluation.h" #include "../filtering/ParticleFilterInitializer.h" + #include "../../Assertions.h" @@ -25,7 +27,19 @@ namespace SMC { class BackwardFilter { public: - virtual State update(std::vector> const& forwardParticles) = 0; + + /** + * @brief updating the backward filter (smoothing) using the informations made in the forward step (filtering) + * @param forwardFilter is a given ForwardFilterHistory containing all Filtering steps made. + * @return return the last smoothed estimation ordered backwards in time direction. + */ + virtual State update(ForwardFilterHistory& forwardFilter, int lag = 0) = 0; + + /** + * @brief getEstimations + * @return vector with all estimations running backward in time. T -> 0 + */ + virtual std::vector getEstimations() = 0; /** access to all backward / smoothed particles */ virtual const std::vector>>& getbackwardParticles() = 0; @@ -48,9 +62,6 @@ namespace SMC { /** get the used transition method */ virtual BackwardFilterTransition* getTransition() = 0; - /** reset */ - virtual void reset() {}; - }; } diff --git a/smc/smoothing/BackwardSimulation.h b/smc/smoothing/BackwardSimulation.h index 5f3729b..271790f 100644 --- a/smc/smoothing/BackwardSimulation.h +++ b/smc/smoothing/BackwardSimulation.h @@ -46,8 +46,8 @@ namespace SMC { /** all smoothed particles T -> 1*/ std::vector>> backwardParticles; - /** container for particles */ - std::vector> smoothedParticles; + /** all estimations calculated */ + std::vector estimatedStates; /** the estimation function to use */ std::unique_ptr> estimation; @@ -77,26 +77,13 @@ namespace SMC { BackwardSimulation(int numRealizations) { this->numRealizations = numRealizations; backwardParticles.reserve(numRealizations); - smoothedParticles.reserve(numRealizations); firstFunctionCall = true; } /** dtor */ ~BackwardSimulation() { - ; - } - - /** reset **/ - void reset(){ - this->numRealizations = numRealizations; - backwardParticles.clear(); - backwardParticles.reserve(numRealizations); - - smoothedParticles.clear(); - smoothedParticles.reserve(numRealizations); - - firstFunctionCall = true; + estimatedStates.clear(); } /** access to all backward / smoothed particles */ @@ -140,118 +127,154 @@ namespace SMC { } /** - * perform update: transition -> correction -> approximation - * gets the weighted sample set of a standard condensation - * particle filter in REVERSED order! + * @brief update + * @param forwardHistory + * @return */ - State update(std::vector> const& forwardParticles) { + State update(ForwardFilterHistory& forwardHistory, int lag = 666) { // sanity checks (if enabled) Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); - //storage for single trajectories / smoothed particles - smoothedParticles.clear(); + //init for backward filtering + std::vector> smoothedParticles; + smoothedParticles.reserve(numRealizations); + firstFunctionCall = true; - // Choose \tilde x_T = x^(i)_T with probability w^(i)_T - // Therefore sample independently from the categorical distribution of weights. - if(firstFunctionCall){ - - smoothedParticles = sampler->drawTrajectorie(forwardParticles, numRealizations); - - firstFunctionCall = false; - backwardParticles.push_back(smoothedParticles); - - const State es = estimation->estimate(smoothedParticles); - return es; + if(lag == 666){ + lag = forwardHistory.size() - 1; } - // compute weights using the transition model - // transitionWeigths[numRealizations][numParticles] - std::vector> transitionWeights = transition->transition(forwardParticles, backwardParticles.back()); + //check if we have enough data for lag + if(forwardHistory.size() <= lag){ + lag = forwardHistory.size() - 1; + } - //get the next trajectorie for a realisation - for(int j = 0; j < numRealizations; ++j){ + //iterate through all forward filtering steps + for(int i = 0; i <= lag; ++i){ + std::vector> forwardParticles = forwardHistory.getParticleSet(i); - //vector for the current smoothedWeights at time t - std::vector> smoothedWeights; - smoothedWeights.resize(forwardParticles.size()); - smoothedWeights = forwardParticles; + //storage for single trajectories / smoothed particles + smoothedParticles.clear(); - //check if all transitionWeights are zero - double weightSumTransition = std::accumulate(transitionWeights[j].begin(), transitionWeights[j].end(), 0.0); - Assert::isNot0(weightSumTransition, "all transition weights for smoothing are zero"); + // Choose \tilde x_T = x^(i)_T with probability w^(i)_T + // Therefore sample independently from the categorical distribution of weights. + if(firstFunctionCall){ - int i = 0; - for (auto& w : transitionWeights.at(j)) { + smoothedParticles = sampler->drawTrajectorie(forwardParticles, numRealizations); - // multiply the weight of the particles at time t and normalize - smoothedWeights.at(i).weight = (smoothedWeights.at(i).weight * w); - if(smoothedWeights.at(i).weight != smoothedWeights.at(i).weight) {throw "detected NaN";} + firstFunctionCall = false; + backwardParticles.push_back(smoothedParticles); - // iter - ++i; + State est = estimation->estimate(smoothedParticles); + estimatedStates.push_back(est); } - //get the sum of all weights - auto lambda = [](double current, const Particle& a){return current + a.weight; }; - double weightSumSmoothed = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); + // compute weights using the transition model + // transitionWeigths[numRealizations][numParticles] + std::vector> transitionWeights = transition->transition(forwardParticles, backwardParticles.back()); - //normalize the weights - if(weightSumSmoothed != 0.0){ - for (int i = 0; i < smoothedWeights.size(); ++i){ - smoothedWeights.at(i).weight /= weightSumSmoothed; + //get the next trajectorie for a realisation + for(int j = 0; j < numRealizations; ++j){ + + //vector for the current smoothedWeights at time t + std::vector> smoothedWeights; + smoothedWeights.resize(forwardParticles.size()); + smoothedWeights = forwardParticles; + + //check if all transitionWeights are zero + double weightSumTransition = std::accumulate(transitionWeights[j].begin(), transitionWeights[j].end(), 0.0); + Assert::isNot0(weightSumTransition, "all transition weights for smoothing are zero"); + + int k = 0; + for (auto& w : transitionWeights.at(j)) { + + // multiply the weight of the particles at time t and normalize + smoothedWeights.at(k).weight = (smoothedWeights.at(k).weight * w); + if(smoothedWeights.at(k).weight != smoothedWeights.at(k).weight) {throw "detected NaN";} + + // iter + ++k; } - //check if normalization worked - double normWeightSum = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); - Assert::isNear(normWeightSum, 1.0, 0.001, "Smoothed weights do not sum to 1"); + //get the sum of all weights + auto lambda = [](double current, const Particle& a){return current + a.weight; }; + double weightSumSmoothed = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); + + //normalize the weights + if(weightSumSmoothed != 0.0){ + for (int l = 0; l < smoothedWeights.size(); ++l){ + smoothedWeights.at(l).weight /= weightSumSmoothed; + } + + //check if normalization worked + double normWeightSum = std::accumulate(smoothedWeights.begin(), smoothedWeights.end(), 0.0, lambda); + Assert::isNear(normWeightSum, 1.0, 0.001, "Smoothed weights do not sum to 1"); + } + + + //draw the next trajectorie at time t for a realization and save them + smoothedParticles.push_back(sampler->drawSingleParticle(smoothedWeights)); + + //throw if weight of smoothedParticle is zero + //in practice this is possible, if a particle is completely separated from the rest and is therefore + //weighted zero or very very low. + Assert::isNot0(smoothedParticles.back().weight, "smoothed particle has zero weight"); } - //draw the next trajectorie at time t for a realization and save them - smoothedParticles.push_back(sampler->drawSingleParticle(smoothedWeights)); + if(resampler) + { + //TODO - does this even make sense? + Assert::doThrow("Warning - Resampling is not yet implemented!"); + } + + // push_back the smoothedParticles + backwardParticles.push_back(smoothedParticles); + + // estimate the current state + if(lag == (forwardHistory.size() - 1) ){ //fixed lag + State est = estimation->estimate(smoothedParticles); + estimatedStates.push_back(est); + } + else if (i == lag) { //fixed interval + State est = estimation->estimate(smoothedParticles); + estimatedStates.push_back(est); + } - //throw if weight of smoothedParticle is zero - //in practice this is possible, if a particle is completely separated from the rest and is therefore - //weighted zero or very very low. - Assert::isNot0(smoothedParticles.back().weight, "smoothed particle has zero weight"); } + //return the calculated estimations + // TODO: Wir interessieren uns beim fixed-lag smoothing immer nur für die letzte estimation und den letzten satz gesmoothet particle da wir ja weiter vorwärts in der Zeit gehen + // und pro zeitschritt ein neues particle set hinzu kommt. also wenn lag = 3, dann smoothen wir t - 3 und sollten auch nur die estimation von t-3 und das particle set von t-3 abspeichern + // + // beim interval smoothing dagegen interessieren uns alle, da ja keine neuen future informationen kommen und wir einfach sequentiell zurück in die zeit wandern. + // + // lösungsvorschlag. + // - observer-pattern? immer wenn eine neue estimation und ein neues particle set kommt, sende. (wird nix bringen, da keine unterschiedlichen threads?) + // - wie vorher machen, also pro update eine estimation aber jedem update einfach alle observations und alle controls mitgeben?!?!? + // - ich gebe zusätzlich den lag mit an. dann kann die forwardfilterhistory auch ständig alles halten. dann gibt es halt keine ständigen updates, sondern man muss die eine + // berechnung abwarten. eventl. eine art ladebalken hinzufügen. (30 von 120 timestamps done) (ich glaube das ist die beste blackboxigste version) man kann den lag natürlich auch beim + // init des backwardsimulation objects mit übergeben. ABER: damit könntem an kein dynamic-lag smoothing mehr machen. also lieber variable lassen :). durch den lag wissen wir einfach was wir + // genau in estimatedStates und backwardParticles speichern müssen ohne über das problem oben zu stoßen. haben halt keine ständigen updates. observer-pattern hier nur bei mehrere threads, + // das wäre jetzt aber overkill und deshalb einfach ladebalken :):):):) + // - oder man macht einfach zwei update funktionen mit den beiden möglichkeiten. halte ich aber für nen dummen kompromiss. ) - if(resampler) - { + // würde es sinn machen, die estimations auch mit zu speichern? - //TODO - does this even make sense? - std::cout << "Warning - Resampling is not yet implemented!" << std::endl; -// //resampling if necessery -// double sum = 0.0; -// double weightSum = std::accumulate(smoothedParticles.begin().weight, smoothedParticles.end().weight, 0.0); -// for (auto& p : smoothedParticles) { -// p.weight /= weightSum; -// sum += (p.weight * p.weight); -// } - -// const double neff = 1.0/sum; -// if (neff != neff) {throw "detected NaN";} - -// // if the number of efficient particles is too low, perform resampling -// if (neff < smoothedParticles.size() * nEffThresholdPercent) { resampler->resample(smoothedParticles); } - } - - // push_back the smoothedParticles - backwardParticles.push_back(smoothedParticles); - - // estimate the current state - const State est = estimation->estimate(smoothedParticles); - - // done - return est; + return estimatedStates.back(); } + std::vector getEstimations(){ + return estimatedStates; + } + + }; + } diff --git a/smc/smoothing/FastKDESmoothing.h b/smc/smoothing/FastKDESmoothing.h new file mode 100644 index 0000000..0777c91 --- /dev/null +++ b/smc/smoothing/FastKDESmoothing.h @@ -0,0 +1,271 @@ +#ifndef FASTKDESMOOTHING_H +#define FASTKDESMOOTHING_H + +#include +#include +#include + +#include "BackwardFilterTransition.h" +#include "BackwardFilter.h" + +#include "../Particle.h" + +#include "../../floorplan/v2/FloorplanHelper.h"; +#include "../../grid/Grid.h"; + +#include "../filtering/resampling/ParticleFilterResampling.h" +#include "../filtering/estimation/ParticleFilterEstimation.h" +#include "../filtering/ParticleFilterEvaluation.h" +#include "../filtering/ParticleFilterInitializer.h" +#include "../filtering/ParticleFilterTransition.h" + +#include "../sampling/ParticleTrajectorieSampler.h" + +#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 "../../Assertions.h" +namespace SMC { + + template + class FastKDESmoothing : public BackwardFilter{ + + private: + + /** all smoothed particles T -> 1*/ + std::vector>> backwardParticles; + + /** all estimations calculated */ + std::vector estimatedStates; + + /** the estimation function to use */ + std::unique_ptr> estimation; + + /** the transition function to use */ + std::unique_ptr> transition; + + /** the resampler to use */ + std::unique_ptr> resampler; + + /** the sampler for drawing trajectories */ + std::unique_ptr> sampler; + + /** the percentage-of-efficient-particles-threshold for resampling */ + double nEffThresholdPercent = 0.25; + + /** number of realizations to be calculated */ + int numParticles; + + /** is update called the first time? */ + bool firstFunctionCall; + + /** boundingBox for the boxKDE */ + BoundingBox bb; + + /** histogram/grid holding the particles*/ + Grid2D grid; + + /** bandwith for KDE */ + Point2 bandwith; + + public: + + /** ctor */ + FastKDESmoothing(int numParticles, const Floorplan::IndoorMap* map, const int gridsize_cm, const Point2 bandwith) { + this->numParticles = numParticles; + backwardParticles.reserve(numParticles); + firstFunctionCall = true; + + const Point3 maxBB = FloorplanHelper::getBBox(map).getMax(); + const Point3 minBB = FloorplanHelper::getBBox(map).getMin(); + bb = BoundingBox(minBB.x, maxBB.x, minBB.y, maxBB.y); + + // Create histogram + size_t nBinsX = static_cast((maxBB.x - minBB.x) / gridsize_cm); + size_t nBinsY = static_cast((maxBB.y - minBB.y) / gridsize_cm); + grid = Grid2D(bb, nBinsX, nBinsY); + + this->bandwith = bandwith; + } + + /** dtor */ + ~FastKDESmoothing() { + backwardParticles.clear(); + estimatedStates.clear(); + } + + /** access to all backward / smoothed particles */ + const std::vector>>& getbackwardParticles() { + return backwardParticles; + } + + /** set the estimation method to use */ + void setEstimation(std::unique_ptr> estimation) { + Assert::isNotNull(estimation, "setEstimation() MUST not be called with a nullptr!"); + this->estimation = std::move(estimation); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::isNotNull(transition, "setTransition() MUST not be called with a nullptr!"); + this->transition = std::move(transition); + } + + /** set the transition method to use */ + void setTransition(std::unique_ptr> transition) { + Assert::doThrow("Do not use a backward transition for fast smoothing! Forward Transition"); + //TODO: two times setTransition is not the best solution + } + /** set the resampling method to use */ + void setResampling(std::unique_ptr> resampler) { + Assert::isNotNull(resampler, "setResampling() MUST not be called with a nullptr!"); + this->resampler = std::move(resampler); + } + + /** set the sampler method to use */ + void setSampler(std::unique_ptr> sampler){ + Assert::isNotNull(sampler, "setSampler() MUST not be called with a nullptr!"); + this->sampler = std::move(sampler); + } + + + /** set the resampling threshold as the percentage of efficient particles */ + void setNEffThreshold(const double thresholdPercent) { + this->nEffThresholdPercent = thresholdPercent; + } + + /** get the used transition method */ + BackwardFilterTransition* getTransition() { + return nullptr; + } + + /** + * @brief update + * @param forwardHistory + * @return + */ + State update(ForwardFilterHistory& forwardHistory, int lag = 666) { + + // sanity checks (if enabled) + Assert::isNotNull(transition, "transition MUST not be null! call setTransition() first!"); + Assert::isNotNull(estimation, "estimation MUST not be null! call setEstimation() first!"); + + //init for backward filtering + std::vector> smoothedParticles; + smoothedParticles.reserve(numParticles); + firstFunctionCall = true; + + // if no lag is given, we have a fixed interval smoothing + if(lag == 666){ + lag = forwardHistory.size() - 1; + } + + //check if we have enough data for lag + if(forwardHistory.size() <= lag){ + lag = forwardHistory.size() - 1; + } + + //iterate through all forward filtering steps + for(int i = 0; i <= lag; ++i){ + std::vector> forwardParticlesForTransition_t1 = forwardHistory.getParticleSet(i); + + //storage for single trajectories / smoothed particles + smoothedParticles.clear(); + + // Choose \tilde x_T = x^(i)_T with probability w^(i)_T + // Therefore sample independently from the categorical distribution of weights. + if(firstFunctionCall){ + + smoothedParticles = sampler->drawTrajectorie(forwardParticlesForTransition_t1, numParticles); + + firstFunctionCall = false; + backwardParticles.push_back(smoothedParticles); + + State est = estimation->estimate(smoothedParticles); + estimatedStates.push_back(est); + return est; + } + + // transition p(q_t+1* | q_t): so we are performing again a forward transition step. + // we are doing this to track single particles between two timesteps! normaly, resampling would destroy + // any identifier given to particles. + // Node: at this point we can integrate future observation and control information for better smoothing + Control controls = forwardHistory.getControl(i-1); + Observation obs = forwardHistory.getObservation(i-1); + transition->transition(forwardParticlesForTransition_t1, &controls); + + // KDE auf q_t+1 Samples = p(q_t+1 | o_1:T) - Smoothed samples from the future + grid.clear(); + for (Particle p : backwardParticles.back()) + { + grid.add(p.state.position.x_cm, p.state.position.y_cm, p.weight); + } + + int nFilt = 3; + float sigmaX = bandwith.x / grid.binSizeX; + float sigmaY = bandwith.y / grid.binSizeY; + BoxGaus boxGaus; + boxGaus.approxGaus(grid.image(), sigmaX, sigmaY, nFilt); + + // Apply Position from Samples from q_t+1* into KDE of p(q_t+1 | o_1:T) to get p(q_t+1* | o_1:T) + // Calculate new weight w(q_(t|T)) = w(q_t) * p(q_t+1* | o_1:T) * p(q_t+1* | q_t) * normalisation + smoothedParticles = forwardHistory.getParticleSet(i); + for(Particle p : smoothedParticles){ + p.weight = p.weight * grid.fetch(p.state.position.x_cm, p.state.position.y_cm); + Assert::isNot0(p.weight, "smoothed particle has zero weight"); + } + + //normalization + auto lambda = [](double current, const Particle& a){return current + a.weight; }; + double weightSumSmoothed = std::accumulate(smoothedParticles.begin(), smoothedParticles.end(), 0.0, lambda); + + if(weightSumSmoothed != 0.0){ + + for (Particle p : smoothedParticles){ + p.weight /= weightSumSmoothed; + } + + //check if normalization worked + double normWeightSum = std::accumulate(smoothedParticles.begin(), smoothedParticles.end(), 0.0, lambda); + Assert::isNear(normWeightSum, 1.0, 0.001, "Smoothed weights do not sum to 1"); + } else { + Assert::doThrow("Weight Sum of smoothed particles is zero!"); + } + + + if(resampler) + { + //TODO - does this even make sense? + Assert::doThrow("Warning - Resampling is not yet implemented!"); + } + + // push_back the smoothedParticles + backwardParticles.push_back(smoothedParticles); + + // estimate the current state + if(lag == (forwardHistory.size() - 1) ){ //fixed interval + State est = estimation->estimate(smoothedParticles); + estimatedStates.push_back(est); + } + else if (i == lag) { //fixed lag + State est = estimation->estimate(smoothedParticles); + estimatedStates.push_back(est); + } + + } + return estimatedStates.back(); + + } + + std::vector getEstimations(){ + return estimatedStates; + } + + + }; + +} +#endif // FASTKDESMOOTHING_H diff --git a/smc/smoothing/ForwardFilterHistory.h b/smc/smoothing/ForwardFilterHistory.h index b8841c7..66ef0b5 100644 --- a/smc/smoothing/ForwardFilterHistory.h +++ b/smc/smoothing/ForwardFilterHistory.h @@ -32,13 +32,11 @@ namespace SMC { //empty ctor } - void add(Timestamp time, std::vector>> set, Control control, Observation obs){ + void add(Timestamp time, std::vector> set, Control control, Observation obs){ - // Is empty? Null? etc. - Assert::isNotNull(time, "Timestamp is Null"); - Assert::isNotNull(set, "Particle Set is Null"); - Assert::isNotNull(control, "Control is Null"); - Assert::isNotNull(obs, "Observation is Null"); + // Is empty? Null? 0?`etc. + Assert::isNot0(time.ms(), "Timestamp is 0"); + if(set.empty()){Assert::doThrow("Particle Set is Empty");} timestamps.push_back(time); particleSets.push_back(set); @@ -64,39 +62,39 @@ namespace SMC { * @brief Return the particles from [latestFilterUpdate - @param idx] * @return returns vector of particles. note: c11 makes a std::move here */ - std::vector> getParticleSet(idx = 0){ - return particleSets.at(particleSets.end() - idx); + std::vector> getParticleSet(int idx = 0){ + return particleSets.at(particleSets.size() - 1 - idx); } /** * @brief getControl from [latestFilterUpdate - @param idx] * @return const controls object */ - const Control getControl(idx = 0){ - return controls.at(controls.end() - idx); + const Control getControl(int idx = 0){ + return controls.at(controls.size() - 1 - idx); } /** * @brief getObservationf rom [latestFilterUpdate - @param idx] * @return const obervations object */ - const Observation getObservation (idx = 0){ - return observations.at(observations.end() - idx); + const Observation getObservation(int idx = 0){ + return observations.at(observations.size() - 1 - idx); } /** * @brief Return the timestamp from [latestFilterUpdate - @param idx] * @return returns a Timstampf object */ - std::vector> getTimestamp(idx = 0){ - return timestamps.at(particleSets.end() - idx); + const Timestamp getTimestamp(int idx = 0){ + return timestamps.at(timestamps.size() - 1 - idx); } /** - * @brief getLatestFilterUpdateNum + * @brief size * @return num of particleSets size */ - const int getLatestFilterUpdateNum(){ + int size(){ return particleSets.size(); } @@ -133,6 +131,14 @@ namespace SMC { return timestamps.back(); } + /** + * @brief getFirstimestamp + * @return const Timestamp object + */ + const Timestamp getFirstTimestamp(){ + return timestamps.front(); + } + };