300 lines
8.3 KiB
C++
300 lines
8.3 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 INDOOR_IMU_POSEDETECTION_H
|
||
#define INDOOR_IMU_POSEDETECTION_H
|
||
|
||
#include "AccelerometerData.h"
|
||
|
||
#include "../../data/Timestamp.h"
|
||
|
||
#include "../../math/MovingStdDevTS.h"
|
||
#include "../../math/MovingAverageTS.h"
|
||
#include "../../math/MovingMedianTS.h"
|
||
#include "../../math/Matrix3.h"
|
||
|
||
#include "../../geo/Point3.h"
|
||
|
||
//#include <eigen3/Eigen/Dense>
|
||
|
||
#include "PoseDetectionPlot.h"
|
||
#include "PoseProvider.h"
|
||
|
||
/**
|
||
* estimate the smartphones world-pose,
|
||
* based on the accelerometer's data
|
||
*/
|
||
class PoseDetection : public PoseProvider {
|
||
|
||
|
||
/** live-pose-estimation using moving average of the smartphone's accelerometer */
|
||
struct EstMovingAverage {
|
||
|
||
// average the accelerometer
|
||
MovingAverageTS<AccelerometerData> avg;
|
||
|
||
EstMovingAverage(const Timestamp window) :
|
||
avg(window, AccelerometerData()) {
|
||
|
||
// start approximately
|
||
addAcc(Timestamp(), AccelerometerData(0,0,9.81));
|
||
|
||
}
|
||
|
||
/** add the given accelerometer reading */
|
||
void addAcc(const Timestamp ts, const AccelerometerData& acc) {
|
||
avg.add(ts, acc);
|
||
}
|
||
|
||
AccelerometerData getBase() const {
|
||
return avg.get();
|
||
}
|
||
|
||
// /** get the current rotation matrix estimation */
|
||
// //Eigen::Matrix3f get() const {
|
||
// Matrix3 get() const {
|
||
|
||
// // get the current acceleromter average
|
||
// const AccelerometerData avgAcc = avg.get();
|
||
// //const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
|
||
// const Vector3 avg(avgAcc.x, avgAcc.y, avgAcc.z);
|
||
|
||
// // rotate average-accelerometer into (0,0,1)
|
||
// //Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
|
||
// const Vector3 zAxis(0,0,1);
|
||
// const Matrix3 rotMat = getRotationMatrix(avg.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 * avg).normalized();
|
||
// Assert::isBetween(aligned.norm(), 0.95f, 1.05f, "result distorted");
|
||
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
||
|
||
// return rotMat;
|
||
|
||
// }
|
||
|
||
// FOR TESTING
|
||
/** get the current rotation matrix estimation */
|
||
//Eigen::Matrix3f get() const {
|
||
Matrix3 get() const {
|
||
|
||
// https://stackoverflow.com/questions/18558910/direction-vector-to-rotation-matrix
|
||
// get the current acceleromter average
|
||
const AccelerometerData avgAcc = avg.get();
|
||
//const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
|
||
const Point3 vZ = Point3(avgAcc.x, avgAcc.y, avgAcc.z).normalized();
|
||
const Point3 vX = cross(Point3(0,1,0), vZ).normalized();//Point3(avgAcc.z, -avgAcc.y, avgAcc.x);
|
||
//const Point3 v2 = cross(v3, vx).normalized();
|
||
const Point3 vY = cross(vZ, vX).normalized();
|
||
|
||
Matrix3 rotMat({
|
||
vX.x, vY.x, vZ.x,
|
||
vX.y, vY.y, vZ.y,
|
||
vX.z, vY.z, vZ.z,
|
||
});
|
||
|
||
// above transposed = inverse matrix = undo rotation
|
||
rotMat = rotMat.transposed();
|
||
|
||
// // https://stackoverflow.com/questions/18558910/direction-vector-to-rotation-matrix
|
||
// // get the current acceleromter average
|
||
// const AccelerometerData avgAcc = avg.get();
|
||
// //const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
|
||
// const Point3 vZ = Point3(-avgAcc.x, -avgAcc.y, -avgAcc.z).normalized();
|
||
// const Point3 vX = cross(vZ, Point3(0,1,0)).normalized();//Point3(avgAcc.z, -avgAcc.y, avgAcc.x);
|
||
// //const Point3 v2 = cross(v3, vx).normalized();
|
||
// const Point3 vY = cross(vX, vZ).normalized();
|
||
|
||
// Matrix3 rotMat({
|
||
// vX.x, vY.x, vZ.x,
|
||
// vX.y, vY.y, vZ.y,
|
||
// vX.z, vY.z, vZ.z,
|
||
// });
|
||
|
||
// //rotMat = Matrix3::getRotationDeg(180, 0, 0) * rotMat;
|
||
// //rotMat = Matrix3::getRotationDeg(0, 0, 180) * rotMat;
|
||
|
||
// // above transposed = inverse matrix = undo rotation
|
||
// //rotMat = rotMat.transposed();
|
||
|
||
// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
|
||
const Vector3 zAxis(0,0,1);
|
||
const Vector3 inp(avgAcc.x, avgAcc.y, avgAcc.z);
|
||
const Vector3 aligned = (rotMat * inp).normalized();
|
||
Assert::isBetween(aligned.norm(), 0.95f, 1.05f, "result distorted");
|
||
//Assert::isTrue((aligned-zAxis).norm() < 0.10f, "deviation too high");
|
||
|
||
return rotMat;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
// /** live-pose-estimation using moving median of the smartphone's accelerometer */
|
||
// struct EstMovingMedian {
|
||
|
||
// // median the accelerometer
|
||
// MovingMedianTS<float> medianX;
|
||
// MovingMedianTS<float> medianY;
|
||
// MovingMedianTS<float> medianZ;
|
||
|
||
// EstMovingMedian(const Timestamp window) :
|
||
// medianX(window), medianY(window), medianZ(window) {
|
||
|
||
// // start approximately
|
||
// addAcc(Timestamp(), AccelerometerData(0,0,9.81));
|
||
|
||
// }
|
||
|
||
// /** add the given accelerometer reading */
|
||
// void addAcc(const Timestamp ts, const AccelerometerData& acc) {
|
||
// medianX.add(ts, acc.x);
|
||
// medianY.add(ts, acc.y);
|
||
// medianZ.add(ts, acc.z);
|
||
// }
|
||
|
||
// AccelerometerData getBase() const {
|
||
// return AccelerometerData(medianX.get(), medianY.get(), medianZ.get());
|
||
// }
|
||
|
||
// /** get the current rotation matrix estimation */
|
||
// //Eigen::Matrix3f get() const {
|
||
// Matrix3 get() const {
|
||
|
||
// const Vector3 base(medianX.get(), medianY.get(), medianZ.get());
|
||
|
||
// // rotate average-accelerometer into (0,0,1)
|
||
// const Vector3 zAxis(0,0,1);
|
||
// const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis);
|
||
|
||
// // just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
|
||
// const Vector3 aligned = (rotMat * base).normalized();
|
||
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
||
|
||
// return rotMat;
|
||
|
||
// }
|
||
|
||
// };
|
||
|
||
|
||
private:
|
||
|
||
struct {
|
||
Matrix3 rotationMatrix = Matrix3::identity();
|
||
float curSigma = 0;
|
||
bool isKnown = false;
|
||
Timestamp lastEstimation;
|
||
} orientation;
|
||
|
||
/** how the pose is estimated */
|
||
//LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250));
|
||
EstMovingAverage est;
|
||
//EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(300));
|
||
|
||
MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
int plotLimit = 0;
|
||
PoseDetectionPlot plot;
|
||
#endif
|
||
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
PoseDetection(const Timestamp delay = Timestamp::fromMS(450)) : est(delay) {
|
||
#ifdef WITH_DEBUG_PLOT
|
||
plot.setName("PoseDetection1");
|
||
#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;
|
||
}
|
||
|
||
Matrix3 getMatrixGyro() const {
|
||
return Matrix3::identity();
|
||
}
|
||
|
||
Matrix3 getMatrixAcc() const {
|
||
return orientation.rotationMatrix;
|
||
}
|
||
|
||
/** current uncertainty */
|
||
float getSigma() const {
|
||
return orientation.curSigma;
|
||
}
|
||
|
||
|
||
void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
|
||
|
||
// uncertainty
|
||
const Vector3 curAcc = Vector3(acc.x, acc.y, acc.z);
|
||
float angleDiff = std::acos(curAcc.normalized().dot(Vector3(0,0,1)));
|
||
if (!std::isnan(angleDiff)) {
|
||
stdDevForSigma.add(ts, angleDiff);
|
||
orientation.curSigma = stdDevForSigma.get()*1;
|
||
}
|
||
|
||
// add accelerometer data
|
||
est.addAcc(ts, acc);
|
||
|
||
// update (if needed)
|
||
orientation.rotationMatrix = est.get();
|
||
orientation.isKnown = true;
|
||
orientation.lastEstimation = ts;
|
||
|
||
// debug-plot (if configured)
|
||
#ifdef WITH_DEBUG_PLOT
|
||
if (++plotLimit > 5) {
|
||
plot.add(ts, est.getBase(), orientation.rotationMatrix);
|
||
plotLimit = 0;
|
||
}
|
||
#endif
|
||
|
||
}
|
||
|
||
public:
|
||
|
||
|
||
/** 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 // INDOOR_IMU_POSEDETECTION_H
|