154 lines
3.8 KiB
C++
154 lines
3.8 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 TURNDETECTION_H
|
||
#define TURNDETECTION_H
|
||
|
||
#include "GyroscopeData.h"
|
||
#include "AccelerometerData.h"
|
||
#include "../../data/Timestamp.h"
|
||
#include "../../math/MovingStdDevTS.h"
|
||
#include "../../math/Matrix3.h"
|
||
|
||
#include "../../geo/Point3.h"
|
||
#include "PoseProvider.h"
|
||
|
||
//#include <eigen3/Eigen/Dense>
|
||
|
||
#include <cmath>
|
||
#include <vector>
|
||
|
||
#include "TurnDetectionPlot.h"
|
||
|
||
|
||
#include "../../Assertions.h"
|
||
#include "TurnProvider.h"
|
||
|
||
class TurnDetection : public TurnProvider {
|
||
|
||
private:
|
||
|
||
PoseProvider* pose = nullptr;
|
||
|
||
//std::vector<GyroscopeData> gyroData;
|
||
//Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
|
||
Vector3 prevGyro = Vector3(0,0,0);
|
||
|
||
Timestamp lastGyroReading;
|
||
|
||
float curSigma = 0;
|
||
|
||
MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
TurnDetectionPlot plot;
|
||
#endif
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
TurnDetection(PoseProvider* pose) : pose(pose) {
|
||
;
|
||
}
|
||
|
||
|
||
// does not seem to help...
|
||
// struct DriftEstimator {
|
||
|
||
// MovingAverageTS<Eigen::Vector3f> avg;
|
||
|
||
// DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) {
|
||
// ;
|
||
// }
|
||
|
||
// void removeDrift(const Timestamp ts, Eigen::Vector3f& gyro) {
|
||
|
||
// if (gyro.norm() < 0.15) {
|
||
// avg.add(ts, gyro);
|
||
// gyro -= avg.get();
|
||
// }
|
||
|
||
// }
|
||
|
||
// } driftEst;
|
||
|
||
|
||
/** get the current uncertainty estimation */
|
||
float getSigma() const override {
|
||
return curSigma;
|
||
}
|
||
|
||
float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
|
||
|
||
// ignore the first reading completely, just remember its timestamp
|
||
if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;}
|
||
|
||
// time-difference between previous and current reading
|
||
const Timestamp curDiff = ts - lastGyroReading;
|
||
lastGyroReading = ts;
|
||
|
||
// fast sensors might lead to delay = 0 ms. filter those values
|
||
if (curDiff.isZero()) {return 0.0f;}
|
||
|
||
// ignore readings until the first orientation-estimation is available
|
||
// otherwise we would use a wrong rotation matrix which yields wrong results!
|
||
if (!pose->isKnown()) {return 0.0f;}
|
||
|
||
// get the current gyro-reading as vector
|
||
//Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
|
||
const Vector3 vec(gyro.x, gyro.y, gyro.z);
|
||
|
||
// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
|
||
//Eigen::Vector3f curGyro = orientation.rotationMatrix * vec;
|
||
const Vector3 curGyro = pose->getMatrix() * vec;
|
||
//driftEst.removeDrift(ts, curGyro);
|
||
|
||
|
||
// area
|
||
//const Eigen::Vector3f area =
|
||
const Vector3 area =
|
||
|
||
// Trapezoid rule (should be more accurate but does not always help?!)
|
||
//(prevGyro * curDiff.sec()) + // squared region
|
||
//((curGyro - prevGyro) * 0.5 * curDiff.sec()); // triangle region to the next (enhances the quality)
|
||
|
||
// average (is the same as above)
|
||
//((curGyro+prevGyro)/2 * curDiff.sec());
|
||
|
||
// just the rectangular region
|
||
(prevGyro * curDiff.sec()); // BEST?!
|
||
|
||
//}
|
||
|
||
// update the old value
|
||
prevGyro = curGyro;
|
||
|
||
// rotation = z-axis only!
|
||
//const float delta = area(2);
|
||
const float delta = area.z;
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
plot.addRelative(ts, delta, gyro, curGyro);
|
||
#endif
|
||
|
||
//stdDevForSigma.add(ts, prevGyro.z); // ignore delta T. directly us radians-per-sec as sigma
|
||
//curSigma = stdDevForSigma.get();
|
||
const float radPerSec = 1.0f / 180.0f * M_PI;
|
||
curSigma = radPerSec + std::abs(prevGyro.z * 0.05); // constant of 1deg/sec + 5% of current turn rate
|
||
|
||
// done
|
||
return delta;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif // TURNDETECTION_H
|