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

fixed some issues

Browse Source 
added new pose/turn detections
new helper classes
define-flags for libEigen
This commit is contained in:
FrankE
2018-09-04 10:49:00 +02:00
parent f990485d44
commit 857d7a1553
51 changed files with 2149 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files

134
sensors/imu/PoseDetectionPlot.h
View File

@@ -17,19 +17,103 @@
#include "../../math/Matrix3.h"
#include "AccelerometerData.h"
class PoseDetectionPlotAngles {
Timestamp plotRef;
Timestamp lastPlot;
K::Gnuplot gp;
K::GnuplotPlot plotAcc;
K::GnuplotPlotElementLines lineAccX;
K::GnuplotPlotElementLines lineAccY;
K::GnuplotPlotElementLines lineGyroX;
K::GnuplotPlotElementLines lineGyroY;
K::GnuplotPlotElementLines lineFusedX;
K::GnuplotPlotElementLines lineFusedY;
public:
PoseDetectionPlotAngles() {
gp << "set autoscale xfix\n";
plotAcc.setTitle("Accelerometer");
plotAcc.add(&lineFusedX); lineFusedX.getStroke().getColor().setHexStr("#990000"); lineFusedX.setTitle("FusedX");lineFusedX.getStroke().setWidth(3);
plotAcc.add(&lineFusedY); lineFusedY.getStroke().getColor().setHexStr("#09900"); lineFusedY.setTitle("FusedY"); lineFusedY.getStroke().setWidth(3);
plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff8888"); lineAccX.setTitle("AccX"); lineAccX.getStroke().setType(K::GnuplotDashtype::DOTTED); lineAccX.getStroke().setWidth(2);
plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#88ff88"); lineAccY.setTitle("AccY"); lineAccY.getStroke().setType(K::GnuplotDashtype::DOTTED); lineAccY.getStroke().setWidth(2);
plotAcc.add(&lineGyroX); lineGyroX.getStroke().getColor().setHexStr("#ff8888"); lineGyroX.setTitle("GyroX"); lineGyroX.getStroke().setType(K::GnuplotDashtype::DASHED); lineGyroX.getStroke().setWidth(2);
plotAcc.add(&lineGyroY); lineGyroY.getStroke().getColor().setHexStr("#88ff88"); lineGyroY.setTitle("GyroY"); lineGyroY.getStroke().setType(K::GnuplotDashtype::DASHED); lineGyroY.getStroke().setWidth(2);
plotAcc.getKey().setVisible(true);
}
void addAcc(Timestamp ts, float x, float y) {
if (plotRef.isZero()) {plotRef = ts;}
const Timestamp tsPlot = (ts-plotRef);
lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), x) );
lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), y) );
}
void addGyro(Timestamp ts, float x, float y) {
if (plotRef.isZero()) {plotRef = ts;}
const Timestamp tsPlot = (ts-plotRef);
lineGyroX.add( K::GnuplotPoint2(tsPlot.ms(), x) );
lineGyroY.add( K::GnuplotPoint2(tsPlot.ms(), y) );
}
void addFused(Timestamp ts, float x, float y) {
if (plotRef.isZero()) {plotRef = ts;}
const Timestamp tsPlot = (ts-plotRef);
lineFusedX.add( K::GnuplotPoint2(tsPlot.ms(), x) );
lineFusedY.add( K::GnuplotPoint2(tsPlot.ms(), y) );
if (++cnt % 40 == 0) {flush(ts);}
}
private:
int cnt = 0;
void flush(Timestamp ts) {
cleanup(ts);
gp.draw(plotAcc);
gp.flush();
}
// remove old entries
void cleanup(Timestamp ts) {
const Timestamp tsPlot = (ts-plotRef);
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(3000);
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
lineAccX.removeIf(remove);
lineAccY.removeIf(remove);
lineGyroX.removeIf(remove);
lineGyroY.removeIf(remove);
lineFusedX.removeIf(remove);
lineFusedY.removeIf(remove);
}
};
class PoseDetectionPlot {
Timestamp plotRef;
Timestamp lastPlot;
K::Gnuplot gp1;
K::Gnuplot gp2;
K::GnuplotPlot plotAcc;
K::GnuplotPlotElementLines lineAccX;
K::GnuplotPlotElementLines lineAccY;
K::GnuplotPlotElementLines lineAccZ;
K::GnuplotSplot plotPose;
K::GnuplotSplotElementLines linePose;
//K::GnuplotSplotElementEmpty emptyPose;
@@ -41,14 +125,9 @@
/** ctor */
PoseDetectionPlot() {
gp1 << "set autoscale xfix\n";
//gp1 << "set autoscale xfix\n";
gp2 << "set view equal xyz\n";
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);
@@ -58,6 +137,10 @@
plotPose.getAxisY().setRange(-8,+8);
plotPose.getAxisZ().setRange(-8,+8);
plotPose.getAxisX().setLabel("x");
plotPose.getAxisY().setLabel("y");
plotPose.getAxisZ().setLabel("z");
const float a = 0.05; const float b = 0.95;
pose = {
{{0, 0, 0},{1, 0, 0},{1, 1, 0},{0, 1, 0},{0, 0, 0}}, // boden
@@ -85,17 +168,20 @@
}
void setName(const std::string& name) {
plotPose.setTitle(name);
}
void add(Timestamp ts, const Vector3& avg, const Matrix3& rotation) {
add(ts, AccelerometerData(avg.x, avg.y, avg.z), rotation);
}
void add(Timestamp ts, const AccelerometerData& avg, const Matrix3& rotation) {
if (plotRef.isZero()) {plotRef = ts;}
const Timestamp tsPlot = (ts-plotRef);
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
// acc
lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), avg.x) );
lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), avg.y) );
lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), avg.z) );
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
lastPlot = tsPlot;
@@ -128,20 +214,18 @@
const Vector3 vx = rotation * Vector3(2,0,0);
const Vector3 vy = rotation * Vector3(0,3,0);
const Vector3 vz = rotation * Vector3(0,0,5);
const Vector3 vA = Vector3(avg.x, avg.y, -avg.z) * 1;
linePose.clear();
linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vx.x, vx.y, vx.z));
linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vy.x, vy.y, vy.z));
linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vz.x, vz.y, vz.z));
linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vA.x, vA.y, vA.z));
// remove old accelerometer entries
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
lineAccX.removeIf(remove);
lineAccY.removeIf(remove);
lineAccZ.removeIf(remove);
gp2 << "set label 91 at " << vx.x << "," << vx.y << "," << vx.z << " 'x' \n";
gp2 << "set label 92 at " << vy.x << "," << vy.y << "," << vy.z << " 'y' \n";
gp2 << "set label 93 at " << vz.x << "," << vz.y << "," << vz.z << " 'z' \n";
gp2 << "set label 99 at " << vA.x << "," << vA.y << "," << vA.z << " 'accel' \n";
// raw accelerometer
gp1.draw(plotAcc);
gp1.flush();
// 3D pose
gp2.draw(plotPose);