126 lines
2.8 KiB
C++
126 lines
2.8 KiB
C++
#ifndef TURNDETECTION_H
|
|
#define TURNDETECTION_H
|
|
|
|
#include "GyroscopeData.h"
|
|
#include "AccelerometerData.h"
|
|
#include "../../data/Timestamp.h"
|
|
#include "../../math/MovingAverageTS.h"
|
|
#include "../../math/Matrix3.h"
|
|
|
|
#include "../../geo/Point3.h"
|
|
#include "PoseDetection.h"
|
|
|
|
//#include <eigen3/Eigen/Dense>
|
|
|
|
#include <cmath>
|
|
#include <vector>
|
|
|
|
#include "TurnDetectionPlot.h"
|
|
|
|
|
|
#include "../../Assertions.h"
|
|
|
|
class TurnDetection {
|
|
|
|
private:
|
|
|
|
PoseDetection* pose = nullptr;
|
|
|
|
//std::vector<GyroscopeData> gyroData;
|
|
//Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
|
|
Vector3 prevGyro = Vector3(0,0,0);
|
|
|
|
Timestamp lastGyroReading;
|
|
|
|
#ifdef WITH_DEBUG_PLOT
|
|
TurnDetectionPlot plot;
|
|
#endif
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
TurnDetection(PoseDetection* 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;
|
|
|
|
|
|
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)
|
|
|
|
// 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.add(ts, delta, gyro, curGyro);
|
|
#endif
|
|
|
|
// done
|
|
return delta;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // TURNDETECTION_H
|