many changes
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
This commit is contained in:
@@ -5,23 +5,18 @@
|
||||
#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>
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
#include <KLib/misc/gnuplot/Gnuplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotPlot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
|
||||
#endif
|
||||
#include "TurnDetectionPlot.h"
|
||||
|
||||
|
||||
#include "../../Assertions.h"
|
||||
|
||||
@@ -29,98 +24,29 @@ class TurnDetection {
|
||||
|
||||
private:
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
Timestamp plotRef;
|
||||
Timestamp lastPlot;
|
||||
|
||||
K::Gnuplot gp1;
|
||||
K::Gnuplot gp2;
|
||||
|
||||
K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,3);
|
||||
|
||||
K::GnuplotPlot plotGyroRaw;
|
||||
K::GnuplotPlotElementLines lineGyroRawX;
|
||||
K::GnuplotPlotElementLines lineGyroRawY;
|
||||
K::GnuplotPlotElementLines lineGyroRawZ;
|
||||
|
||||
K::GnuplotPlot plotGyroFix;
|
||||
K::GnuplotPlotElementLines lineGyroFixX;
|
||||
K::GnuplotPlotElementLines lineGyroFixY;
|
||||
K::GnuplotPlotElementLines lineGyroFixZ;
|
||||
|
||||
K::GnuplotPlot plotAcc;
|
||||
K::GnuplotPlotElementLines lineAccX;
|
||||
K::GnuplotPlotElementLines lineAccY;
|
||||
K::GnuplotPlotElementLines lineAccZ;
|
||||
|
||||
K::GnuplotSplot plotPose;
|
||||
K::GnuplotSplotElementLines linePose;
|
||||
|
||||
float plotCurHead = 0;
|
||||
|
||||
#endif
|
||||
|
||||
PoseDetection* pose = nullptr;
|
||||
|
||||
//std::vector<GyroscopeData> gyroData;
|
||||
Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
|
||||
//Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
|
||||
Vector3 prevGyro = Vector3(0,0,0);
|
||||
|
||||
Timestamp lastGyroReading;
|
||||
|
||||
|
||||
struct {
|
||||
Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
|
||||
bool isKnown = false;
|
||||
Timestamp lastEstimation;
|
||||
} orientation;
|
||||
|
||||
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
TurnDetectionPlot plot;
|
||||
#endif
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
TurnDetection() {
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
|
||||
gp1 << "set autoscale xfix\n";
|
||||
gp1 << "set view equal xyz\n";
|
||||
|
||||
multiplot.add(&plotGyroRaw);
|
||||
multiplot.add(&plotGyroFix);
|
||||
multiplot.add(&plotAcc);
|
||||
|
||||
plotGyroRaw.setTitle("Gyroscope (raw)");
|
||||
plotGyroRaw.add(&lineGyroRawX); lineGyroRawX.getStroke().getColor().setHexStr("#ff0000"); lineGyroRawX.setTitle("gyroX");
|
||||
plotGyroRaw.add(&lineGyroRawY); lineGyroRawY.getStroke().getColor().setHexStr("#00ff00"); lineGyroRawY.setTitle("gyroY");
|
||||
plotGyroRaw.add(&lineGyroRawZ); lineGyroRawZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroRawZ.setTitle("gyroZ");
|
||||
|
||||
plotGyroFix.setTitle("Gyroscope (fixed)");
|
||||
plotGyroFix.add(&lineGyroFixX); lineGyroFixX.getStroke().getColor().setHexStr("#ff0000"); lineGyroFixX.setTitle("gyroX");
|
||||
plotGyroFix.add(&lineGyroFixY); lineGyroFixY.getStroke().getColor().setHexStr("#00ff00"); lineGyroFixY.setTitle("gyroY");
|
||||
plotGyroFix.add(&lineGyroFixZ); lineGyroFixZ.getStroke().getColor().setHexStr("#0000ff"); lineGyroFixZ.setTitle("gyroZ");
|
||||
|
||||
plotAcc.setTitle("Accelerometer");
|
||||
plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX");
|
||||
plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY");
|
||||
plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ");
|
||||
|
||||
plotPose.setTitle("Pose");
|
||||
plotPose.getView().setEnabled(false);
|
||||
plotPose.add(&linePose);
|
||||
plotPose.getAxisX().setRange(-5,+5);
|
||||
plotPose.getAxisY().setRange(-5,+5);
|
||||
plotPose.getAxisZ().setRange(-5,+5);
|
||||
|
||||
#endif
|
||||
|
||||
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()) {
|
||||
@@ -139,12 +65,8 @@ public:
|
||||
// } 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;}
|
||||
|
||||
@@ -157,17 +79,20 @@ public:
|
||||
|
||||
// ignore readings until the first orientation-estimation is available
|
||||
// otherwise we would use a wrong rotation matrix which yields wrong results!
|
||||
if (!orientation.isKnown) {return 0.0f;}
|
||||
if (!pose->isKnown()) {return 0.0f;}
|
||||
|
||||
// get the current gyro-reading as vector
|
||||
Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
|
||||
//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;
|
||||
//Eigen::Vector3f curGyro = orientation.rotationMatrix * vec;
|
||||
const Vector3 curGyro = pose->getMatrix() * vec;
|
||||
//driftEst.removeDrift(ts, curGyro);
|
||||
|
||||
// area
|
||||
const Eigen::Vector3f area =
|
||||
//const Eigen::Vector3f area =
|
||||
const Vector3 area =
|
||||
|
||||
// Trapezoid rule (should be more accurate but does not always help?!)
|
||||
//(prevGyro * curDiff.sec()) + // squared region
|
||||
@@ -183,485 +108,18 @@ public:
|
||||
prevGyro = curGyro;
|
||||
|
||||
// rotation = z-axis only!
|
||||
const float delta = area(2);
|
||||
|
||||
//const float delta = area(2);
|
||||
const float delta = area.z;
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
|
||||
plotCurHead += delta;
|
||||
|
||||
if (plotRef.isZero()) {plotRef = ts;}
|
||||
const Timestamp tsPlot = (ts-plotRef);
|
||||
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
|
||||
|
||||
// raw gyro
|
||||
lineGyroRawX.add( K::GnuplotPoint2(tsPlot.ms(), gyro.x) );
|
||||
lineGyroRawY.add( K::GnuplotPoint2(tsPlot.ms(), gyro.y) );
|
||||
lineGyroRawZ.add( K::GnuplotPoint2(tsPlot.ms(), gyro.z) );
|
||||
|
||||
// adjusted gyro
|
||||
lineGyroFixX.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(0)) );
|
||||
lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(1)) );
|
||||
lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curGyro(2)) );
|
||||
|
||||
// adjusted gyro
|
||||
lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().x) );
|
||||
lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().y) );
|
||||
lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), est.getAvg().z) );
|
||||
|
||||
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
|
||||
|
||||
lastPlot = tsPlot;
|
||||
|
||||
// plot 3D pose
|
||||
std::vector<Point3> pose = {
|
||||
Point3(-1, -2, -0.1), Point3(+1, -2, -0.1), Point3(+1, +2, -0.1), Point3(-1, +2, -0.1),
|
||||
Point3(-1, -2, +0.1), Point3(+1, -2, +0.1), Point3(+1, +2, +0.1), Point3(-1, +2, +0.1),
|
||||
};
|
||||
linePose.clear();
|
||||
for (const Point3 p : pose) {
|
||||
Eigen::Vector3f vec1; vec1 << p.x, p.y, p.z;
|
||||
Eigen::Vector3f vec2 = orientation.rotationMatrix * vec1;
|
||||
K::GnuplotPoint3 gp3(vec2(0), vec2(1), vec2(2));
|
||||
linePose.add(gp3);
|
||||
}
|
||||
|
||||
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
|
||||
lineGyroRawX.removeIf(remove);
|
||||
lineGyroRawY.removeIf(remove);
|
||||
lineGyroRawZ.removeIf(remove);
|
||||
lineGyroFixX.removeIf(remove);
|
||||
lineGyroFixY.removeIf(remove);
|
||||
lineGyroFixZ.removeIf(remove);
|
||||
lineAccX.removeIf(remove);
|
||||
lineAccY.removeIf(remove);
|
||||
lineAccZ.removeIf(remove);
|
||||
|
||||
const float ax = 0.85 + std::cos(plotCurHead)*0.1;
|
||||
const float ay = 0.85 + std::sin(plotCurHead)*0.1;
|
||||
gp1 << "set arrow 1 from screen 0.85,0.85 to screen " << ax << "," << ay << "\n";
|
||||
gp1 << "set object 2 circle at screen 0.85,0.85 radius screen 0.1 \n";
|
||||
|
||||
|
||||
gp1.draw(multiplot);
|
||||
gp1.flush();
|
||||
|
||||
gp2.draw(plotPose);
|
||||
gp2.flush();
|
||||
|
||||
//usleep(100);
|
||||
|
||||
}
|
||||
|
||||
plot.add(ts, delta, gyro, curGyro);
|
||||
#endif
|
||||
|
||||
|
||||
// done
|
||||
return delta;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
|
||||
|
||||
// add accelerometer data
|
||||
//pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z));
|
||||
est.addAcc(ts, acc);
|
||||
|
||||
if (1 == 0) {
|
||||
|
||||
// FASTER
|
||||
|
||||
// start with the first available timestamp
|
||||
if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;}
|
||||
|
||||
// if we have at-least 500 ms of acc-data, re-calculate the current smartphone holding
|
||||
if (ts - orientation.lastEstimation > Timestamp::fromMS(1500)) {
|
||||
orientation.rotationMatrix = est.get();
|
||||
orientation.isKnown = true;
|
||||
orientation.lastEstimation = ts;
|
||||
est.reset();
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
// MORE ACCURATE
|
||||
|
||||
orientation.rotationMatrix = est.get();
|
||||
orientation.isKnown = true;
|
||||
orientation.lastEstimation = ts;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
// /** 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");
|
||||
|
||||
// }
|
||||
|
||||
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));
|
||||
|
||||
}
|
||||
|
||||
|
||||
struct XYZ {
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
struct XYZ2 {
|
||||
|
||||
// average the accelerometer
|
||||
MovingAverageTS<AccelerometerData> avg = MovingAverageTS<AccelerometerData>(Timestamp::fromMS(1250), AccelerometerData());
|
||||
|
||||
XYZ2() {
|
||||
|
||||
// start approximately
|
||||
addAcc(Timestamp(), AccelerometerData(0,0,9.81));
|
||||
|
||||
}
|
||||
|
||||
/** 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;}
|
||||
|
||||
avg.add(ts, acc);
|
||||
|
||||
}
|
||||
|
||||
AccelerometerData getAvg() const {
|
||||
return avg.get();
|
||||
}
|
||||
|
||||
/** get the current rotation matrix estimation */
|
||||
Eigen::Matrix3f get() const {
|
||||
|
||||
// get the current acceleromter average
|
||||
AccelerometerData avgAcc = getAvg();
|
||||
const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
|
||||
|
||||
// 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() {
|
||||
;
|
||||
}
|
||||
|
||||
} est;
|
||||
|
||||
|
||||
|
||||
// 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;
|
||||
|
||||
|
||||
/** 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;
|
||||
}
|
||||
|
||||
|
||||
// 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();
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//// static K::Gnuplot gp;
|
||||
//// gp << "set view equal xyz\n";
|
||||
//// gp << "set xrange[-1:+1]\n";
|
||||
//// gp << "set yrange[-1:+1]\n";
|
||||
//// gp << "set zrange[-1:+1]\n";
|
||||
|
||||
//// K::GnuplotSplot plot;
|
||||
//// K::GnuplotSplotElementLines lines; plot.add(&lines);
|
||||
|
||||
//// K::GnuplotPoint3 p0(0,0,0);
|
||||
//// K::GnuplotPoint3 px(evec(0,0), evec(1,0), evec(2,0)); //px = px * eval(0);
|
||||
//// K::GnuplotPoint3 py(evec(0,1), evec(1,1), evec(2,1)); //py = py * eval(1);
|
||||
//// K::GnuplotPoint3 pz(evec(0,2), evec(1,2), evec(2,2)); //pz = pz * eval(2);
|
||||
|
||||
//// K::GnuplotPoint3 pa(avg(0), avg(1), avg(2));
|
||||
|
||||
//// lines.addSegment(p0, px);
|
||||
//// lines.addSegment(p0, py);
|
||||
//// lines.addSegment(p0, pz);
|
||||
//// lines.addSegment(p0, pa);
|
||||
|
||||
//// gp.draw(plot);
|
||||
//// gp.flush();
|
||||
|
||||
// }
|
||||
|
||||
};
|
||||
|
||||
#endif // TURNDETECTION_H
|
||||
|
||||
Reference in New Issue
Block a user