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

added fixelag smoothing

Browse Source
This commit is contained in:
toni
2016-03-22 18:01:18 +01:00
parent ee0c778728
commit 94fb34e6f9
7 changed files with 632 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
code/CMakeLists.txt.user
View File

@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 3.6.0, 2016-03-16T19:01:25. -->
<!-- Written by QtCreator 3.6.0, 2016-03-22T18:00:37. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>

4
code/Settings.h
View File

@@ -19,6 +19,10 @@ namespace MiscSettings {
const int numParticles = 7500;
const int lag = 5;
const int fixedLagGap = 1;
}

349
code/eval/FixedLagEvalBase.h Normal file
View File

@@ -0,0 +1,349 @@
#ifndef FIXEDLAGEVALBASE_H
#define FIXEDLAGEVALBASE_H
#include "../Settings.h"
#include "../Helper.h"
#include "../Vis.h"
#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterHistory.h>
#include <KLib/math/filter/smoothing/BackwardSimulation.h>
#include <KLib/math/statistics/Statistics.h>
#include "GroundTruthWay.h"
#include "../particles/MyState.h"
#include "../particles/MyObservation.h"
#include "../particles/MyEvaluation.h"
#include "../particles/MyTransition.h"
#include "../particles/MyInitializer.h"
#include "../particles/smoothing/MySmoothingTransition.h"
#include "../particles/smoothing/MySmoothingTransitionSimple.h"
#include "../reader/SensorReader.h"
#include "../reader/SensorReaderStep.h"
#include "../reader/SensorReaderTurn.h"
#include "../lukas/TurnObservation.h"
#include "../lukas/StepObservation.h"
#include "../toni/BarometerSensorReader.h"
#include "../frank/WiFiSensorReader.h"
#include "../frank/BeaconSensorReader.h"
#include "../frank/OrientationSensorReader.h"
class FixedLagEvalBase {
protected:
Grid<MyGridNode> grid;
Helper::FHWSFloors floors;
Vis vis;
K::ParticleFilterHistory<MyState, MyControl, MyObservation>* pf;
K::BackwardFilter<MyState>* bf;
SensorReader* sr;
SensorReaderTurn* srt;
SensorReaderStep* srs;
std::string runName;
GroundTruthWay gtw;
// OLD
//std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
//std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};
// NEW
std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path1dbl = {29, 29, 28,27,26,255,25,24,23,22,21,20};
std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path2dbl = {19, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44}; // duplicate 1st waypoint!
public:
FixedLagEvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {
// build the grid
Helper::buildTheGrid(grid, floors);
// setup the visualisation
vis.addFloor(floors.f0, floors.h0);
vis.addFloor(floors.f1, floors.h1);
vis.addFloor(floors.f2, floors.h2);
vis.addFloor(floors.f3, floors.h3);
vis.floors.setColorHex("#666666");
vis.groundTruth.setCustomAttr("dashtype 3");
vis.groundTruth.setColorHex("#009900");
vis.gp << "unset cbrange\n";
}
static GridPoint conv(const Point3& p) {
return GridPoint(p.x, p.y, p.z);
}
GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {
// construct the ground-truth-path by using all contained waypoint ids
std::vector<Point3> path;
for (int id : ids) {
auto it = waypoints.find(id);
if(it == waypoints.end()) {throw "not found";}
path.push_back(it->second);
}
// new created the timed path
GroundTruthWay gtw;
int i = 0;
while (sr.hasNext()) {
const SensorEntry se = sr.getNext();
if (se.data.empty()) {continue;} // why necessary??
if (se.idx == 99) {
gtw.add(se.ts, path[i]);
++i;
}
}
// ensure the sensor-data contained usable timestamps for the ground-truth mapping
assert(i>0);
sr.rewind();
return gtw;
}
void run() {
// sensor numbers
const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
//const int s_linearAcceleration = 2;
std::list<TurnObservation> turn_observations;
std::list<StepObservation> step_observations;
//Create an BarometerSensorReader
BarometerSensorReader baroSensorReader;
//Read all turn Observations
while(srt->hasNext()) {
SensorEntryTurn set = srt->getNext();
TurnObservation to;
to.ts = set.ts;
to.delta_heading = set.delta_heading;
to.delta_motion = set.delta_motion;
turn_observations.push_back(to);
}
//Step Observations
while(srs->hasNext()) {
SensorEntryStep ses = srs->getNext();
StepObservation so;
so.ts = ses.ts;
step_observations.push_back(so);
}
// the to-be-evaluated observation
MyObservation obs;
obs.step = new StepObservation(); obs.step->steps = 0;
obs.turn = new TurnObservation(); obs.turn->delta_heading = 0; obs.turn->delta_motion = 0;
// control data
MyControl ctrl;
//History of all estimated particles. Used for smoothing
std::vector<std::vector<K::Particle<MyState>>> pfHistory;
std::vector<Point3> smoothedEst;
std::vector<uint64_t> tsHistory;
std::vector<Point3> pathEst;
uint64_t lastTransitionTS = 0;
int64_t start_time = -1;
K::Statistics<double> statsFiltering;
K::Statistics<double> statsSmoothing;
int cnt = 0;
//stats file
std::ofstream statsout("/tmp/unsmoothed_" + runName + ".stats");
// process each single sensor reading
while(sr->hasNext()) {
// get the next sensor reading from the CSV
const SensorEntry se = sr->getNext();
//start_time needed for time calculation of steps and turns
obs.latestSensorDataTS = se.ts;
if (start_time == -1) {start_time = se.ts;}
int64_t current_time = se.ts - start_time;
switch(se.idx) {
case s_wifi: {
obs.wifi = WiFiSensorReader::readWifi(se);
break;
}
case s_beacons: {
BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se);
if (!boe.mac.empty()) {
obs.beacons.entries.push_back(boe);
} // add the observed beacon
obs.beacons.removeOld(obs.latestSensorDataTS);
break;
}
case s_barometer: {
obs.barometer = baroSensorReader.readBarometer(se);
break;
}
// case s_linearAcceleration:{
// baroSensorReader.readVerticalAcceleration(se);
// break;
// }
case s_orientation: {
obs.orientation = OrientationSensorReader::read(se);
break;
}
}
// process all occurred turns
while (!step_observations.empty() && current_time > step_observations.front().ts) {
const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
obs.step->steps++;
ctrl.walked_m = obs.step->steps * 0.71;
}
// process all occurred steps
while (!turn_observations.empty() && current_time > turn_observations.front().ts) {
const TurnObservation _to = turn_observations.front(); turn_observations.pop_front();
obs.turn->delta_heading += _to.delta_heading;
obs.turn->delta_motion += _to.delta_motion;
ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);
}
// time for a transition?
if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {
lastTransitionTS = se.ts;
// timed updates
((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);
// update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
const MyState est = pf->update(&ctrl, obs);
const Point3 curEst = est.pCur;
// error calculation. compare ground-truth to estimation
const int offset = 0;
const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
const Point3 diff = curEst - curGT;
pathEst.push_back(curEst);
const float err = diff.length();
// skip the first 24 scans due to uniform distribution start
if (++cnt > 24) {
statsFiltering.add(err);
std::cout << "Filtering: " << statsFiltering.asString() << std::endl;
//save the current estimation for later smoothing.
pfHistory.push_back(pf->getNonResamplingParticles());
tsHistory.push_back(se.ts);
}
//fixed-lag smoothing
MyState estBF;
if(pfHistory.size() >= MiscSettings::lag){
bf->reset();
//use every n-th (std = 1) particle set of the history within a given lag (std = 5)
for(int i = 0; i < MiscSettings::lag; i += MiscSettings::fixedLagGap){
((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[i]);
estBF = bf->update(pfHistory[(pfHistory.size() - 1) - i]);
}
//use and visualize the smoothed particle set
const Point3 curSmoothedEst = estBF.pCur;
smoothedEst.push_back(curSmoothedEst);
// error calculation. compare ground-truth to estimation
const Point3 curGTSmoothed = gtw.getPosAtTime(se.ts - (MiscSettings::lag * MiscSettings::timeSteps));
const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;
const float errSmoothed = diffSmoothed.length();
statsSmoothing.add(errSmoothed);
std::cout << "Smoothing: " << statsSmoothing.asString() << std::endl;
// plot
vis.clearStates();
for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
vis.addState(p.state.walkState);
}
vis.setTimestamp(se.ts);
vis.addGroundTruth(gtw);
vis.addEstPath(smoothedEst);
vis.setEstAndShould(curSmoothedEst, curGTSmoothed);
if (obs.barometer != nullptr) {
vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
}
vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
//vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";
Point2 p1(0.1, 0.1);
Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
//Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";
vis.show();
// prevent gnuplot errors
usleep(1000*33);
}
}
}
statsout.close();
}
};
#endif // EVALBASE_H

