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

ref #39 smoothing is refactored

Browse Source 
KDE smoothing algorithmisch mal geschrieben, jetzt noch testen
This commit is contained in:
toni
2017-12-06 17:37:14 +01:00
parent 95a5c8f34f
commit 74981c6a45
14 changed files with 1303 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
smc/smoothing/BackwardFilter.h
View File

@@ -6,6 +6,7 @@
#include <memory>
#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<Particle<State>> 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<State, Control, Observation>& forwardFilter, int lag = 0) = 0;
/**
* @brief getEstimations
* @return vector with all estimations running backward in time. T -> 0
*/
virtual std::vector<State> getEstimations() = 0;
/** access to all backward / smoothed particles */
virtual const std::vector<std::vector<Particle<State>>>& getbackwardParticles() = 0;
@@ -48,9 +62,6 @@ namespace SMC {
/** get the used transition method */
virtual BackwardFilterTransition<State, Control>* getTransition() = 0;
/** reset */
virtual void reset() {};
};
}

213
smc/smoothing/BackwardSimulation.h
View File

@@ -46,8 +46,8 @@ namespace SMC {
/** all smoothed particles T -> 1*/
std::vector<std::vector<Particle<State>>> backwardParticles;
/** container for particles */
std::vector<Particle<State>> smoothedParticles;
/** all estimations calculated */
std::vector<State> estimatedStates;
/** the estimation function to use */
std::unique_ptr<ParticleFilterEstimation<State>> 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<Particle<State>> const& forwardParticles) {
State update(ForwardFilterHistory<State, Control, Observation>& 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<Particle<State>> 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<std::vector<double>> 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<Particle<State>> forwardParticles = forwardHistory.getParticleSet(i);
//vector for the current smoothedWeights at time t
std::vector<Particle<State>> 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<State>& 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<std::vector<double>> 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<Particle<State>> 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<State>& 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<State> getEstimations(){
return estimatedStates;
}
};
}

271
smc/smoothing/FastKDESmoothing.h Normal file
View File

@@ -0,0 +1,271 @@
#ifndef FASTKDESMOOTHING_H
#define FASTKDESMOOTHING_H
#include <vector>
#include <memory>
#include <algorithm>
#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 <typename State, typename Control, typename Observation>
class FastKDESmoothing : public BackwardFilter<State, Control, Observation>{
private:
/** all smoothed particles T -> 1*/
std::vector<std::vector<Particle<State>>> backwardParticles;
/** all estimations calculated */
std::vector<State> estimatedStates;
/** the estimation function to use */
std::unique_ptr<ParticleFilterEstimation<State>> estimation;
/** the transition function to use */
std::unique_ptr<ParticleFilterTransition<State, Control>> transition;
/** the resampler to use */
std::unique_ptr<ParticleFilterResampling<State>> resampler;
/** the sampler for drawing trajectories */
std::unique_ptr<ParticleTrajectorieSampler<State>> 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<float> bb;
/** histogram/grid holding the particles*/
Grid2D<float> 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<float>(minBB.x, maxBB.x, minBB.y, maxBB.y);
// Create histogram
size_t nBinsX = static_cast<size_t>((maxBB.x - minBB.x) / gridsize_cm);
size_t nBinsY = static_cast<size_t>((maxBB.y - minBB.y) / gridsize_cm);
grid = Grid2D<float>(bb, nBinsX, nBinsY);
this->bandwith = bandwith;
}
/** dtor */
~FastKDESmoothing() {
backwardParticles.clear();
estimatedStates.clear();
}
/** access to all backward / smoothed particles */
const std::vector<std::vector<Particle<State>>>& getbackwardParticles() {
return backwardParticles;
}
/** set the estimation method to use */
void setEstimation(std::unique_ptr<ParticleFilterEstimation<State>> 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<ParticleFilterTransition<State, Control>> 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<BackwardFilterTransition<State, Control>> 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<ParticleFilterResampling<State>> 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<ParticleTrajectorieSampler<State>> 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<State, Control>* getTransition() {
return nullptr;
}
/**
* @brief update
* @param forwardHistory
* @return
*/
State update(ForwardFilterHistory<State, Control, Observation>& 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<Particle<State>> 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<Particle<State>> 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<State> 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<float> 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<State> 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<State>& a){return current + a.weight; };
double weightSumSmoothed = std::accumulate(smoothedParticles.begin(), smoothedParticles.end(), 0.0, lambda);
if(weightSumSmoothed != 0.0){
for (Particle<State> 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<State> getEstimations(){
return estimatedStates;
}
};
}
#endif // FASTKDESMOOTHING_H

38
smc/smoothing/ForwardFilterHistory.h
View File

@@ -32,13 +32,11 @@ namespace SMC {
//empty ctor
}
void add(Timestamp time, std::vector<std::vector<Particle<State>>> set, Control control, Observation obs){
void add(Timestamp time, std::vector<Particle<State>> 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<Particle<State>> getParticleSet(idx = 0){
return particleSets.at(particleSets.end() - idx);
std::vector<Particle<State>> 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<Particle<State>> 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();
}
};