312 lines
9.5 KiB
C++
312 lines
9.5 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 POSEDETECTION2_H
|
||
#define POSEDETECTION2_H
|
||
|
||
|
||
#include "AccelerometerData.h"
|
||
#include "GyroscopeData.h"
|
||
|
||
#include "../../data/Timestamp.h"
|
||
|
||
//#include "../../math/MovingAverageTS.h"
|
||
//#include "../../math/MovingMedianTS.h"
|
||
#include "../../math/Matrix3.h"
|
||
|
||
#include "../../math/filter/Complementary.h"
|
||
|
||
#include "../../geo/Point3.h"
|
||
|
||
//#include "../../math/FixedFrequencyInterpolator.h"
|
||
|
||
#include "PoseDetectionPlot.h"
|
||
#include "PoseProvider.h"
|
||
|
||
/**
|
||
* estimate the smartphones world-pose,
|
||
* based on the accelerometer's data
|
||
*
|
||
* https://robotics.stackexchange.com/questions/6953/how-to-calculate-euler-angles-from-gyroscope-output
|
||
*/
|
||
class PoseDetection2 : public PoseProvider {
|
||
|
||
private:
|
||
|
||
struct {
|
||
//Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
|
||
Matrix3 rotationMatrix = Matrix3::identity();
|
||
bool isKnown = false;
|
||
Timestamp lastEstimation;
|
||
} orientation;
|
||
|
||
/** how the pose is estimated */
|
||
//LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250));
|
||
//EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(450));
|
||
//EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300));
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
PoseDetectionPlot plot;
|
||
PoseDetectionPlotAngles plotAngles;
|
||
#endif
|
||
|
||
Vector3 lastGyroReading;
|
||
Timestamp lastGyroReadingTS;
|
||
|
||
//FixedFrequencyInterpolator<Vector3> interpolAcc;
|
||
//FixedFrequencyInterpolator<Vector3> interpolGyro;
|
||
|
||
Filter::Complementary<Vector3> filter;
|
||
|
||
//std::vector<Vector3> accBuf;
|
||
//std::vector<Vector3> gyroBuf;
|
||
|
||
|
||
|
||
Vector3 curAccel;
|
||
|
||
bool firstAccel = true;
|
||
Vector3 curAccel_rad;
|
||
Vector3 curGyroSum_rad;
|
||
|
||
//Matrix3 curGyroMatrix = Matrix3::identity();
|
||
|
||
static constexpr float splitFreq = 2.5;
|
||
static constexpr float sRate = 50;
|
||
static constexpr int sRateTS = 1000/sRate;
|
||
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
//PoseDetection2() : interpolAcc(Timestamp::fromMS(sRateTS)), interpolGyro(Timestamp::fromMS(sRateTS)), filter(splitFreq, sRate) {
|
||
PoseDetection2() : filter(1, 2.5) {
|
||
#ifdef WITH_DEBUG_PLOT
|
||
plot.setName("PoseDetection2");
|
||
#endif
|
||
}
|
||
|
||
/** get the smartphone's rotation matrix */
|
||
const Matrix3& getMatrix() const override {
|
||
return orientation.rotationMatrix;
|
||
}
|
||
|
||
/** is the pose known and stable? */
|
||
bool isKnown() const override {
|
||
return orientation.isKnown;
|
||
}
|
||
|
||
|
||
float angleX_old_rad = 0;
|
||
float angleY_old_rad = 0;
|
||
|
||
|
||
void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
|
||
|
||
// https://theccontinuum.com/2012/09/24/arduino-imu-pitch-roll-from-accelerometer/#docs
|
||
|
||
float angleX_rad = std::atan2( acc.y, acc.z);
|
||
float angleY_rad = std::atan2(-acc.x, std::sqrt(acc.y*acc.y + acc.z*acc.z));
|
||
float angleZ_rad = 0;
|
||
|
||
if (std::abs(angleX_rad-angleX_old_rad) > M_PI) {angleX_rad += 2*M_PI;}
|
||
if (std::abs(angleY_rad-angleY_old_rad) > M_PI) {angleY_rad += 2*M_PI;}
|
||
|
||
angleX_old_rad = angleX_rad;
|
||
angleY_old_rad = angleY_rad;
|
||
|
||
// float angleX_rad = std::atan2(acc.y, std::sqrt(acc.x*acc.x + acc.z*acc.z));
|
||
// float angleY_rad = std::atan2(-acc.x, std::sqrt(acc.y*acc.y + acc.z*acc.z));
|
||
// float angleZ_rad = 0;
|
||
|
||
// Point2 ref(0,1);
|
||
// Point2 xz( acc.x, acc.z);
|
||
// Point2 yz(-acc.y, acc.z);
|
||
// float angleY_rad = std::atan2( determinant(ref,xz), dot(ref,xz) );
|
||
// float angleX_rad = std::atan2( determinant(ref,yz), dot(ref,yz) );
|
||
// float angleZ_rad = 0.0;
|
||
|
||
// float angleX_rad = std::atan2(acc.y, acc.z);
|
||
// float angleY_rad = -std::atan2(acc.x, acc.z);
|
||
// float angleZ_rad = 0;//-std::atan2(acc.y, acc.x);
|
||
|
||
|
||
// float angleX2_rad = (angleX1_rad > 0) ? (angleX1_rad - 2*M_PI) : (angleX1_rad + 2*M_PI);
|
||
// float angleY2_rad = (angleY1_rad > 0) ? (angleY1_rad - 2*M_PI) : (angleY1_rad + 2*M_PI);
|
||
// float angleX_rad = (std::abs(curAngle_rad.x-angleX1_rad) < std::abs(curAccel_rad.x-angleX2_rad)) ? (angleX1_rad) : (angleX2_rad);
|
||
// float angleY_rad = (std::abs(curAngle_rad.y-angleY1_rad) < std::abs(curAccel_rad.y-angleY2_rad)) ? (angleY1_rad) : (angleY2_rad);
|
||
|
||
curAccel = Vector3(acc.x, acc.y, acc.z);
|
||
curAccel_rad = Vector3(angleX_rad, angleY_rad, angleZ_rad);
|
||
|
||
//curAccel_rad = Matrix3::getRotationRad(0,0,-angleZ_rad) * Vector3(angleX_rad, angleY_rad, angleZ_rad);
|
||
|
||
|
||
if (firstAccel) {
|
||
curGyroSum_rad = curAccel_rad;
|
||
firstAccel = false;
|
||
}
|
||
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
void addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
|
||
|
||
Vector3 vec(gyro.x, gyro.y, gyro.z);
|
||
|
||
// ignore the first reading completely, just remember its timestamp
|
||
if (lastGyroReadingTS.isZero()) {lastGyroReadingTS = ts; return;}
|
||
|
||
// time-difference between previous and current reading
|
||
const Timestamp curDiff = ts - lastGyroReadingTS;
|
||
lastGyroReadingTS = ts;
|
||
|
||
// fast sensors might lead to delay = 0 ms. filter those values
|
||
if (curDiff.isZero()) {return;}
|
||
|
||
// current area
|
||
const Vector3 curArea = (lastGyroReading * curDiff.sec());
|
||
|
||
// // update sum
|
||
// curAngle_rad += curArea;
|
||
// curGyroSum_rad += curArea;
|
||
|
||
// // PAPER
|
||
const float dx = 1 * lastGyroReading.x + std::sin(curGyroSum_rad.x)*std::sin(curGyroSum_rad.y)/std::cos(curGyroSum_rad.y)*lastGyroReading.y + std::cos(curGyroSum_rad.x)*std::sin(curGyroSum_rad.y)/std::cos(curGyroSum_rad.y)*lastGyroReading.z;
|
||
const float dy = std::cos(curGyroSum_rad.x)*lastGyroReading.y + -std::sin(curGyroSum_rad.x)*lastGyroReading.z;
|
||
const float dz = std::sin(curGyroSum_rad.x)/std::cos(curGyroSum_rad.y)*lastGyroReading.y + std::cos(curGyroSum_rad.x)/std::cos(curGyroSum_rad.y)*lastGyroReading.z;
|
||
curGyroSum_rad.x += dx*curDiff.sec();
|
||
curGyroSum_rad.y += dy*curDiff.sec();
|
||
curGyroSum_rad.z += dz*curDiff.sec();
|
||
|
||
|
||
// // PAPER
|
||
// const Vector3 n = lastGyroReading / lastGyroReading.norm();
|
||
// const float mag = lastGyroReading.norm() * curDiff.sec();
|
||
// if (mag != 0) {
|
||
// curGyroMatrix = Matrix3::getRotationVec(n.x, n.y, n.z, mag).transposed() * curGyroMatrix;
|
||
// }
|
||
|
||
// DEBUG PLOT
|
||
|
||
|
||
// update old reading
|
||
lastGyroReading = vec;
|
||
|
||
update(ts);
|
||
|
||
|
||
|
||
}
|
||
|
||
Matrix3 getMatrixGyro() const {
|
||
return Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z);
|
||
}
|
||
|
||
Matrix3 getMatrixAcc() const {
|
||
return Matrix3::getRotationRad(curAccel_rad.x, curAccel_rad.y, curAccel_rad.z);
|
||
}
|
||
|
||
private:
|
||
|
||
Vector3 curAngle_rad;
|
||
int cnt = 0;
|
||
|
||
void update(const Timestamp ts) {
|
||
|
||
|
||
// complementary filter x = alpha * x + (1-alpha) * y;
|
||
const float alpha = 0.98f;
|
||
//curAngle_rad = curAngle_rad*alpha + curAccel_rad*(1-alpha);
|
||
//curAngle_rad = curAccel_rad;
|
||
//curAngle_rad = curGyroSum_rad;
|
||
|
||
//std::cout << curGyroSum_rad.x <<" " << curGyroSum_rad.y << " " << curGyroSum_rad.z << std::endl;
|
||
|
||
//curAngle_rad = filter.get();
|
||
|
||
filter.addSlow(ts, curAccel_rad);
|
||
filter.addFast(ts, curGyroSum_rad);
|
||
|
||
static Vector3 fused;
|
||
static Vector3 pref;
|
||
Vector3 diff = curGyroSum_rad - pref;
|
||
fused = (fused+diff) * alpha + curAccel_rad * (1-alpha);
|
||
pref = curGyroSum_rad;
|
||
|
||
// update
|
||
//orientation.rotationMatrix = Matrix3::getRotationRad(curAccel_rad.x, curAccel_rad.y, curAccel_rad.z); //getMatrixFor(cur);
|
||
//orientation.rotationMatrix = Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z); //getMatrixFor(cur);
|
||
//orientation.rotationMatrix = curGyroMatrix;
|
||
//orientation.rotationMatrix = Matrix3::getRotationRadZ(curGyroSum_rad.z) * Matrix3::getRotationRadX(curGyroSum_rad.x) * Matrix3::getRotationRadY(curGyroSum_rad.y);
|
||
//orientation.rotationMatrix = Matrix3::getRotationRad(curGyroSum_rad.x, curGyroSum_rad.y, curGyroSum_rad.z);
|
||
orientation.rotationMatrix = Matrix3::getRotationRad(fused.x, fused.y, 0); //getMatrixFor(cur);
|
||
|
||
|
||
orientation.isKnown = true;
|
||
orientation.lastEstimation = ts;
|
||
|
||
// debug-plot (if configured)
|
||
#ifdef WITH_DEBUG_PLOT
|
||
plot.add(ts, curAccel, orientation.rotationMatrix);
|
||
if (++cnt % 2 == 0) {
|
||
plotAngles.addAcc(ts, curAccel_rad.x, curAccel_rad.y);
|
||
plotAngles.addGyro(ts, curGyroSum_rad.x, curGyroSum_rad.y);
|
||
plotAngles.addFused(ts, fused.x, fused.y);
|
||
}
|
||
#endif
|
||
|
||
}
|
||
|
||
// /** get the current rotation matrix estimation */
|
||
// Matrix3 getMatrixFor(const Vector3 cur) const {
|
||
|
||
// // rotate average-accelerometer into (0,0,1)
|
||
// //Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
|
||
// const Vector3 zAxis(0,0,1);
|
||
// const Matrix3 rotMat = getRotationMatrix(cur.normalized(), zAxis);
|
||
// //const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized()); // INVERSE
|
||
// //const Eigen::Matrix3f rotMat = getRotationMatrix(avg.normalized(), zAxis);
|
||
|
||
// // just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
|
||
// //Eigen::Vector3f aligned = (rotMat * avg).normalized();
|
||
// const Vector3 aligned = (rotMat * cur).normalized();
|
||
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
||
|
||
// return rotMat;
|
||
|
||
// }
|
||
|
||
// /** get a matrix that rotates the vector "from" into the vector "to" */
|
||
// static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 to) {
|
||
|
||
// // http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
|
||
|
||
// const Vector3 v = from.cross(to) / from.cross(to).norm();
|
||
// const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
|
||
|
||
// Matrix3 A({
|
||
// 0.0f, -v.z, v.y,
|
||
// v.z, 0.0f, -v.x,
|
||
// -v.y, v.x, 0.0f
|
||
// });
|
||
|
||
// return Matrix3::identity() + (A * std::sin(angle)) + ((A*A) * (1-std::cos(angle)));
|
||
|
||
// }
|
||
|
||
};
|
||
|
||
#endif // POSEDETECTION2_H
|