254 lines
8.9 KiB
C++
254 lines
8.9 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#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 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 gp2;
|
||
|
||
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";
|
||
|
||
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);
|
||
|
||
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
|
||
{{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 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);
|
||
|
||
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));
|
||
}
|
||
}
|
||
|
||
// // update un-rotated 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);
|
||
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));
|
||
|
||
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";
|
||
|
||
|
||
// 3D pose
|
||
gp2.draw(plotPose);
|
||
gp2.flush();
|
||
|
||
|
||
}
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif
|
||
|
||
#endif // INDOOR_IMU_POSEDETECTIONPLOT_H
|