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

worked on synthetic sensors

Browse Source 
worked on grid-walker
minor changes/fixes/improvements
This commit is contained in:
k-a-z-u
2017-10-18 16:54:57 +02:00
parent 72f083d32a
commit 3e31f6da53
8 changed files with 522 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
sensors/imu/AccelerometerData.h
View File

@@ -42,6 +42,14 @@ struct AccelerometerData {
return AccelerometerData(x/val, y/val, z/val);
}
AccelerometerData operator * (const float val) const {
return AccelerometerData(x*val, y*val, z*val);
}
AccelerometerData operator + (const AccelerometerData o) const {
return AccelerometerData(x+o.x, y+o.y, z+o.z);
}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";

73
sensors/imu/StepDetection.h
View File

@@ -8,12 +8,14 @@
#include <cmath>
#include <vector>
#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>
#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/GnuplotPlotElementPoints.h>
#endif
#include "../../Assertions.h"
#include "../../math/MovingAverageTS.h"
@@ -43,11 +45,12 @@ private:
K::Gnuplot gp;
K::GnuplotPlot plot;
K::GnuplotPlotElementLines lineDet;
K::GnuplotPlotElementPoints pointDet;
K::GnuplotPlotElementLines lineX;
K::GnuplotPlotElementLines lineY;
K::GnuplotPlotElementLines lineZ;
Timestamp plotRef;
int plotCnt = 0;
Timestamp lastPlot;
#endif
@@ -57,10 +60,13 @@ public:
StepDetection() : avgLong(Timestamp::fromMS(500), 0), avgShort(Timestamp::fromMS(40), 0) {
#ifdef WITH_DEBUG_PLOT
plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000");
plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00");
plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff");
gp << "set autoscale xfix\n";
plot.setTitle("Step Detection");
plot.add(&lineX); lineX.getStroke().getColor().setHexStr("#ff0000"); lineX.setTitle("accX");
plot.add(&lineY); lineY.getStroke().getColor().setHexStr("#00ff00"); lineY.setTitle("accY");
plot.add(&lineZ); lineZ.getStroke().getColor().setHexStr("#0000ff"); lineZ.setTitle("accZ");
plot.add(&lineDet); lineDet.getStroke().getColor().setHexStr("#000000");
plot.add(&pointDet); pointDet.setPointSize(2); pointDet.setPointType(7);
#endif
}
@@ -78,19 +84,25 @@ public:
bool step = false;
if (blockUntil > ts) {return false;}
if (blockUntil > ts) {
// wait for a rising edge
if (waitForUp && delta > upperThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = false;
}
step = false;
} else {
// 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;
}
// wait for a falling edge
if (!waitForUp && delta < lowerThreshold) {
blockUntil = ts + blockTime; // block some time
waitForUp = true;
step = true;
}
@@ -98,6 +110,7 @@ public:
if (plotRef.isZero()) {plotRef = ts;}
const Timestamp tsPlot = (ts-plotRef);
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
//lines1.add( K::GnuplotPoint2((ts-ref).ms(), _delta) );
lineX.add( K::GnuplotPoint2(tsPlot.ms(), acc.x) );
@@ -105,10 +118,24 @@ public:
lineZ.add( K::GnuplotPoint2(tsPlot.ms(), acc.z) );
lineDet.add( K::GnuplotPoint2(tsPlot.ms(), delta) );
if (++plotCnt % 25 == 0) {
if (step) {
pointDet.add( K::GnuplotPoint2(tsPlot.ms(), 0) );
}
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
lastPlot = tsPlot;
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
lineX.removeIf(remove);
lineY.removeIf(remove);
lineZ.removeIf(remove);
lineDet.removeIf(remove);
pointDet.removeIf(remove);
gp.draw(plot);
gp.flush();
//usleep(1000*25);
usleep(100);
}
#endif

243
sensors/imu/TurnDetection.h
View File

@@ -5,18 +5,23 @@
#include "AccelerometerData.h"
#include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h"
#include "../../geo/Point3.h"
#include <eigen3/Eigen/Dense>
#include <cmath>
#include <vector>
#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>
#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 "../../Assertions.h"
@@ -24,6 +29,37 @@ 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
//std::vector<GyroscopeData> gyroData;
Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
@@ -44,7 +80,40 @@ 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
}
@@ -90,7 +159,6 @@ public:
// otherwise we would use a wrong rotation matrix which yields wrong results!
if (!orientation.isKnown) {return 0.0f;}
// get the current gyro-reading as vector
Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
@@ -117,6 +185,77 @@ public:
// rotation = z-axis only!
const float delta = area(2);
#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);
}
#endif
// done
return delta;
@@ -128,17 +267,31 @@ public:
// add accelerometer data
//pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z));
est.addAcc(acc);
est.addAcc(ts, acc);
// start with the first available timestamp
if (orientation.lastEstimation.isZero()) {orientation.lastEstimation = ts;}
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
// 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();
}
}
@@ -207,7 +360,7 @@ public:
}
/** add the given accelerometer reading */
void addAcc(const AccelerometerData& acc) {
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);
@@ -218,6 +371,11 @@ public:
sum += vec;
++cnt;
}
AccelerometerData getAvg() const {
return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
}
/** get the current rotation matrix estimation */
@@ -245,9 +403,64 @@ public:
}
};
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;

12
sensors/offline/FilePlayer.h
View File

@@ -83,6 +83,10 @@ namespace Offline {
// get all sensor events from the offline file
const std::vector<Entry> events = reader->getEntries();
// reference time (system vs. first-event)
Timestamp tsRef1 = Timestamp::fromMS(events.front().ts);
Timestamp tsRef2 = Timestamp::fromRunningTime();
// process every event
for (const Entry& e : events) {
@@ -92,6 +96,14 @@ namespace Offline {
// timestamp
const Timestamp ts = Timestamp::fromMS(e.ts);
// ensure events happen occur fast as they did during recording
if (realtime) {
const Timestamp ts1 = ts-tsRef1;
const Timestamp ts2 = Timestamp::fromRunningTime() - tsRef2;
const Timestamp diff = ts1-ts2;
if (diff.ms() > 0) {std::this_thread::sleep_for(std::chrono::milliseconds(diff.ms()));}
}
// event index
const size_t idx = e.idx;