fixed some issues
added new pose/turn detections new helper classes define-flags for libEigen
This commit is contained in:
@@ -5,6 +5,7 @@
|
||||
|
||||
#include "../../data/Timestamp.h"
|
||||
|
||||
#include "../../math/MovingStdDevTS.h"
|
||||
#include "../../math/MovingAverageTS.h"
|
||||
#include "../../math/MovingMedianTS.h"
|
||||
#include "../../math/Matrix3.h"
|
||||
@@ -14,12 +15,13 @@
|
||||
//#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "PoseDetectionPlot.h"
|
||||
#include "PoseProvider.h"
|
||||
|
||||
/**
|
||||
* estimate the smartphones world-pose,
|
||||
* based on the accelerometer's data
|
||||
*/
|
||||
class PoseDetection {
|
||||
class PoseDetection : public PoseProvider {
|
||||
|
||||
|
||||
/** live-pose-estimation using moving average of the smartphone's accelerometer */
|
||||
@@ -29,7 +31,7 @@ class PoseDetection {
|
||||
MovingAverageTS<AccelerometerData> avg;
|
||||
|
||||
EstMovingAverage(const Timestamp window) :
|
||||
avg(MovingAverageTS<AccelerometerData>(window, AccelerometerData())) {
|
||||
avg(window, AccelerometerData()) {
|
||||
|
||||
// start approximately
|
||||
addAcc(Timestamp(), AccelerometerData(0,0,9.81));
|
||||
@@ -45,26 +47,82 @@ class PoseDetection {
|
||||
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 Vector3 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();
|
||||
|
||||
// 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);
|
||||
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)
|
||||
//Eigen::Vector3f aligned = (rotMat * avg).normalized();
|
||||
const Vector3 aligned = (rotMat * avg).normalized();
|
||||
Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
||||
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;
|
||||
|
||||
@@ -120,22 +178,24 @@ class PoseDetection {
|
||||
// };
|
||||
|
||||
|
||||
|
||||
private:
|
||||
|
||||
struct {
|
||||
//Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
|
||||
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 = EstMovingAverage(Timestamp::fromMS(450));
|
||||
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
|
||||
|
||||
@@ -143,22 +203,46 @@ private:
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
PoseDetection() {
|
||||
;
|
||||
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 {
|
||||
const Matrix3& getMatrix() const override {
|
||||
return orientation.rotationMatrix;
|
||||
}
|
||||
|
||||
/** is the pose known and stable? */
|
||||
bool isKnown() const {
|
||||
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);
|
||||
|
||||
@@ -169,7 +253,10 @@ public:
|
||||
|
||||
// debug-plot (if configured)
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
plot.add(ts, est.getBase(), orientation.rotationMatrix);
|
||||
if (++plotLimit > 5) {
|
||||
plot.add(ts, est.getBase(), orientation.rotationMatrix);
|
||||
plotLimit = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user