FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

geometry changes/fixes/new features

Browse Source 
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:
FrankE
2016-09-07 10:16:51 +02:00
parent a203305628
commit d283d9b326
27 changed files with 976 additions and 333 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
sensors/imu/AccelerometerData.h
View File

@@ -18,6 +18,28 @@ struct AccelerometerData {
return std::sqrt( x*x + y*y + z*z );
}
AccelerometerData& operator += (const AccelerometerData& o) {
this->x += o.x;
this->y += o.y;
this->z += o.z;
return *this;
}
AccelerometerData& operator -= (const AccelerometerData& o) {
this->x -= o.x;
this->y -= o.y;
this->z -= o.z;
return *this;
}
AccelerometerData operator - (const AccelerometerData& o) const {
return AccelerometerData(x-o.x, y-o.y, z-o.z);
}
AccelerometerData operator / (const float val) const {
return AccelerometerData(x/val, y/val, z/val);
}
};
#endif // ACCELEROMETERDATA_H

195
sensors/imu/StepDetection.h
View File

@@ -16,6 +16,7 @@
#include "../../Assertions.h"
#include "../../math/MovingAverageTS.h"
/**
@@ -27,85 +28,155 @@ class StepDetection {
private:
/** low pass acc-magnitude */
float avg1 = 0;
MovingAverageTS<float> avgLong;
MovingAverageTS<float> avgShort;
/** even-more low-pass acc-magnitude */
float avg2 = 0;
Timestamp blockUntil;
bool waitForUp = false;
private:
class Stepper {
private:
/** block for 300 ms after every step */
const Timestamp blockTime = Timestamp::fromMS(300);
/** the threshold for detecting a spike as step */
const float threshold = 0.17;
/** block until the given timestamp before detecting additional steps */
Timestamp blockUntil;
public:
/** is the given (relative!) magnitude (mag - ~9.81) a step? */
bool isStep(const Timestamp ts, const float mag) {
// still blocking
if (ts < blockUntil) {
return false;
}
// threshold reached? -> step!
if (mag > threshold) {
// block x milliseconds until detecting the next step
blockUntil = ts + blockTime;
// we have a step
return true;
}
// no step
return false;
}
};
Stepper stepper;
const Timestamp blockTime = Timestamp::fromMS(150); // 150-250 looks good
const float upperThreshold = +0.4f; // + is usually smaller than down (look at graphs)
const float lowerThreshold = -0.8f;
public:
/** ctor */
StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
;
}
/** does the given data indicate a step? */
bool isStep(const Timestamp ts, const AccelerometerData& acc) {
bool add(const Timestamp ts, const AccelerometerData& acc) {
avg1 = avg1 * 0.91 + acc.magnitude() * 0.09; // short-time average [filtered steps]
avg2 = avg2 * 0.97 + acc.magnitude() * 0.03; // long-time average [gravity]
// update averages
avgLong.add(ts, acc.magnitude());
avgShort.add(ts, acc.magnitude());
// average maginitude must be > 9.0 to be stable enough to proceed
if (avg2 > 9) {
// difference between long-term-average (gravity) and very-short-time average
const float delta = avgShort.get() - avgLong.get();
// gravity-free magnitude
const float avg = avg1 - avg2;
bool step = false;
// detect steps
return stepper.isStep(ts, avg);
} else {
return false;
if (blockUntil > ts) {return false;}
// wait for a rising edge
if (waitForUp && delta > upperThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = false;
}
// wait for a falling edge
if (!waitForUp && delta < lowerThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = true;
step = true;
}
// static K::Gnuplot gp;
// static K::GnuplotPlot plot;
// static K::GnuplotPlotElementLines lines1; plot.add(&lines1);
// static K::GnuplotPlotElementLines lines2; plot.add(&lines2); lines2.setColorHex("#0000ff");
// static Timestamp ref = ts;
// static int i = 0;
// //lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
// lines2.add( K::GnuplotPoint2((ts-ref).ms(), delta) );
// if (++i % 100 == 0) {
// gp.draw(plot);
// gp.flush();
// usleep(1000*25);
// }
return step;
}
//private:
// /** low pass acc-magnitude */
// float avg1 = 0;
// /** even-more low-pass acc-magnitude */
// float avg2 = 0;
//private:
// class Stepper {
// private:
// /** block for 300 ms after every step */
// const Timestamp blockTime = Timestamp::fromMS(300);
// /** the threshold for detecting a spike as step */
// const float threshold = 0.30;
// /** block until the given timestamp before detecting additional steps */
// Timestamp blockUntil;
// public:
// /** is the given (relative!) magnitude (mag - ~9.81) a step? */
// bool isStep(const Timestamp ts, const float mag) {
// // still blocking
// if (ts < blockUntil) {
// return false;
// }
// // threshold reached? -> step!
// if (mag > threshold) {
// // block x milliseconds until detecting the next step
// blockUntil = ts + blockTime;
// // we have a step
// return true;
// }
// // no step
// return false;
// }
// };
// Stepper stepper;
//public:
// /** does the given data indicate a step? */
// bool add(const Timestamp ts, const AccelerometerData& acc) {
// avg1 = avg1 * 0.91 + acc.magnitude() * 0.09; // short-time average [filtered steps]
// avg2 = avg2 * 0.97 + acc.magnitude() * 0.03; // long-time average [gravity]
// // average maginitude must be > 9.0 to be stable enough to proceed
// if (avg2 > 9) {
// // gravity-free magnitude
// const float avg = avg1 - avg2;
// // detect steps
// return stepper.isStep(ts, avg);
// } else {
// return false;
// }
// }
};

237
sensors/imu/TurnDetection.h
View File

@@ -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();
}