toni/IPIN2016
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added astar to smoothing transition

Browse Source
This commit is contained in:
toni
2016-04-11 10:57:29 +02:00
parent 5aad08e17c
commit de8783b9ca
13 changed files with 146 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
code/particles/smoothing/MySmoothingTransition.h
View File

@@ -7,6 +7,7 @@
#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"
@@ -22,6 +23,8 @@ 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:
@@ -75,6 +78,7 @@ public:
// 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;
auto p1 = particles_old.begin();
auto p2 = particles_new.begin();
@@ -94,12 +98,40 @@ public:
const MyGridNode* dst = grid->getNodePtrFor(GridPoint(p1->state.pCur.x, p1->state.pCur.y, p1->state.pCur.z));
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));
// Dijkstra<MyGridNode> dijkstra;
// dijkstra.build(src, dst, DijkstraMapper(*grid));
double distDijkstra_m = dijkstra.getNode(*src)->cumWeight;
// double distDijkstra_m = dijkstra.getNode(*src)->cumWeight;
const double distProb = distWalk.getProbability(distDijkstra_m);
AStar<MyGridNode> aStar;
DijkstraMapper dm(*grid);
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);
//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;
@@ -127,7 +159,7 @@ public:
//if(distance_m != distance_m) {throw "detected NaN";}
//if(distProb != distProb) {throw "detected NaN";}
if(angle != angle) {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";}