190 lines
7.0 KiB
C++
190 lines
7.0 KiB
C++
#ifndef MYSMOOTHINGTRANSITION_H
|
|
#define MYSMOOTHINGTRANSITION_H
|
|
|
|
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
|
|
#include <KLib/math/filter/smoothing/BackwardFilterTransition.h>
|
|
#include <KLib/math/distribution/Normal.h>
|
|
#include <KLib/math/distribution/Uniform.h>
|
|
|
|
#include <Indoor/nav/dijkstra/Dijkstra.h>
|
|
#include <Indoor/nav/a-star/AStar.h>
|
|
#include <Indoor/grid/Grid.h>
|
|
|
|
#include "../MyState.h"
|
|
#include "../MyControl.h"
|
|
|
|
#include "../../DijkstraMapper.h"
|
|
|
|
#include "../../toni/barometric.h"
|
|
#include <map>
|
|
#include <omp.h>
|
|
|
|
static double smoothing_walk_mu = 0.7;
|
|
static double smoothing_walk_sigma = 0.5;
|
|
static double smoothing_heading_sigma = 15.0;
|
|
static double smoothing_baro_sigma = 0.2;
|
|
|
|
typedef std::pair<const MyGridNode*, const MyGridNode*> my_key_type;
|
|
|
|
class MySmoothingTransition : public K::BackwardFilterTransition<MyState> {
|
|
|
|
private:
|
|
|
|
/** the created grid to draw transitions from */
|
|
Grid<MyGridNode>* grid;
|
|
|
|
/** a simple normal distribution */
|
|
K::NormalDistribution distWalk;
|
|
|
|
|
|
public:
|
|
|
|
/**
|
|
* ctor
|
|
* @param choice the choice to use for randomly drawing nodes
|
|
* @param fp the underlying floorplan
|
|
*/
|
|
MySmoothingTransition(Grid<MyGridNode>* grid) :
|
|
grid(grid), distWalk(smoothing_walk_mu, smoothing_walk_sigma) {
|
|
distWalk.setSeed(4321);
|
|
}
|
|
|
|
public:
|
|
|
|
uint64_t ts = 0;
|
|
uint64_t deltaMS = 0;
|
|
|
|
/** set the current time in millisconds */
|
|
void setCurrentTime(const uint64_t ts) {
|
|
if (this->ts == 0) {
|
|
this->ts = ts;
|
|
deltaMS = 0;
|
|
} else {
|
|
deltaMS = this->ts - ts;
|
|
this->ts = ts;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* smoothing transition starting at T with t, t-1,...0
|
|
* @param particles_new p_t (Forward Filter)
|
|
* @param particles_old p_t+1 (Smoothed Particles from Step before)
|
|
*/
|
|
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
|
|
std::vector<K::Particle<MyState>>const& particles_old ) override {
|
|
|
|
|
|
// calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
|
|
// this means, predict all possible states q_t+1 with all passible states q_t
|
|
// e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
|
|
// p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
|
|
std::vector<std::vector<double>> predictionProbabilities;
|
|
std::map<my_key_type, double> shortestPathMap;
|
|
AStar<MyGridNode> aStar;
|
|
DijkstraMapper dm(*grid);
|
|
|
|
omp_set_dynamic(0); // Explicitly disable dynamic teams
|
|
omp_set_num_threads(7);
|
|
#pragma omp parallel for shared(predictionProbabilities)
|
|
for (int i = 0; i < particles_old.size(); ++i) {
|
|
|
|
std::vector<double> innerVector;
|
|
auto p1 = &particles_old[i];
|
|
const MyGridNode* dst = grid->getNodePtrFor(GridPoint(p1->state.pCur.x, p1->state.pCur.y, p1->state.pCur.z));
|
|
|
|
for(int j = 0; j < particles_new.size(); ++j){
|
|
|
|
auto p2 = &particles_new[j];
|
|
|
|
// find the node (square) the particle is within
|
|
// just to be safe, we round z to the nearest floor
|
|
|
|
//TODO:: Nullptr check! sometimes src/dst can be nullptr
|
|
//const Node3* dst = graph->getNearestNode(p1->state.x_cm, p1->state.y_cm, std::round(p1->state.z_nr));
|
|
//const Node3* src = graph->getNearestNode(p2->state.x_cm, p2->state.y_cm, std::round(p2->state.z_nr));
|
|
|
|
const MyGridNode* src = grid->getNodePtrFor(GridPoint(p2->state.pCur.x, p2->state.pCur.y, p2->state.pCur.z));
|
|
|
|
// Dijkstra<MyGridNode> dijkstra;
|
|
// dijkstra.build(src, dst, DijkstraMapper(*grid));
|
|
|
|
// double distDijkstra_m = dijkstra.getNode(*src)->cumWeight;
|
|
|
|
double distDijkstra_m = 0;
|
|
//std::vector<const MyGridNode*> shortestPath;
|
|
|
|
// check if this shortestPath was already calculated
|
|
std::map<my_key_type, double>::iterator it;
|
|
it = shortestPathMap.find(my_key_type(dst, src));
|
|
if(it != shortestPathMap.end()){
|
|
distDijkstra_m = it->second;
|
|
}
|
|
else{
|
|
|
|
//Dijkstra/A* for shortest path
|
|
//shortestPath = aStar.get(src, dst, dm);
|
|
|
|
distDijkstra_m = aStar.get(src, dst, dm);
|
|
|
|
//get distance walked and getProb using the walking model
|
|
// for(int i = 0; i < shortestPath.size() - 1; ++i){
|
|
// distDijkstra_m += dm.getWeightBetween(*shortestPath[i], *shortestPath[i+1]);
|
|
// }
|
|
|
|
if(distDijkstra_m != distDijkstra_m) {throw "detected NaN";}
|
|
|
|
//save distance and nodes in lookup map
|
|
#pragma omp critical
|
|
shortestPathMap.insert(std::make_pair(my_key_type(dst, src), distDijkstra_m));
|
|
}
|
|
|
|
const double distProb = distWalk.getProbability(distDijkstra_m * 0.01);
|
|
|
|
//getProb using the angle(heading) between src and dst
|
|
// double angle = 0.0;
|
|
// if(!(p2->state.pCur.x == p1->state.pCur.x) && !(p2->state.pCur.y == p1->state.pCur.y)){
|
|
// angle = Angle::getDEG_360(p2->state.pCur.x, p2->state.pCur.y, p1->state.pCur.x, p1->state.pCur.y);
|
|
// }
|
|
// const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
|
|
|
|
// is the heading change similiar to the measurement?
|
|
double p2AngleDeg = p2->state.walkState.heading.getRAD() * 180/3.14159265359;
|
|
double p1AngleDeg = p1->state.walkState.heading.getRAD() * 180/3.14159265359;
|
|
|
|
double diffDeg = p2AngleDeg - p1AngleDeg;
|
|
const double headingProb = K::NormalDistribution::getProbability(p1->state.angularHeadingChange, smoothing_heading_sigma, diffDeg);
|
|
|
|
//assert(headingProb != 0.0);
|
|
//assert(distProb != 0.0);
|
|
|
|
//check how near we are to the measurement
|
|
double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
|
|
|
|
//combine the probabilities
|
|
double prob = distProb * floorProb * headingProb;
|
|
innerVector.push_back(prob);
|
|
|
|
//if(distance_m != distance_m) {throw "detected NaN";}
|
|
//if(distProb != distProb) {throw "detected NaN";}
|
|
//if(angle != angle) {throw "detected NaN";}
|
|
if(headingProb != headingProb) {throw "detected NaN";}
|
|
if(floorProb != floorProb) {throw "detected NaN";}
|
|
if(floorProb == 0) {throw "detected NaN";}
|
|
if(prob != prob) {throw "detected NaN";}
|
|
|
|
//assert(prob != 0.0);
|
|
|
|
|
|
}
|
|
#pragma omp critical
|
|
predictionProbabilities.push_back(innerVector);
|
|
}
|
|
return predictionProbabilities;
|
|
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // MYTRANSITION_H
|