geometry changes/fixes/new features
new grid walkers + fixes new test-cases worked on step/and turn detection android offline-data-reader worked on vap-grouping
This commit is contained in:
@@ -4,6 +4,7 @@
|
||||
#include "GyroscopeData.h"
|
||||
#include "AccelerometerData.h"
|
||||
#include "../../data/Timestamp.h"
|
||||
#include "../../math/MovingAverageTS.h"
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
@@ -23,147 +24,100 @@ class TurnDetection {
|
||||
|
||||
private:
|
||||
|
||||
//std::vector<AccelerometerData> accData;
|
||||
std::vector<GyroscopeData> gyroData;
|
||||
|
||||
Timestamp lastGyro;
|
||||
Timestamp lastRotMatEst;
|
||||
//std::vector<GyroscopeData> gyroData;
|
||||
Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
|
||||
|
||||
Timestamp lastGyroReading;
|
||||
|
||||
|
||||
struct {
|
||||
Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
|
||||
bool isKnown = false;
|
||||
Timestamp lastEstimation;
|
||||
} orientation;
|
||||
|
||||
|
||||
Eigen::Matrix3f rotMat;
|
||||
Eigen::Vector3f avgAcc;
|
||||
|
||||
Eigen::Vector3f leGyro;
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
TurnDetection() : rotMat(Eigen::Matrix3f::Identity()) {
|
||||
|
||||
TurnDetection() {
|
||||
;
|
||||
}
|
||||
|
||||
|
||||
// does not seem to help...
|
||||
// struct DriftEstimator {
|
||||
|
||||
|
||||
// MovingAverageTS<Eigen::Vector3f> avg;
|
||||
|
||||
// DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) {
|
||||
// ;
|
||||
// }
|
||||
|
||||
// void removeDrift(const Timestamp ts, Eigen::Vector3f& gyro) {
|
||||
|
||||
// if (gyro.norm() < 0.15) {
|
||||
// avg.add(ts, gyro);
|
||||
// gyro -= avg.get();
|
||||
// }
|
||||
|
||||
// }
|
||||
|
||||
// } driftEst;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
|
||||
|
||||
if (lastGyro.isZero()) {lastGyro = ts;}
|
||||
|
||||
// ignore the first reading completely, just remember its timestamp
|
||||
if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;}
|
||||
|
||||
// time-difference between previous and current reading
|
||||
const Timestamp curDiff = ts - lastGyroReading;
|
||||
lastGyroReading = ts;
|
||||
|
||||
// fast sensors might lead to delay = 0 ms. filter those values
|
||||
if (curDiff.isZero()) {return 0.0f;}
|
||||
|
||||
// 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;}
|
||||
|
||||
|
||||
// TESTING!
|
||||
gyroData.push_back(gyro);
|
||||
static Eigen::Matrix3f rotMat = Eigen::Matrix3f::Identity();
|
||||
if (gyroData.size() > 25) {
|
||||
Eigen::Vector3f sum = Eigen::Vector3f::Zero();
|
||||
int cnt = 0;
|
||||
for (GyroscopeData gd : gyroData) {
|
||||
//if (gd.z > gd.x && gd.z > gd.y) {
|
||||
Eigen::Vector3f vec; vec << (gd.x), (gd.y), (gd.z);
|
||||
sum += vec;
|
||||
++cnt;
|
||||
//}
|
||||
}
|
||||
gyroData.clear();
|
||||
if (cnt > 10) {
|
||||
Eigen::Vector3f z; z << 0,0, sum(2) < 0 ? -1 : +1;
|
||||
rotMat = getRotationMatrix(sum.normalized(), z.normalized());
|
||||
}
|
||||
}
|
||||
|
||||
// current gyro-reading as vector
|
||||
// get the current gyro-reading as vector
|
||||
Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
|
||||
leGyro = vec;
|
||||
|
||||
// current value, rotated into the new coordinate system
|
||||
Eigen::Vector3f curVec = rotMat * vec;
|
||||
|
||||
// previous value
|
||||
static Eigen::Vector3f oldVec = curVec;
|
||||
|
||||
// time-difference between previous and current value
|
||||
const Timestamp diff = ts - lastGyro;
|
||||
lastGyro = ts;
|
||||
// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
|
||||
Eigen::Vector3f curGyro = orientation.rotationMatrix * vec;
|
||||
//driftEst.removeDrift(ts, curGyro);
|
||||
|
||||
// area
|
||||
Eigen::Vector3f area = Eigen::Vector3f::Zero();
|
||||
if (!diff.isZero()) {
|
||||
area = (oldVec * diff.sec()) + // squared region
|
||||
((curVec - oldVec) * 0.5 * diff.sec()); // triangle region to the next (enhances the quality)
|
||||
}
|
||||
const Eigen::Vector3f area =
|
||||
|
||||
// Trapezoid rule (should be more accurate but does not always help?!)
|
||||
//(prevGyro * curDiff.sec()) + // squared region
|
||||
//((curGyro - prevGyro) * 0.5 * curDiff.sec()); // triangle region to the next (enhances the quality)
|
||||
|
||||
// just the rectangular region
|
||||
(prevGyro * curDiff.sec()); // BEST?!
|
||||
|
||||
|
||||
//}
|
||||
|
||||
// update the old value
|
||||
oldVec = curVec;
|
||||
prevGyro = curGyro;
|
||||
|
||||
const float delta = area(2);// * 0.8;
|
||||
|
||||
|
||||
|
||||
|
||||
static int i = 0; ++i;
|
||||
if (i % 50 == 0) {
|
||||
|
||||
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 pO(vec(0), vec(1), vec(2));
|
||||
|
||||
K::GnuplotPoint3 px(rotMat(0,0), rotMat(1,0), rotMat(2,0)); //px = px * eval(0);
|
||||
K::GnuplotPoint3 py(rotMat(0,1), rotMat(1,1), rotMat(2,1)); //py = py * eval(1);
|
||||
K::GnuplotPoint3 pz(rotMat(0,2), rotMat(1,2), rotMat(2,2)); //pz = pz * eval(2);
|
||||
|
||||
lines.addSegment(p0, px*0.15);
|
||||
lines.addSegment(p0, py*0.4);
|
||||
lines.addSegment(p0, pz*1.0);
|
||||
|
||||
Eigen::Vector3f ori = leGyro;
|
||||
Eigen::Vector3f re = rotMat * leGyro;
|
||||
Eigen::Vector3f avg = est.lastAvg * 0.3;
|
||||
|
||||
gp << "set arrow 1 from 0,0,0 to " << avg(0) << "," << avg(1) << "," << avg(2) << " lw 2\n";
|
||||
|
||||
gp << "set arrow 2 from 0,0,0 to " << ori(0) << "," << ori(1) << "," << ori(2) << " lw 1 dashtype 2 \n";
|
||||
gp << "set arrow 3 from 0,0,0 to " << re(0) << "," << re(1) << "," << re(2) << " lw 1\n";
|
||||
|
||||
// gp << "set arrow 2 from 0,0,0 to " << vec(0) << "," << vec(1) << "," << vec(2) << "\n";
|
||||
// gp << "set arrow 3 from 0,0,0 to " << nVec(0) << "," << nVec(1) << "," << nVec(2) << "\n";
|
||||
|
||||
gp.draw(plot);
|
||||
//gp.flush();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
static Eigen::Vector3f sum = Eigen::Vector3f::Zero();
|
||||
sum += area;
|
||||
|
||||
if (i % 30 == 0) {
|
||||
|
||||
static int idx = 0;
|
||||
static K::Gnuplot gp2;
|
||||
gp2 << "set arrow 1 from 0,0 to 10000,0\n";
|
||||
gp2 << "set arrow 2 from 0,5 to 10000,5\n";
|
||||
gp2 << "set arrow 3 from 0,10 to 10000,10\n";
|
||||
|
||||
K::GnuplotPlot plot2;
|
||||
static K::GnuplotPlotElementLines linesX; plot2.add(&linesX);
|
||||
static K::GnuplotPlotElementLines linesY; plot2.add(&linesY);
|
||||
static K::GnuplotPlotElementLines linesZ; plot2.add(&linesZ);
|
||||
|
||||
//linesX.add(K::GnuplotPoint2(idx, sum(0) + 0));
|
||||
//linesY.add(K::GnuplotPoint2(idx, sum(1) + 5));
|
||||
linesZ.add(K::GnuplotPoint2(idx, sum(2) + 10));
|
||||
++idx;
|
||||
|
||||
gp2.draw(plot2);
|
||||
//gp2.flush();
|
||||
|
||||
}
|
||||
// rotation = z-axis only!
|
||||
const float delta = area(2);
|
||||
|
||||
// done
|
||||
return delta;
|
||||
|
||||
}
|
||||
@@ -174,15 +128,17 @@ public:
|
||||
|
||||
// add accelerometer data
|
||||
//pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z));
|
||||
est.add((acc.x), (acc.y), (acc.z));
|
||||
est.addAcc(acc);
|
||||
|
||||
// start with the first available timestamp
|
||||
if (lastRotMatEst.isZero()) {lastRotMatEst = ts;}
|
||||
if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;}
|
||||
|
||||
// if we have at-least 1 sec of acc-data, re-calculate the current smartphone holding
|
||||
if (ts - lastRotMatEst > Timestamp::fromMS(500)) {
|
||||
rotMat = est.get();
|
||||
lastRotMatEst = 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();
|
||||
}
|
||||
|
||||
}
|
||||
@@ -243,44 +199,49 @@ public:
|
||||
|
||||
struct XYZ {
|
||||
|
||||
Eigen::Vector3f lastAvg;
|
||||
Eigen::Vector3f avg;
|
||||
Eigen::Vector3f sum;
|
||||
int cnt;
|
||||
|
||||
XYZ() {
|
||||
reset();
|
||||
}
|
||||
|
||||
void add(const float x, const float y, const float z) {
|
||||
/** add the given accelerometer reading */
|
||||
void addAcc(const AccelerometerData& acc) {
|
||||
|
||||
Eigen::Vector3f vec; vec << x,y,z;
|
||||
avg += vec;
|
||||
// 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;
|
||||
|
||||
}
|
||||
|
||||
Eigen::Matrix3f get() {
|
||||
/** get the current rotation matrix estimation */
|
||||
Eigen::Matrix3f get() const {
|
||||
|
||||
avg/= cnt;
|
||||
lastAvg = avg;
|
||||
// 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);
|
||||
|
||||
// sanity check
|
||||
// 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");
|
||||
|
||||
reset();
|
||||
|
||||
return rotMat;
|
||||
|
||||
}
|
||||
|
||||
/** reset the current sum etc. */
|
||||
void reset() {
|
||||
cnt = 0;
|
||||
avg = Eigen::Vector3f::Zero();
|
||||
sum = Eigen::Vector3f::Zero();
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user