76 lines
2.0 KiB
C++
76 lines
2.0 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef GPSPROBABILITY_H
|
||
#define GPSPROBABILITY_H
|
||
|
||
#include "GPSData.h"
|
||
#include "../../geo/Point3.h"
|
||
#include "../../math/distribution/Region.h"
|
||
#include "../../geo/EarthMapping.h"
|
||
|
||
class GPSProbability {
|
||
|
||
private:
|
||
|
||
/** convert between map and earth */
|
||
const EarthMapping& em;
|
||
|
||
public:
|
||
|
||
/** ctor with the map<->earth translator */
|
||
GPSProbability(const EarthMapping& em) : em(em) {
|
||
;
|
||
}
|
||
|
||
/** get the probability for residing at pos_m [in meter, map-coordinates] given some GPS measurement */
|
||
double getProbability(const Point3 pos_m, const GPSData& d) const {
|
||
|
||
// pad GPS? -> no gps eval
|
||
if (isBad(d)) {return 1.0;}
|
||
|
||
// adjust accuracy [sometimes strange values are provided here!]
|
||
float accuracy = d.accuracy;
|
||
if (accuracy < 3.0) {
|
||
std::cout << "note: adjusting gps accuracy as '" << accuracy << "'' seems invalid" << std::endl;
|
||
accuracy = 3.0;
|
||
}
|
||
|
||
// convert GPS to map coordinats
|
||
const Point3 gpsToMap_m = em.worldToMap(d.toEarthPos());
|
||
|
||
// distance between given point and GPS's estimation
|
||
const float dist = pos_m.getDistance(gpsToMap_m);
|
||
|
||
// calculate probability
|
||
//const double prob = Distribution::Region<double>::getProbability(0, d.accuracy, dist);
|
||
const double prob = Distribution::Normal<double>::getProbability(0, accuracy, dist);
|
||
|
||
// sanity checks
|
||
Assert::isNot0(prob, "gps probability is 0.0");
|
||
Assert::isNotNaN(prob, "gps probability is NaN");
|
||
|
||
// done
|
||
return prob;
|
||
|
||
}
|
||
|
||
private:
|
||
|
||
/** returns true if the given GPS reading is bad [inaccurate, invalid, ...] */
|
||
static inline bool isBad(const GPSData& d) {
|
||
return (!d.isValid()) || (d.accuracy == 0) || (d.accuracy > 25);
|
||
}
|
||
|
||
|
||
};
|
||
|
||
#endif // GPSPROBABILITY_H
|