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

many changes

Browse Source 
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:
FrankE
2017-10-31 19:38:08 +01:00
parent d97b325650
commit 284c6b11a6
14 changed files with 1377 additions and 579 deletions
Download Patch File Download Diff File Expand all files Collapse all files

146
sensors/imu/PoseDetectionPlot.h Normal file
View File

@@ -0,0 +1,146 @@
#ifndef INDOOR_IMU_POSEDETECTIONPLOT_H
#define INDOOR_IMU_POSEDETECTIONPLOT_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>
#include <KLib/misc/gnuplot/GnuplotSplotElementEmpty.h>
#include "../../data/Timestamp.h"
#include "../../math/Matrix3.h"
#include "AccelerometerData.h"
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;
std::vector<std::vector<std::vector<float>>> pose;
public:
/** ctor */
PoseDetectionPlot() {
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);
//plotPose.add(&emptyPose);
plotPose.getAxisX().setRange(-8,+8);
plotPose.getAxisY().setRange(-8,+8);
plotPose.getAxisZ().setRange(-8,+8);
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
{{0, 0, 0},{0, 0, 1},{0, 1, 1},{0, 1, 0},{0, 0, 0}}, // links
{{1, 0, 0},{1, 0, 1},{1, 1, 1},{1, 1, 0},{1, 0, 0}}, // rechts
{{0, 1, 0},{1, 1, 0},{1, 1, 1},{0, 1, 1},{0, 1, 0}}, // oben
{{0, 0, 0},{1, 0, 0},{1, 0, 1},{0, 0, 1},{0, 0, 0}}, // unten
{{0, 0, 1},{1, 0, 1},{1, 1, 1},{0, 1, 1},{0, 0, 1}}, // deckel
{{a, 0.15, 1},{b, 0.15, 1},{b, 0.95, 1},{a, 0.95, 1},{a, 0.15, 1}}, // display
};
//K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#000000"));
K::GnuplotStroke stroke = K::GnuplotStroke::NONE();
K::GnuplotFill fillOut = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"));
K::GnuplotFill fillSide = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"));
K::GnuplotFill fillDisp = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#333333"));
plotPose.getObjects().set(1, new K::GnuplotObjectPolygon(fillOut, stroke));
plotPose.getObjects().set(2, new K::GnuplotObjectPolygon(fillSide, stroke));
plotPose.getObjects().set(3, new K::GnuplotObjectPolygon(fillSide, stroke));
plotPose.getObjects().set(4, new K::GnuplotObjectPolygon(fillSide, stroke));
plotPose.getObjects().set(5, new K::GnuplotObjectPolygon(fillSide, stroke));
plotPose.getObjects().set(6, new K::GnuplotObjectPolygon(fillOut, stroke));
plotPose.getObjects().set(7, new K::GnuplotObjectPolygon(fillDisp, stroke));
}
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;
// update 3D smartphone model
for (size_t i = 0; i < pose.size(); ++i) {
K::GnuplotObjectPolygon* gp = (K::GnuplotObjectPolygon*) plotPose.getObjects().get(i+1); gp->clear();
for (const std::vector<float>& pts : pose[i]) {
const Vector3 vec1(pts[0], pts[1], pts[2]);
const Vector3 vec2 = vec1 - Vector3(0.5, 0.5, 0.5); // center cube at 0,0,0
const Vector3 vec3 = vec2 * Vector3(7, 15, 1); // stretch cube
const Vector3 vec4 = rotation * vec3;
gp->add(K::GnuplotCoordinate3(vec4.x, vec4.y, vec4.z, K::GnuplotCoordinateSystem::FIRST));
}
}
// add coordinate system
const Vector3 vx = rotation * Vector3(2,0,0);
const Vector3 vy = rotation * Vector3(0,3,0);
const Vector3 vz = rotation * Vector3(0,0,5);
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));
// 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);
// raw accelerometer
gp1.draw(plotAcc);
gp1.flush();
// 3D pose
gp2.draw(plotPose);
gp2.flush();
}
}
};
#endif
#endif // INDOOR_IMU_POSEDETECTIONPLOT_H