9eb774f7b9
changed particle init fixed uninitialized values new ground-truth helper methods added new stats to the eval (distance from ground-truth)
48 lines
1.1 KiB
C++
48 lines
1.1 KiB
C++
#ifndef GROUNDTRUTHWAY_H
|
|
#define GROUNDTRUTHWAY_H
|
|
|
|
#include <Indoor/math/Interpolator.h>
|
|
#include <Indoor/geo/Point3.h>
|
|
|
|
/**
|
|
* interpolated ground-trouth based on timed check-points
|
|
*/
|
|
class GroundTruthWay : public Interpolator<uint64_t, Point3> {
|
|
|
|
public:
|
|
|
|
Point3 getPosAtTime(const uint64_t ts) const {
|
|
return get(ts);
|
|
}
|
|
|
|
/** get the ground truth way */
|
|
const std::vector<InterpolatorEntry>& getWay() const {return entries;}
|
|
|
|
/** get the time where the ground-truth is nearest to p */
|
|
uint64_t getNearestTime(const Point3 p) const {
|
|
|
|
// TODO only an approximation (100ms) -> minor errors possible
|
|
|
|
float minD = INFINITY;
|
|
uint64_t minTS = 0;
|
|
for (uint64_t ts = getMinKey(); ts <= getMaxKey(); ts += 100) {
|
|
const Point3 cur = getPosAtTime(ts);
|
|
const float curD = cur.getDistance(p);
|
|
if (curD < minD) {minD = curD; minTS = ts;}
|
|
}
|
|
return minTS;
|
|
|
|
}
|
|
|
|
/** get the minimum distance between the given point and the ground-truth (at any time) */
|
|
float getMinDist(const Point3 p1) const {
|
|
const uint64_t minTS = getNearestTime(p1);
|
|
const Point3 p2 = getPosAtTime(minTS);
|
|
return p1.getDistance(p2);
|
|
}
|
|
|
|
};
|
|
|
|
|
|
#endif // GROUNDTRUTHWAY_H
|