kazu
04d8ae8c74
- some should be the same as previous commit (sorry!) - some should be new: LINT checks, ...?
66 lines
1.6 KiB
C++
66 lines
1.6 KiB
C++
#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
|