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

added activity recognition to smoothing transition

Browse Source
This commit is contained in:
toni
2016-04-26 10:12:10 +02:00
parent ed8e37108a
commit f7e817d5e4
13 changed files with 204 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -96,22 +96,8 @@ public:
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;
@@ -121,16 +107,8 @@ public:
}
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
@@ -140,22 +118,12 @@ public:
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);
//heading change prob
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.walkState.heading.getRAD(), p1->state.walkState.heading.getRAD());
double diffDeg = Angle::radToDeg(diffRad);
double angularChangeZeroToPi = std::fmod(std::abs(p1->state.angularHeadingChange), 360.0);
// 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);
const double headingProb = K::NormalDistribution::getProbability(angularChangeZeroToPi, smoothing_heading_sigma, diffDeg);
//check how near we are to the measurement
double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
@@ -167,10 +135,10 @@ public:
//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";}
//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);

78
code/particles/smoothing/MySmoothingTransitionExperimental.h
View File

@@ -52,8 +52,9 @@ public:
/**
* 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)
* @param particles_new p_t (Forward Filter) p2
* @param particles_old p_t+1 (Smoothed Particles from Step before) p1
* q(p1 | p2) is calculated
*/
std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
std::vector<K::Particle<MyState>>const& particles_old ) override {
@@ -76,59 +77,76 @@ public:
auto p2 = &particles_new[j];
//!!!distance kann hier zu groß werden!!!
const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0;
double muDistance = 1.0;
double sigmaDistance = 0.5;
//get distance walked and getProb using the walking model
//double distDijkstra_m = ((GRID_DISTANCE_CM / 100.0) * (8 - 1));
const double distProb = distWalk.getProbability(distance_m);
switch (p2->state.currentActivity) {
case Activity::ELEVATOR:
muDistance = 0.0;
sigmaDistance = 0.3;
break;
case Activity::STAIRS_DOWN:
muDistance = 0.5;
sigmaDistance = 0.3;
break;
//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);
// }
case Activity::STAIRS_UP:
muDistance = 0.4;
sigmaDistance = 0.2;
break;
// const double headingProb = K::NormalDistribution::getProbability(p1->state.cumulativeHeading, smoothing_heading_sigma, angle);
case Activity::STANDING:
muDistance = 0.0;
sigmaDistance = 0.2;
break;
case Activity::WALKING:
muDistance = 1.0;
sigmaDistance = 0.5;
break;
default:
muDistance = 1.0;
sigmaDistance = 0.5;
break;
}
const double distProb = K::NormalDistribution::getProbability(muDistance, sigmaDistance, distance_m);
// 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);
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.walkState.heading.getRAD(), p1->state.walkState.heading.getRAD());
double diffDeg = Angle::radToDeg(diffRad);
double angularChangeZeroToPi = std::fmod(std::abs(p1->state.angularHeadingChange), 360.0);
const double headingProb = K::NormalDistribution::getProbability(angularChangeZeroToPi, smoothing_heading_sigma, diffDeg);
//check how near we are to the measurement
double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
const double floorProb = K::NormalDistribution::getProbability(p1->state.measurement_pressure, smoothing_baro_sigma, p2->state.hPa);
//combine the probabilities
double prob = distProb * headingProb * floorProb;
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);
//error checks
// if(distance_m != distance_m) {throw "detected NaN";}
// if(distProb != distProb) {throw "detected NaN";}
// if(headingProb != headingProb) {throw "detected NaN";}
// if(floorProb != floorProb) {throw "detected NaN";}
// if(floorProb == 0) {throw "detected zero prob in floorprob";}
// if(prob != prob) {throw "detected NaN";}
//if(prob == 0) {++zeroCounter;}
}
#pragma omp critical
predictionProbabilities.push_back(innerVector);
}
return predictionProbabilities;
}

87
code/particles/smoothing/MySmoothingTransitionSimple.h
View File

@@ -17,37 +17,38 @@ class MySmoothingTransitionSimple : public K::BackwardFilterTransition<MyState>
private:
/** a simple normal distribution */
K::NormalDistribution distWalk;
/** a simple normal distribution */
K::NormalDistribution distWalk;
public:
/**
* ctor
* @param choice the choice to use for randomly drawing nodes
* @param fp the underlying floorplan
*/
/**
* ctor
* @param choice the choice to use for randomly drawing nodes
* @param fp the underlying floorplan
*/
MySmoothingTransitionSimple() :
distWalk(smoothing_walk_mu, smoothing_walk_sigma) {
distWalk(smoothing_walk_mu, smoothing_walk_sigma)
{
distWalk.setSeed(4321);
}
}
public:
uint64_t ts = 0;
uint64_t deltaMS = 0;
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 {
/** 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;
}
}
this->ts = ts;
}
}
/**
* smoothing transition starting at T with t, t-1,...0
@@ -64,52 +65,44 @@ public:
// 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;
auto p1 = particles_old.begin(); //smoothed / backward filter p_t+1
auto p2 = particles_new.begin(); //forward filter p_t
#pragma omp parallel for private(p2) shared(predictionProbabilities)
for (p1 = particles_old.begin(); p1 < particles_old.end(); ++p1) {
omp_set_dynamic(0); // Explicitly disable dynamic teams
omp_set_num_threads(6);
#pragma omp parallel for shared(predictionProbabilities)
for (int i = 0; i < particles_old.size(); ++i) {
std::vector<double> innerVector;
for(p2 = particles_new.begin(); p2 < particles_new.end(); ++p2){
auto p1 = &particles_old[i];
//!!!distance kann hier zu groß werden!!!
for(int j = 0; j < particles_new.size(); ++j){
auto p2 = &particles_new[j];
//distance can be pretty big here
const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0;
//get distance walked and getProb using the walking model
//double distDijkstra_m = ((GRID_DISTANCE_CM / 100.0) * (8 - 1));
const double distProb = distWalk.getProbability(distance_m);
//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);
//assert(headingProb != 0.0);
//assert(distProb != 0.0);
//get proba for heading change
double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.walkState.heading.getRAD(), p1->state.walkState.heading.getRAD());
double diffDeg = Angle::radToDeg(diffRad);
double angularChangeZeroToPi = std::fmod(std::abs(p1->state.angularHeadingChange), 360.0);
const double headingProb = K::NormalDistribution::getProbability(angularChangeZeroToPi, smoothing_heading_sigma, diffDeg);
//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 * headingProb * floorProb;
innerVector.push_back(prob);
//error checks
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(floorProb == 0) {throw "detected zero prob in floorprob";}
if(prob != prob) {throw "detected NaN";}
//assert(prob != 0.0);
//if(prob == 0) {throw "detected zero prob in smoothing transition";}
}
#pragma omp critical
@@ -122,4 +115,4 @@ public:
};
#endif // MYTRANSITION_H
#endif // MYTRANSITIONSIMPLE_H