frank
284c6b11a6
added new helper class for 3x3 matrices and vec3 added magnetometer data added compass detection refactored pose-estimation (single class) refactored debug plots (move to own class) minor changes
481 lines
12 KiB
C++
481 lines
12 KiB
C++
#ifndef INDOOR_IMU_POSEDETECTION_H
|
|
#define INDOOR_IMU_POSEDETECTION_H
|
|
|
|
#include "AccelerometerData.h"
|
|
|
|
#include "../../data/Timestamp.h"
|
|
|
|
#include "../../math/MovingAverageTS.h"
|
|
#include "../../math/MovingMedianTS.h"
|
|
#include "../../math/Matrix3.h"
|
|
|
|
#include "../../geo/Point3.h"
|
|
|
|
#include <eigen3/Eigen/Dense>
|
|
|
|
#include "PoseDetectionPlot.h"
|
|
|
|
/**
|
|
* estimate the smartphones world-pose,
|
|
* based on the accelerometer's data
|
|
*/
|
|
class PoseDetection {
|
|
|
|
// struct LongTermTriggerAverage {
|
|
|
|
// Eigen::Vector3f sum;
|
|
// int cnt;
|
|
|
|
// XYZ() {
|
|
// reset();
|
|
// }
|
|
|
|
// /** add the given accelerometer reading */
|
|
// void addAcc(const Timestamp ts, const AccelerometerData& acc) {
|
|
|
|
// // did NOT improve the result for every smartphone (only some)
|
|
// //const float deltaMag = std::abs(acc.magnitude() - 9.81);
|
|
// //if (deltaMag > 5.0) {return;}
|
|
|
|
// // adjust sum and count (for average calculation)
|
|
// Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z;
|
|
// sum += vec;
|
|
// ++cnt;
|
|
|
|
|
|
// }
|
|
|
|
// AccelerometerData getAvg() const {
|
|
// return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
|
|
// }
|
|
|
|
// /** get the current rotation matrix estimation */
|
|
// Eigen::Matrix3f get() const {
|
|
|
|
// // get the current acceleromter average
|
|
// const Eigen::Vector3f avg = sum / cnt;
|
|
|
|
// // rotate average accelerometer into (0,0,1)
|
|
// Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
|
|
// 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();
|
|
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
|
|
|
// return rotMat;
|
|
|
|
// }
|
|
|
|
// /** reset the current sum etc. */
|
|
// void reset() {
|
|
// cnt = 0;
|
|
// sum = Eigen::Vector3f::Zero();
|
|
// }
|
|
|
|
|
|
// };
|
|
|
|
|
|
/** live-pose-estimation using moving average of the smartphone's accelerometer */
|
|
struct EstMovingAverage {
|
|
|
|
// average the accelerometer
|
|
MovingAverageTS<AccelerometerData> avg;
|
|
|
|
EstMovingAverage(const Timestamp window) :
|
|
avg(MovingAverageTS<AccelerometerData>(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::isTrue((aligned-zAxis).norm() < 0.1f, "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 {
|
|
//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(300));
|
|
//EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(400));
|
|
|
|
#ifdef WITH_DEBUG_PLOT
|
|
PoseDetectionPlot plot;
|
|
#endif
|
|
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
PoseDetection() {
|
|
;
|
|
}
|
|
|
|
// /** get the smartphone's rotation matrix */
|
|
// Eigen::Matrix3f getMatrix() const {
|
|
// return orientation.rotationMatrix;
|
|
// }
|
|
|
|
/** get the smartphone's rotation matrix */
|
|
const Matrix3& getMatrix() const {
|
|
return orientation.rotationMatrix;
|
|
}
|
|
|
|
/** is the pose known and stable? */
|
|
bool isKnown() const {
|
|
return orientation.isKnown;
|
|
}
|
|
|
|
void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
|
|
|
|
// 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
|
|
plot.add(ts, est.getBase(), orientation.rotationMatrix);
|
|
#endif
|
|
|
|
}
|
|
|
|
public:
|
|
|
|
// /** get a matrix that rotates the vector "from" into the vector "to" */
|
|
// static Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) {
|
|
|
|
// // http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
|
|
|
|
// const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm();
|
|
// const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
|
|
|
|
// Eigen::Matrix3f A; A <<
|
|
// 0, -x(2), x(1),
|
|
// x(2), 0, -x(0),
|
|
// -x(1), x(0), 0;
|
|
|
|
// return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A));
|
|
|
|
// }
|
|
|
|
/** 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)));
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// /** get a rotation matrix for the given x,y,z rotation (in radians) */
|
|
// static Eigen::Matrix3f getRotation(const float x, const float y, const float z) {
|
|
// const float g = x; const float b = y; const float a = z;
|
|
// const float a11 = std::cos(a)*std::cos(b);
|
|
// const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
|
|
// const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
|
|
// const float a21 = std::sin(a)*std::cos(b);
|
|
// const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
|
|
// const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
|
|
// const float a31 = -std::sin(b);
|
|
// const float a32 = std::cos(b)*std::sin(g);
|
|
// const float a33 = std::cos(b)*std::cos(g);
|
|
// Eigen::Matrix3f m;
|
|
// m <<
|
|
// a11, a12, a13,
|
|
// a21, a22, a23,
|
|
// a31, a32, a33;
|
|
// ;
|
|
// return m;
|
|
// }
|
|
|
|
// /** estimate the smartphones current holding position */
|
|
// void estimateHolding2() {
|
|
|
|
|
|
// // z-axis points through the device and is the axis we are interested in
|
|
// // http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics
|
|
|
|
// avgAcc = Eigen::Vector3f::Zero();
|
|
|
|
// for (const AccelerometerData& acc : accData) {
|
|
// //for (const GyroscopeData& acc : gyroData) {
|
|
// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
|
|
// // Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
|
|
// avgAcc += vec;
|
|
// }
|
|
|
|
// //avgAcc /= accData.size();
|
|
// avgAcc = avgAcc.normalized();
|
|
|
|
// Eigen::Vector3f rev; rev << 0,0,1;
|
|
|
|
// rotMat = getRotationMatrix(avgAcc, rev);
|
|
|
|
|
|
// //Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis");
|
|
// //Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis");
|
|
//// Eigen::Vector3f re = rotMat * avgAcc;
|
|
//// Eigen::Vector3f diff = rev-re;
|
|
//// Assert::isTrue(diff.norm() < 0.001, "rotation error");
|
|
|
|
// }
|
|
|
|
// struct RotationMatrixEstimationUsingAccAngle {
|
|
|
|
// Eigen::Vector3f lastAvg;
|
|
// Eigen::Vector3f avg;
|
|
// int cnt;
|
|
|
|
// RotationMatrixEstimationUsingAccAngle() {
|
|
// reset();
|
|
// }
|
|
|
|
// void add(const float x, const float y, const float z) {
|
|
|
|
// Eigen::Vector3f vec; vec << x,y,z;
|
|
// avg += vec;
|
|
// ++cnt;
|
|
|
|
// }
|
|
|
|
// void reset() {
|
|
// cnt = 0;
|
|
// avg = Eigen::Vector3f::Zero();
|
|
// }
|
|
|
|
// Eigen::Matrix3f get() {
|
|
|
|
// // http://www.hobbytronics.co.uk/accelerometer-info
|
|
|
|
// avg /= cnt;
|
|
// lastAvg = avg;
|
|
|
|
// //const float mag = avg.norm();
|
|
|
|
// const float baseX = 0;
|
|
// const float baseY = 0;
|
|
// const float baseZ = 0; // mag?
|
|
|
|
// const float x = avg(0) - baseX;
|
|
// const float y = avg(1) - baseY;
|
|
// const float z = avg(2) - baseZ;
|
|
|
|
// const float ax = std::atan( x / (std::sqrt(y*y + z*z)) );
|
|
// const float ay = std::atan( y / (std::sqrt(x*x + z*z)) );
|
|
|
|
// const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0); // TODO -ax or +ax?
|
|
|
|
// // sanity check
|
|
// Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
|
|
// Eigen::Vector3f aligned = (rotMat * avg).normalized();
|
|
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
|
// // int i = 0; (void) i;
|
|
|
|
// reset();
|
|
// return rotMat;
|
|
|
|
// }
|
|
|
|
// } est;
|
|
|
|
// struct PCA {
|
|
|
|
// Eigen::Vector3f avg;
|
|
// Eigen::Vector3f lastAvg;
|
|
// Eigen::Matrix3f covar;
|
|
// int cnt = 0;
|
|
|
|
// PCA() {
|
|
// reset();
|
|
// }
|
|
|
|
// void add(const float x, const float y, const float z) {
|
|
|
|
// Eigen::Vector3f vec; vec << x,y,z;
|
|
// avg += vec;
|
|
// covar += vec*vec.transpose();
|
|
// ++cnt;
|
|
|
|
// }
|
|
|
|
// Eigen::Matrix3f get() {
|
|
|
|
// avg /= cnt;
|
|
// covar /= cnt;
|
|
// lastAvg = avg;
|
|
|
|
// std::cout << avg << std::endl;
|
|
|
|
// Eigen::Matrix3f Q = covar;// - avg*avg.transpose();
|
|
// for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));}
|
|
|
|
// Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(Q);
|
|
// solver.eigenvalues();
|
|
// solver.eigenvectors();
|
|
|
|
// const auto eval = solver.eigenvalues();
|
|
// const auto evec = solver.eigenvectors();
|
|
// Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!");
|
|
|
|
// Eigen::Matrix3f rotMat;
|
|
// rotMat.col(0) = evec.col(0);
|
|
// rotMat.col(1) = evec.col(1);
|
|
// rotMat.col(2) = evec.col(2); // 0,0,1 (z-axis) belongs to the strongest eigenvalue
|
|
// rotMat.transposeInPlace();
|
|
|
|
// //Eigen::Vector3f gy; gy << 0, 30, 30;
|
|
// Eigen::Vector3f avg1 = rotMat * avg;
|
|
// int i = 0; (void) i;
|
|
|
|
// reset();
|
|
|
|
// return rotMat;
|
|
|
|
// }
|
|
|
|
// void reset() {
|
|
// cnt = 0;
|
|
// avg = Eigen::Vector3f::Zero();
|
|
// covar = Eigen::Matrix3f::Zero();
|
|
// }
|
|
|
|
|
|
// } pca1;
|
|
|
|
|
|
|
|
|
|
// /** estimate the smartphones current holding position */
|
|
// void estimateHolding() {
|
|
|
|
// Eigen::Vector3f avg = Eigen::Vector3f::Zero();
|
|
// Eigen::Matrix3f covar = Eigen::Matrix3f::Zero();
|
|
|
|
// for (const AccelerometerData& acc : accData) {
|
|
//// for (const GyroscopeData& acc : gyroData) {
|
|
// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
|
|
//// Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z);
|
|
// avg += vec;
|
|
// covar += vec * vec.transpose();
|
|
// }
|
|
|
|
// avg /= accData.size(); // TODO
|
|
// covar /= accData.size(); //TODO
|
|
|
|
// avgAcc = avg.normalized();
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // INDOOR_IMU_POSEDETECTION_H
|