10
code/eval/PaperVisImportance.h
View File

@@ -115,8 +115,8 @@ public:
// start and end
//const MyGridNode& gnStart = grid.getNodeFor(GridPoint(1500, 300, 0));
//const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(900, 4600, 0));
const MyGridNode& gnStart = grid.getNodeFor(GridPoint(300, 300, 0));
const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(4700, 1300, 0));
const MyGridNode& gnStart = grid.getNodeFor(GridPoint(300, 300, 0));
const MyGridNode& gnEnd = grid.getNodeFor(GridPoint(4700, 1300, 0));
// build all shortest path to reach th target
Dijkstra<MyGridNode> dijkstra;
@@ -124,7 +124,7 @@ public:
DijkstraMapperNormal accNormal(grid);
// path without importance
dijkstra.build(gnStart, gnStart, accNormal);
dijkstra.build(&gnStart, accNormal);
DijkstraPath<MyGridNode> pathNormal(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
// stamp importance information onto the grid-nodes
@@ -132,7 +132,7 @@ public:
gridImp.addImportance(grid, h0.cm());
// path WITH importance
dijkstra.build(gnStart, gnStart, accImp);
dijkstra.build(&gnStart, accImp);
DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
// build plot
@@ -240,7 +240,7 @@ public:
// path WITH importance
DijkstraMapper accImp(grid);
dijkstra.build(gnStart, gnStart, accImp);
dijkstra.build(&gnStart, accImp);
DijkstraPath<MyGridNode> pathImp(dijkstra.getNode(gnEnd), dijkstra.getNode(gnStart));
// stamp distance information onto the grid

5
code/eval/SmoothingEval1.h
View File

@@ -2,6 +2,7 @@
#define SMOOTHINGEVAL1_H
#include "SmoothingEvalBase.h"
#include "FixedLagEvalBase.h"
#include "../DijkstraMapper.h"
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
@@ -20,7 +21,7 @@
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>
class SmoothingEval1 : public SmoothingEvalBase {
class SmoothingEval1 : public FixedLagEvalBase {
public:
@@ -75,7 +76,7 @@ public:
bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
if(smoothing_resample)
bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
bf->setTransition(std::unique_ptr<MySmoothingTransition>( new MySmoothingTransition(&grid)) );
bf->setTransition(std::unique_ptr<MySmoothingTransitionSimple>( new MySmoothingTransitionSimple()) );
}

145
code/particles/smoothing/MySmoothingTransition.h Normal file
View File

@@ -0,0 +1,145 @@
#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/grid/Grid.h>
#include "../MyState.h"
#include "../MyControl.h"
#include "../../DijkstraMapper.h"
#include "../../toni/barometric.h"
#include <map>
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;
static bool smoothing_baro_use_hPa = false;
static double smoothing_baro_with_measurement = false;
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;
auto p1 = particles_old.begin();
auto p2 = particles_new.begin();
#pragma omp parallel for private(p2) shared(predictionProbabilities)
for (p1 = particles_old.begin(); p1 < particles_old.end(); ++p1) {
std::vector<double> innerVector;
for(p2 = particles_new.begin(); p2 < particles_new.end(); ++p2){
// 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* 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));
double distDijkstra_m = dijkstra.getNode(*src)->cumWeight;
const double distProb = distWalk.getProbability(distDijkstra_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);
//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

125
code/particles/smoothing/MySmoothingTransitionSimple.h Normal file
View File

@@ -0,0 +1,125 @@
#ifndef MYSMOOTHINGTRANSITIONSIMPLE_H
#define MYSMOOTHINGTRANSITIONSIMPLE_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/geo/Angle.h>
#include "../MyState.h"
#include "../MyControl.h"
#include "../../Helper.h"
#include "../../toni/barometric.h"
class MySmoothingTransitionSimple : public K::BackwardFilterTransition<MyState> {
private:
/** a simple normal distribution */
K::NormalDistribution distWalk;
public:
/**
* 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.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;
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) {
std::vector<double> innerVector;
for(p2 = particles_new.begin(); p2 < particles_new.end(); ++p2){
//!!!distance kann hier zu groß werden!!!
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);
//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);
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