many changes
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:
112
sensors/imu/CompassDetection.h
Normal file
112
sensors/imu/CompassDetection.h
Normal file
@@ -0,0 +1,112 @@
|
||||
#ifndef INDOOR_IMU_COMPASSDETECTION_H
|
||||
#define INDOOR_IMU_COMPASSDETECTION_H
|
||||
|
||||
#include "MagnetometerData.h"
|
||||
#include "PoseDetection.h"
|
||||
|
||||
#include "../../data/Timestamp.h"
|
||||
#include "../../math/MovingAverageTS.h"
|
||||
#include "../../math/MovingStdDevTS.h"
|
||||
#include "../../geo/Point3.h"
|
||||
#include "../../Assertions.h"
|
||||
|
||||
#include "CompassDetectionPlot.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
|
||||
/**
|
||||
* custom compass implementation, similar to turn-detection
|
||||
*/
|
||||
class CompassDetection {
|
||||
|
||||
private:
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
CompassDetectionPlot plot;
|
||||
#endif
|
||||
|
||||
// struct {
|
||||
// Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
|
||||
// bool isKnown = false;
|
||||
// Timestamp lastEstimation;
|
||||
// } orientation;
|
||||
|
||||
MovingAverageTS<MagnetometerData> avgIn = MovingAverageTS<MagnetometerData>(Timestamp::fromMS(150), MagnetometerData(0,0,0));
|
||||
|
||||
//MovingStdDevTS<MagnetometerData> stdDev = MovingStdDevTS<MagnetometerData>(Timestamp::fromMS(2000), MagnetometerData(0,0,0));
|
||||
MovingStdDevTS<float> stdDev = MovingStdDevTS<float>(Timestamp::fromMS(1500), 0);
|
||||
|
||||
PoseDetection* pose = nullptr;
|
||||
int numMagReadings = 0;
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
CompassDetection(PoseDetection* pose) : pose(pose) {
|
||||
;
|
||||
}
|
||||
|
||||
/** add magnetometer readings, returns the current heading, or NAN (if unstable/unknown) */
|
||||
float addMagnetometer(const Timestamp& ts, const MagnetometerData& mag) {
|
||||
|
||||
++numMagReadings;
|
||||
|
||||
// get the current magnetometer-reading as vector
|
||||
//Eigen::Vector3f vec; vec << mag.x, mag.y, mag.z;
|
||||
const Vector3 vec(mag.x, mag.y, mag.z);
|
||||
|
||||
// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
|
||||
//Eigen::Vector3f _curMag = pose->getMatrix() * vec;
|
||||
const Vector3 _curMag = pose->getMatrix() * vec;
|
||||
|
||||
//avgIn.add(ts, MagnetometerData(_curMag(0), _curMag(1), _curMag(2)));
|
||||
avgIn.add(ts, MagnetometerData(_curMag.x, _curMag.y, _curMag.z));
|
||||
const MagnetometerData curMag = avgIn.get();
|
||||
//const MagnetometerData curMag =MagnetometerData(_curMag.x, _curMag.y, _curMag.z);
|
||||
|
||||
// calculate standard-deviation
|
||||
//stdDev.add(ts, curMag);
|
||||
//const float dev = std::max(stdDev.get().x, stdDev.get().y);
|
||||
|
||||
|
||||
// calculate angle
|
||||
// https://aerocontent.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
|
||||
const float mx = curMag.x;
|
||||
const float my = curMag.y;
|
||||
const float tmp = std::atan2(my, mx);
|
||||
//const float tmp = (my > 0) ? (M_PI*0.5 - std::atan(mx/my)) : (M_PI*1.5 - atan(mx/my));
|
||||
|
||||
// http://www.magnetic-declination.com/
|
||||
// http://davidegironi.blogspot.de/2013/01/avr-atmega-hmc5883l-magnetometer-lib-01.html
|
||||
const float declination = 3.0f / 180.0f * M_PI; // GERMANY!
|
||||
const float curHeading = - tmp + declination;
|
||||
float resHeading;
|
||||
bool stable = true;
|
||||
|
||||
// calculate standard-deviation within a certain timeframe
|
||||
stdDev.add(ts, curHeading);
|
||||
|
||||
// if the standard-deviation is too high,
|
||||
// the compass is considered unstable
|
||||
if (numMagReadings < 250 || stdDev.get() > 0.30) {
|
||||
resHeading = NAN;
|
||||
stable = false;
|
||||
} else {
|
||||
resHeading = curHeading;
|
||||
stable = true;
|
||||
}
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
plot.add(ts, curHeading, stable, mag, curMag);
|
||||
#endif
|
||||
|
||||
// done
|
||||
return resHeading;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif // INDOOR_IMU_COMPASSDETECTION_H
|
||||
123
sensors/imu/CompassDetectionPlot.h
Normal file
123
sensors/imu/CompassDetectionPlot.h
Normal file
@@ -0,0 +1,123 @@
|
||||
#ifndef INDOOR_IMU_COMPASSDETECTIONPLOT_H
|
||||
#define INDOOR_IMU_COMPASSDETECTIONPLOT_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/GnuplotPlotElementPoints.h>
|
||||
|
||||
#include "MagnetometerData.h"
|
||||
#include "../../data/Timestamp.h"
|
||||
#include "../../math/Matrix3.h"
|
||||
|
||||
class CompassDetectionPlot {
|
||||
|
||||
Timestamp plotRef;
|
||||
Timestamp lastPlot;
|
||||
|
||||
K::Gnuplot gp1;
|
||||
K::Gnuplot gp2;
|
||||
K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);
|
||||
|
||||
K::GnuplotPlot plotMagRaw;
|
||||
K::GnuplotPlotElementLines lineMagRawX;
|
||||
K::GnuplotPlotElementLines lineMagRawY;
|
||||
K::GnuplotPlotElementLines lineMagRawZ;
|
||||
K::GnuplotPlotElementLines lineMagRawMagnitude;
|
||||
|
||||
K::GnuplotPlot plotMagFix;
|
||||
K::GnuplotPlotElementLines lineMagFixX;
|
||||
K::GnuplotPlotElementLines lineMagFixY;
|
||||
K::GnuplotPlotElementLines lineMagFixZ;
|
||||
|
||||
K::GnuplotPlot plotMagScatter;
|
||||
K::GnuplotPlotElementPoints scatterPoints;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
|
||||
CompassDetectionPlot() {
|
||||
|
||||
gp1 << "set autoscale xfix\n";
|
||||
gp2 << "set size ratio -1\n";
|
||||
|
||||
multiplot.add(&plotMagRaw);
|
||||
multiplot.add(&plotMagFix);
|
||||
|
||||
plotMagRaw.setTitle("Magnetometer (raw)");
|
||||
plotMagRaw.add(&lineMagRawX); lineMagRawX.getStroke().getColor().setHexStr("#ff0000"); lineMagRawX.setTitle("x");
|
||||
plotMagRaw.add(&lineMagRawY); lineMagRawY.getStroke().getColor().setHexStr("#00ff00"); lineMagRawY.setTitle("y");
|
||||
plotMagRaw.add(&lineMagRawZ); lineMagRawZ.getStroke().getColor().setHexStr("#0000ff"); lineMagRawZ.setTitle("z");
|
||||
plotMagRaw.add(&lineMagRawMagnitude); lineMagRawMagnitude.getStroke().getColor().setHexStr("#000000"); lineMagRawMagnitude.setTitle("magnitude");
|
||||
|
||||
plotMagFix.setTitle("Magnetometer (re-aligned)");
|
||||
plotMagFix.add(&lineMagFixX); lineMagFixX.getStroke().getColor().setHexStr("#ff0000"); lineMagFixX.setTitle("x");
|
||||
plotMagFix.add(&lineMagFixY); lineMagFixY.getStroke().getColor().setHexStr("#00ff00"); lineMagFixY.setTitle("y");
|
||||
plotMagFix.add(&lineMagFixZ); lineMagFixZ.getStroke().getColor().setHexStr("#0000ff"); lineMagFixZ.setTitle("z");
|
||||
|
||||
plotMagScatter.add(&scatterPoints);
|
||||
|
||||
|
||||
}
|
||||
|
||||
void add(Timestamp ts, float curHeading, bool stable, const MagnetometerData& mag, const MagnetometerData& curMag) {
|
||||
|
||||
if (plotRef.isZero()) {plotRef = ts;}
|
||||
const Timestamp tsPlot = (ts-plotRef);
|
||||
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
|
||||
|
||||
// raw gyro
|
||||
lineMagRawX.add( K::GnuplotPoint2(tsPlot.ms(), mag.x) );
|
||||
lineMagRawY.add( K::GnuplotPoint2(tsPlot.ms(), mag.y) );
|
||||
lineMagRawZ.add( K::GnuplotPoint2(tsPlot.ms(), mag.z) );
|
||||
lineMagRawMagnitude.add( K::GnuplotPoint2(tsPlot.ms(), mag.magnitude()) );
|
||||
|
||||
// adjusted mag
|
||||
lineMagFixX.add( K::GnuplotPoint2(tsPlot.ms(), curMag.x) );
|
||||
lineMagFixY.add( K::GnuplotPoint2(tsPlot.ms(), curMag.y) );
|
||||
lineMagFixZ.add( K::GnuplotPoint2(tsPlot.ms(), curMag.z) );
|
||||
|
||||
const float len = 1;//curMag.magnitude();// std::sqrt(curMag.x*curMag.x + curMag.y*curMag.y);
|
||||
scatterPoints.add(K::GnuplotPoint2(curMag.x/len, curMag.y/len));
|
||||
|
||||
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
|
||||
|
||||
lastPlot = tsPlot;
|
||||
|
||||
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
|
||||
lineMagRawX.removeIf(remove);
|
||||
lineMagRawY.removeIf(remove);
|
||||
lineMagRawZ.removeIf(remove);
|
||||
lineMagRawMagnitude.removeIf(remove);
|
||||
lineMagFixX.removeIf(remove);
|
||||
lineMagFixY.removeIf(remove);
|
||||
lineMagFixZ.removeIf(remove);
|
||||
|
||||
|
||||
const float s = stable ? 0.1 : 0.03;
|
||||
const float ax = 0.85 + std::cos(curHeading)*s;
|
||||
const float ay = 0.85 + std::sin(curHeading)*s;
|
||||
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(plotMagScatter);
|
||||
gp2.flush();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
81
sensors/imu/MagnetometerData.h
Normal file
81
sensors/imu/MagnetometerData.h
Normal file
@@ -0,0 +1,81 @@
|
||||
#ifndef INDOOR_IMU_MAGNETOMETERDATA_H
|
||||
#define INDOOR_IMU_MAGNETOMETERDATA_H
|
||||
|
||||
|
||||
#include <cmath>
|
||||
#include <sstream>
|
||||
|
||||
/**
|
||||
* data received from a magnetometer sensor
|
||||
*/
|
||||
struct MagnetometerData {
|
||||
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
|
||||
MagnetometerData() : x(0), y(0), z(0) {;}
|
||||
|
||||
/** ctor from RADIANS */
|
||||
MagnetometerData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
|
||||
|
||||
std::string asString() const {
|
||||
std::stringstream ss;
|
||||
ss << "(" << x << "," << y << "," << z << ")";
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
bool isValid() const {
|
||||
return (x == x) && (y == y) && (z == z);
|
||||
}
|
||||
|
||||
bool operator == (const GyroscopeData& o ) const {
|
||||
return EQ_OR_NAN(x, o.x) &&
|
||||
EQ_OR_NAN(y, o.y) &&
|
||||
EQ_OR_NAN(z, o.z);
|
||||
}
|
||||
|
||||
float magnitude() const {
|
||||
return std::sqrt( x*x + y*y + z*z );
|
||||
}
|
||||
|
||||
MagnetometerData& operator += (const MagnetometerData& o) {
|
||||
this->x += o.x;
|
||||
this->y += o.y;
|
||||
this->z += o.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
MagnetometerData& operator -= (const MagnetometerData& o) {
|
||||
this->x -= o.x;
|
||||
this->y -= o.y;
|
||||
this->z -= o.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
MagnetometerData operator * (const MagnetometerData& o) const {
|
||||
return MagnetometerData(x*o.x, y*o.y, z*o.z);
|
||||
}
|
||||
|
||||
MagnetometerData operator - (const MagnetometerData& o) const {
|
||||
return MagnetometerData(x-o.x, y-o.y, z-o.z);
|
||||
}
|
||||
|
||||
MagnetometerData operator / (const float val) const {
|
||||
return MagnetometerData(x/val, y/val, z/val);
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
static inline bool EQ_OR_NAN(const float a, const float b) {return (a==b) || ( (a!=a) && (b!=b) );}
|
||||
|
||||
};
|
||||
|
||||
namespace std {
|
||||
MagnetometerData sqrt(const MagnetometerData& o) {
|
||||
return MagnetometerData(std::sqrt(o.x), std::sqrt(o.y), std::sqrt(o.z));
|
||||
}
|
||||
}
|
||||
|
||||
#endif // INDOOR_IMU_MAGNETOMETERDATA_H
|
||||
480
sensors/imu/PoseDetection.h
Normal file
480
sensors/imu/PoseDetection.h
Normal file
@@ -0,0 +1,480 @@
|
||||
#ifndef INDOOR_IMU_POSEDETECTION_H
|
||||
#define INDOOR_IMU_POSEDETECTION_H
|
||||
|
||||
#include "AccelerometerData.h"
|
||||
|
||||
#include "../../data/Timestamp.h"
|
||||
|
||||
#include "../../math/MovingAverageTS.h"
|
||||
#include "../../math/MovingMedianTS.h"
|
||||
#include "../../math/Matrix3.h"
|
||||
|
||||
#include "../../geo/Point3.h"
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "PoseDetectionPlot.h"
|
||||
|
||||
/**
|
||||
* estimate the smartphones world-pose,
|
||||
* based on the accelerometer's data
|
||||
*/
|
||||
class PoseDetection {
|
||||
|
||||
// struct LongTermTriggerAverage {
|
||||
|
||||
// Eigen::Vector3f sum;
|
||||
// int cnt;
|
||||
|
||||
// XYZ() {
|
||||
// reset();
|
||||
// }
|
||||
|
||||
// /** 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;}
|
||||
|
||||
// // adjust sum and count (for average calculation)
|
||||
// Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z;
|
||||
// sum += vec;
|
||||
// ++cnt;
|
||||
|
||||
|
||||
// }
|
||||
|
||||
// AccelerometerData getAvg() const {
|
||||
// return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
|
||||
// }
|
||||
|
||||
// /** get the current rotation matrix estimation */
|
||||
// Eigen::Matrix3f get() const {
|
||||
|
||||
// // 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);
|
||||
|
||||
// // 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() {
|
||||
// cnt = 0;
|
||||
// sum = Eigen::Vector3f::Zero();
|
||||
// }
|
||||
|
||||
|
||||
// };
|
||||
|
||||
|
||||
/** live-pose-estimation using moving average of the smartphone's accelerometer */
|
||||
struct EstMovingAverage {
|
||||
|
||||
// average the accelerometer
|
||||
MovingAverageTS<AccelerometerData> avg;
|
||||
|
||||
EstMovingAverage(const Timestamp window) :
|
||||
avg(MovingAverageTS<AccelerometerData>(window, AccelerometerData())) {
|
||||
|
||||
// start approximately
|
||||
addAcc(Timestamp(), AccelerometerData(0,0,9.81));
|
||||
|
||||
}
|
||||
|
||||
/** add the given accelerometer reading */
|
||||
void addAcc(const Timestamp ts, const AccelerometerData& acc) {
|
||||
avg.add(ts, acc);
|
||||
}
|
||||
|
||||
AccelerometerData getBase() const {
|
||||
return avg.get();
|
||||
}
|
||||
|
||||
/** get the current rotation matrix estimation */
|
||||
//Eigen::Matrix3f get() const {
|
||||
Matrix3 get() const {
|
||||
|
||||
// get the current acceleromter average
|
||||
const AccelerometerData avgAcc = avg.get();
|
||||
//const Eigen::Vector3f avg(avgAcc.x, avgAcc.y, avgAcc.z);
|
||||
const Vector3 avg(avgAcc.x, avgAcc.y, avgAcc.z);
|
||||
|
||||
// rotate average-accelerometer into (0,0,1)
|
||||
//Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
|
||||
const Vector3 zAxis(0,0,1);
|
||||
const Matrix3 rotMat = getRotationMatrix(avg.normalized(), zAxis);
|
||||
//const Matrix3 rotMat = getRotationMatrix(zAxis, avg.normalized()); // INVERSE
|
||||
//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();
|
||||
const Vector3 aligned = (rotMat * avg).normalized();
|
||||
Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
||||
|
||||
return rotMat;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** live-pose-estimation using moving median of the smartphone's accelerometer */
|
||||
struct EstMovingMedian {
|
||||
|
||||
// median the accelerometer
|
||||
MovingMedianTS<float> medianX;
|
||||
MovingMedianTS<float> medianY;
|
||||
MovingMedianTS<float> medianZ;
|
||||
|
||||
EstMovingMedian(const Timestamp window) :
|
||||
medianX(window), medianY(window), medianZ(window) {
|
||||
|
||||
// start approximately
|
||||
addAcc(Timestamp(), AccelerometerData(0,0,9.81));
|
||||
|
||||
}
|
||||
|
||||
/** add the given accelerometer reading */
|
||||
void addAcc(const Timestamp ts, const AccelerometerData& acc) {
|
||||
medianX.add(ts, acc.x);
|
||||
medianY.add(ts, acc.y);
|
||||
medianZ.add(ts, acc.z);
|
||||
}
|
||||
|
||||
AccelerometerData getBase() const {
|
||||
return AccelerometerData(medianX.get(), medianY.get(), medianZ.get());
|
||||
}
|
||||
|
||||
/** get the current rotation matrix estimation */
|
||||
//Eigen::Matrix3f get() const {
|
||||
Matrix3 get() const {
|
||||
|
||||
const Vector3 base(medianX.get(), medianY.get(), medianZ.get());
|
||||
|
||||
// rotate average-accelerometer into (0,0,1)
|
||||
const Vector3 zAxis(0,0,1);
|
||||
const Matrix3 rotMat = getRotationMatrix(base.normalized(), zAxis);
|
||||
|
||||
// just a small sanity check. after applying to rotation the acc-average should become (0,0,1)
|
||||
const Vector3 aligned = (rotMat * base).normalized();
|
||||
Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
||||
|
||||
return rotMat;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
private:
|
||||
|
||||
struct {
|
||||
//Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
|
||||
Matrix3 rotationMatrix = Matrix3::identity();
|
||||
bool isKnown = false;
|
||||
Timestamp lastEstimation;
|
||||
} orientation;
|
||||
|
||||
/** how the pose is estimated */
|
||||
//LongTermMovingAverage est = LongTermMovingAverage(Timestamp::fromMS(1250));
|
||||
EstMovingAverage est = EstMovingAverage(Timestamp::fromMS(300));
|
||||
//EstMovingMedian est = EstMovingMedian(Timestamp::fromMS(400));
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
PoseDetectionPlot plot;
|
||||
#endif
|
||||
|
||||
|
||||
public:
|
||||
|
||||
/** ctor */
|
||||
PoseDetection() {
|
||||
;
|
||||
}
|
||||
|
||||
// /** get the smartphone's rotation matrix */
|
||||
// Eigen::Matrix3f getMatrix() const {
|
||||
// return orientation.rotationMatrix;
|
||||
// }
|
||||
|
||||
/** get the smartphone's rotation matrix */
|
||||
const Matrix3& getMatrix() const {
|
||||
return orientation.rotationMatrix;
|
||||
}
|
||||
|
||||
/** is the pose known and stable? */
|
||||
bool isKnown() const {
|
||||
return orientation.isKnown;
|
||||
}
|
||||
|
||||
void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
|
||||
|
||||
// add accelerometer data
|
||||
est.addAcc(ts, acc);
|
||||
|
||||
// update (if needed)
|
||||
orientation.rotationMatrix = est.get();
|
||||
orientation.isKnown = true;
|
||||
orientation.lastEstimation = ts;
|
||||
|
||||
// debug-plot (if configured)
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
plot.add(ts, est.getBase(), orientation.rotationMatrix);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
// /** get a matrix that rotates the vector "from" into the vector "to" */
|
||||
// static Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) {
|
||||
|
||||
// // http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
|
||||
|
||||
// const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm();
|
||||
// const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
|
||||
|
||||
// Eigen::Matrix3f A; A <<
|
||||
// 0, -x(2), x(1),
|
||||
// x(2), 0, -x(0),
|
||||
// -x(1), x(0), 0;
|
||||
|
||||
// return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A));
|
||||
|
||||
// }
|
||||
|
||||
/** get a matrix that rotates the vector "from" into the vector "to" */
|
||||
static Matrix3 getRotationMatrix(const Vector3& from, const Vector3 to) {
|
||||
|
||||
// http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
|
||||
|
||||
const Vector3 v = from.cross(to) / from.cross(to).norm();
|
||||
const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
|
||||
|
||||
Matrix3 A({
|
||||
0.0f, -v.z, v.y,
|
||||
v.z, 0.0f, -v.x,
|
||||
-v.y, v.x, 0.0f
|
||||
});
|
||||
|
||||
return Matrix3::identity() + (A * std::sin(angle)) + ((A*A) * (1-std::cos(angle)));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// /** get a rotation matrix for the given x,y,z rotation (in radians) */
|
||||
// static Eigen::Matrix3f getRotation(const float x, const float y, const float z) {
|
||||
// const float g = x; const float b = y; const float a = z;
|
||||
// const float a11 = std::cos(a)*std::cos(b);
|
||||
// const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
|
||||
// const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
|
||||
// const float a21 = std::sin(a)*std::cos(b);
|
||||
// const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
|
||||
// const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
|
||||
// const float a31 = -std::sin(b);
|
||||
// const float a32 = std::cos(b)*std::sin(g);
|
||||
// const float a33 = std::cos(b)*std::cos(g);
|
||||
// Eigen::Matrix3f m;
|
||||
// m <<
|
||||
// a11, a12, a13,
|
||||
// a21, a22, a23,
|
||||
// a31, a32, a33;
|
||||
// ;
|
||||
// return m;
|
||||
// }
|
||||
|
||||
// /** estimate the smartphones current holding position */
|
||||
// void estimateHolding2() {
|
||||
|
||||
|
||||
// // z-axis points through the device and is the axis we are interested in
|
||||
// // http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics
|
||||
|
||||
// avgAcc = Eigen::Vector3f::Zero();
|
||||
|
||||
// for (const AccelerometerData& acc : accData) {
|
||||
// //for (const GyroscopeData& acc : gyroData) {
|
||||
// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
|
||||
// // Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
|
||||
// avgAcc += vec;
|
||||
// }
|
||||
|
||||
// //avgAcc /= accData.size();
|
||||
// avgAcc = avgAcc.normalized();
|
||||
|
||||
// Eigen::Vector3f rev; rev << 0,0,1;
|
||||
|
||||
// rotMat = getRotationMatrix(avgAcc, rev);
|
||||
|
||||
|
||||
// //Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis");
|
||||
// //Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis");
|
||||
//// Eigen::Vector3f re = rotMat * avgAcc;
|
||||
//// Eigen::Vector3f diff = rev-re;
|
||||
//// Assert::isTrue(diff.norm() < 0.001, "rotation error");
|
||||
|
||||
// }
|
||||
|
||||
// struct RotationMatrixEstimationUsingAccAngle {
|
||||
|
||||
// Eigen::Vector3f lastAvg;
|
||||
// Eigen::Vector3f avg;
|
||||
// int cnt;
|
||||
|
||||
// RotationMatrixEstimationUsingAccAngle() {
|
||||
// reset();
|
||||
// }
|
||||
|
||||
// void add(const float x, const float y, const float z) {
|
||||
|
||||
// Eigen::Vector3f vec; vec << x,y,z;
|
||||
// avg += vec;
|
||||
// ++cnt;
|
||||
|
||||
// }
|
||||
|
||||
// void reset() {
|
||||
// cnt = 0;
|
||||
// avg = Eigen::Vector3f::Zero();
|
||||
// }
|
||||
|
||||
// Eigen::Matrix3f get() {
|
||||
|
||||
// // http://www.hobbytronics.co.uk/accelerometer-info
|
||||
|
||||
// avg /= cnt;
|
||||
// lastAvg = avg;
|
||||
|
||||
// //const float mag = avg.norm();
|
||||
|
||||
// const float baseX = 0;
|
||||
// const float baseY = 0;
|
||||
// const float baseZ = 0; // mag?
|
||||
|
||||
// const float x = avg(0) - baseX;
|
||||
// const float y = avg(1) - baseY;
|
||||
// const float z = avg(2) - baseZ;
|
||||
|
||||
// const float ax = std::atan( x / (std::sqrt(y*y + z*z)) );
|
||||
// const float ay = std::atan( y / (std::sqrt(x*x + z*z)) );
|
||||
|
||||
// const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0); // TODO -ax or +ax?
|
||||
|
||||
// // sanity check
|
||||
// Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
|
||||
// Eigen::Vector3f aligned = (rotMat * avg).normalized();
|
||||
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
||||
// // int i = 0; (void) i;
|
||||
|
||||
// reset();
|
||||
// return rotMat;
|
||||
|
||||
// }
|
||||
|
||||
// } est;
|
||||
|
||||
// struct PCA {
|
||||
|
||||
// Eigen::Vector3f avg;
|
||||
// Eigen::Vector3f lastAvg;
|
||||
// Eigen::Matrix3f covar;
|
||||
// int cnt = 0;
|
||||
|
||||
// PCA() {
|
||||
// reset();
|
||||
// }
|
||||
|
||||
// void add(const float x, const float y, const float z) {
|
||||
|
||||
// Eigen::Vector3f vec; vec << x,y,z;
|
||||
// avg += vec;
|
||||
// covar += vec*vec.transpose();
|
||||
// ++cnt;
|
||||
|
||||
// }
|
||||
|
||||
// Eigen::Matrix3f get() {
|
||||
|
||||
// avg /= cnt;
|
||||
// covar /= cnt;
|
||||
// lastAvg = avg;
|
||||
|
||||
// std::cout << avg << std::endl;
|
||||
|
||||
// Eigen::Matrix3f Q = covar;// - avg*avg.transpose();
|
||||
// for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));}
|
||||
|
||||
// Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(Q);
|
||||
// solver.eigenvalues();
|
||||
// solver.eigenvectors();
|
||||
|
||||
// const auto eval = solver.eigenvalues();
|
||||
// const auto evec = solver.eigenvectors();
|
||||
// Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!");
|
||||
|
||||
// Eigen::Matrix3f rotMat;
|
||||
// rotMat.col(0) = evec.col(0);
|
||||
// rotMat.col(1) = evec.col(1);
|
||||
// rotMat.col(2) = evec.col(2); // 0,0,1 (z-axis) belongs to the strongest eigenvalue
|
||||
// rotMat.transposeInPlace();
|
||||
|
||||
// //Eigen::Vector3f gy; gy << 0, 30, 30;
|
||||
// Eigen::Vector3f avg1 = rotMat * avg;
|
||||
// int i = 0; (void) i;
|
||||
|
||||
// reset();
|
||||
|
||||
// return rotMat;
|
||||
|
||||
// }
|
||||
|
||||
// void reset() {
|
||||
// cnt = 0;
|
||||
// avg = Eigen::Vector3f::Zero();
|
||||
// covar = Eigen::Matrix3f::Zero();
|
||||
// }
|
||||
|
||||
|
||||
// } pca1;
|
||||
|
||||
|
||||
|
||||
|
||||
// /** estimate the smartphones current holding position */
|
||||
// void estimateHolding() {
|
||||
|
||||
// Eigen::Vector3f avg = Eigen::Vector3f::Zero();
|
||||
// Eigen::Matrix3f covar = Eigen::Matrix3f::Zero();
|
||||
|
||||
// for (const AccelerometerData& acc : accData) {
|
||||
//// for (const GyroscopeData& acc : gyroData) {
|
||||
// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
|
||||
//// Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z);
|
||||
// avg += vec;
|
||||
// covar += vec * vec.transpose();
|
||||
// }
|
||||
|
||||
// avg /= accData.size(); // TODO
|
||||
// covar /= accData.size(); //TODO
|
||||
|
||||
// avgAcc = avg.normalized();
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif // INDOOR_IMU_POSEDETECTION_H
|
||||
146
sensors/imu/PoseDetectionPlot.h
Normal file
146
sensors/imu/PoseDetectionPlot.h
Normal 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
|
||||
@@ -5,23 +5,18 @@
|
||||
#include "AccelerometerData.h"
|
||||
#include "../../data/Timestamp.h"
|
||||
#include "../../math/MovingAverageTS.h"
|
||||
#include "../../math/Matrix3.h"
|
||||
|
||||
#include "../../geo/Point3.h"
|
||||
#include "PoseDetection.h"
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#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 "TurnDetectionPlot.h"
|
||||
|
||||
|
||||
#include "../../Assertions.h"
|
||||
|
||||
@@ -29,98 +24,29 @@ 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
|
||||
|
||||
PoseDetection* pose = nullptr;
|
||||
|
||||
//std::vector<GyroscopeData> gyroData;
|
||||
Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
|
||||
//Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
|
||||
Vector3 prevGyro = Vector3(0,0,0);
|
||||
|
||||
Timestamp lastGyroReading;
|
||||
|
||||
|
||||
struct {
|
||||
Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity();
|
||||
bool isKnown = false;
|
||||
Timestamp lastEstimation;
|
||||
} orientation;
|
||||
|
||||
|
||||
|
||||
#ifdef WITH_DEBUG_PLOT
|
||||
TurnDetectionPlot plot;
|
||||
#endif
|
||||
|
||||
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
|
||||
|
||||
TurnDetection(PoseDetection* pose) : pose(pose) {
|
||||
;
|
||||
}
|
||||
|
||||
|
||||
// does not seem to help...
|
||||
// struct DriftEstimator {
|
||||
|
||||
|
||||
// MovingAverageTS<Eigen::Vector3f> avg;
|
||||
|
||||
// DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) {
|
||||
@@ -139,12 +65,8 @@ public:
|
||||
// } driftEst;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
|
||||
|
||||
|
||||
// ignore the first reading completely, just remember its timestamp
|
||||
if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;}
|
||||
|
||||
@@ -157,17 +79,20 @@ public:
|
||||
|
||||
// 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;}
|
||||
if (!pose->isKnown()) {return 0.0f;}
|
||||
|
||||
// get the current gyro-reading as vector
|
||||
Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
|
||||
//Eigen::Vector3f vec; vec << gyro.x, gyro.y, gyro.z;
|
||||
const Vector3 vec(gyro.x, gyro.y, gyro.z);
|
||||
|
||||
// rotate it into our desired coordinate system, where the smartphone lies flat on the ground
|
||||
Eigen::Vector3f curGyro = orientation.rotationMatrix * vec;
|
||||
//Eigen::Vector3f curGyro = orientation.rotationMatrix * vec;
|
||||
const Vector3 curGyro = pose->getMatrix() * vec;
|
||||
//driftEst.removeDrift(ts, curGyro);
|
||||
|
||||
// area
|
||||
const Eigen::Vector3f area =
|
||||
//const Eigen::Vector3f area =
|
||||
const Vector3 area =
|
||||
|
||||
// Trapezoid rule (should be more accurate but does not always help?!)
|
||||
//(prevGyro * curDiff.sec()) + // squared region
|
||||
@@ -183,485 +108,18 @@ public:
|
||||
prevGyro = curGyro;
|
||||
|
||||
// rotation = z-axis only!
|
||||
const float delta = area(2);
|
||||
|
||||
//const float delta = area(2);
|
||||
const float delta = area.z;
|
||||
|
||||
#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);
|
||||
|
||||
}
|
||||
|
||||
plot.add(ts, delta, gyro, curGyro);
|
||||
#endif
|
||||
|
||||
|
||||
// done
|
||||
return delta;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void addAccelerometer(const Timestamp& ts, const AccelerometerData& acc) {
|
||||
|
||||
// add accelerometer data
|
||||
//pca.add(std::abs(acc.x), std::abs(acc.y), std::abs(acc.z));
|
||||
est.addAcc(ts, acc);
|
||||
|
||||
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
|
||||
|
||||
orientation.rotationMatrix = est.get();
|
||||
orientation.isKnown = true;
|
||||
orientation.lastEstimation = ts;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
// /** estimate the smartphones current holding position */
|
||||
// void estimateHolding2() {
|
||||
|
||||
|
||||
// // z-axis points through the device and is the axis we are interested in
|
||||
// // http://www.kircherelectronics.com/blog/index.php/11-android/sensors/15-android-gyroscope-basics
|
||||
|
||||
// avgAcc = Eigen::Vector3f::Zero();
|
||||
|
||||
// for (const AccelerometerData& acc : accData) {
|
||||
// //for (const GyroscopeData& acc : gyroData) {
|
||||
// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
|
||||
// // Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
|
||||
// avgAcc += vec;
|
||||
// }
|
||||
|
||||
// //avgAcc /= accData.size();
|
||||
// avgAcc = avgAcc.normalized();
|
||||
|
||||
// Eigen::Vector3f rev; rev << 0,0,1;
|
||||
|
||||
// rotMat = getRotationMatrix(avgAcc, rev);
|
||||
|
||||
|
||||
// //Assert::isTrue(avgAcc(2) > avgAcc(0), "z is not the gravity axis");
|
||||
// //Assert::isTrue(avgAcc(2) > avgAcc(1), "z is not the gravity axis");
|
||||
//// Eigen::Vector3f re = rotMat * avgAcc;
|
||||
//// Eigen::Vector3f diff = rev-re;
|
||||
//// Assert::isTrue(diff.norm() < 0.001, "rotation error");
|
||||
|
||||
// }
|
||||
|
||||
public:
|
||||
|
||||
/** get a matrix that rotates the vector "from" into the vector "to" */
|
||||
static Eigen::Matrix3f getRotationMatrix(const Eigen::Vector3f& from, const Eigen::Vector3f to) {
|
||||
|
||||
// http://math.stackexchange.com/questions/293116/rotating-one-3d-vector-to-another
|
||||
|
||||
const Eigen::Vector3f x = from.cross(to) / from.cross(to).norm();
|
||||
const float angle = std::acos( from.dot(to) / from.norm() / to.norm() );
|
||||
|
||||
Eigen::Matrix3f A; A <<
|
||||
0, -x(2), x(1),
|
||||
x(2), 0, -x(0),
|
||||
-x(1), x(0), 0;
|
||||
|
||||
return Eigen::Matrix3f::Identity() + (std::sin(angle) * A) + ((1-std::cos(angle)) * (A*A));
|
||||
|
||||
}
|
||||
|
||||
|
||||
struct XYZ {
|
||||
|
||||
Eigen::Vector3f sum;
|
||||
int cnt;
|
||||
|
||||
XYZ() {
|
||||
reset();
|
||||
}
|
||||
|
||||
/** 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;}
|
||||
|
||||
// adjust sum and count (for average calculation)
|
||||
Eigen::Vector3f vec; vec << acc.x, acc.y, acc.z;
|
||||
sum += vec;
|
||||
++cnt;
|
||||
|
||||
|
||||
}
|
||||
|
||||
AccelerometerData getAvg() const {
|
||||
return AccelerometerData(sum(0), sum(1), sum(2)) / cnt;
|
||||
}
|
||||
|
||||
/** get the current rotation matrix estimation */
|
||||
Eigen::Matrix3f get() const {
|
||||
|
||||
// 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);
|
||||
|
||||
// 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() {
|
||||
cnt = 0;
|
||||
sum = Eigen::Vector3f::Zero();
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
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;
|
||||
// Eigen::Vector3f avg;
|
||||
// int cnt;
|
||||
|
||||
// RotationMatrixEstimationUsingAccAngle() {
|
||||
// reset();
|
||||
// }
|
||||
|
||||
// void add(const float x, const float y, const float z) {
|
||||
|
||||
// Eigen::Vector3f vec; vec << x,y,z;
|
||||
// avg += vec;
|
||||
// ++cnt;
|
||||
|
||||
// }
|
||||
|
||||
// void reset() {
|
||||
// cnt = 0;
|
||||
// avg = Eigen::Vector3f::Zero();
|
||||
// }
|
||||
|
||||
// Eigen::Matrix3f get() {
|
||||
|
||||
// // http://www.hobbytronics.co.uk/accelerometer-info
|
||||
|
||||
// avg /= cnt;
|
||||
// lastAvg = avg;
|
||||
|
||||
// //const float mag = avg.norm();
|
||||
|
||||
// const float baseX = 0;
|
||||
// const float baseY = 0;
|
||||
// const float baseZ = 0; // mag?
|
||||
|
||||
// const float x = avg(0) - baseX;
|
||||
// const float y = avg(1) - baseY;
|
||||
// const float z = avg(2) - baseZ;
|
||||
|
||||
// const float ax = std::atan( x / (std::sqrt(y*y + z*z)) );
|
||||
// const float ay = std::atan( y / (std::sqrt(x*x + z*z)) );
|
||||
|
||||
// const Eigen::Matrix3f rotMat = getRotation(ay, -ax, 0); // TODO -ax or +ax?
|
||||
|
||||
// // sanity check
|
||||
// Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
|
||||
// Eigen::Vector3f aligned = (rotMat * avg).normalized();
|
||||
// Assert::isTrue((aligned-zAxis).norm() < 0.1f, "deviation too high");
|
||||
// // int i = 0; (void) i;
|
||||
|
||||
// reset();
|
||||
// return rotMat;
|
||||
|
||||
// }
|
||||
|
||||
// } est;
|
||||
|
||||
|
||||
/** get a rotation matrix for the given x,y,z rotation (in radians) */
|
||||
static Eigen::Matrix3f getRotation(const float x, const float y, const float z) {
|
||||
const float g = x; const float b = y; const float a = z;
|
||||
const float a11 = std::cos(a)*std::cos(b);
|
||||
const float a12 = std::cos(a)*std::sin(b)*std::sin(g)-std::sin(a)*std::cos(g);
|
||||
const float a13 = std::cos(a)*std::sin(b)*std::cos(g)+std::sin(a)*std::sin(g);
|
||||
const float a21 = std::sin(a)*std::cos(b);
|
||||
const float a22 = std::sin(a)*std::sin(b)*std::sin(g)+std::cos(a)*std::cos(g);
|
||||
const float a23 = std::sin(a)*std::sin(b)*std::cos(g)-std::cos(a)*std::sin(g);
|
||||
const float a31 = -std::sin(b);
|
||||
const float a32 = std::cos(b)*std::sin(g);
|
||||
const float a33 = std::cos(b)*std::cos(g);
|
||||
Eigen::Matrix3f m;
|
||||
m <<
|
||||
a11, a12, a13,
|
||||
a21, a22, a23,
|
||||
a31, a32, a33;
|
||||
;
|
||||
return m;
|
||||
}
|
||||
|
||||
|
||||
// struct PCA {
|
||||
|
||||
// Eigen::Vector3f avg;
|
||||
// Eigen::Vector3f lastAvg;
|
||||
// Eigen::Matrix3f covar;
|
||||
// int cnt = 0;
|
||||
|
||||
// PCA() {
|
||||
// reset();
|
||||
// }
|
||||
|
||||
// void add(const float x, const float y, const float z) {
|
||||
|
||||
// Eigen::Vector3f vec; vec << x,y,z;
|
||||
// avg += vec;
|
||||
// covar += vec*vec.transpose();
|
||||
// ++cnt;
|
||||
|
||||
// }
|
||||
|
||||
// Eigen::Matrix3f get() {
|
||||
|
||||
// avg /= cnt;
|
||||
// covar /= cnt;
|
||||
// lastAvg = avg;
|
||||
|
||||
// std::cout << avg << std::endl;
|
||||
|
||||
// Eigen::Matrix3f Q = covar;// - avg*avg.transpose();
|
||||
// for (int i = 0; i < 9; ++i) {Q(i) = std::abs(Q(i));}
|
||||
|
||||
// Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(Q);
|
||||
// solver.eigenvalues();
|
||||
// solver.eigenvectors();
|
||||
|
||||
// const auto eval = solver.eigenvalues();
|
||||
// const auto evec = solver.eigenvectors();
|
||||
// Assert::isTrue(eval(2) > eval(1) && eval(1) > eval(0), "eigenvalues are not sorted!");
|
||||
|
||||
// Eigen::Matrix3f rotMat;
|
||||
// rotMat.col(0) = evec.col(0);
|
||||
// rotMat.col(1) = evec.col(1);
|
||||
// rotMat.col(2) = evec.col(2); // 0,0,1 (z-axis) belongs to the strongest eigenvalue
|
||||
// rotMat.transposeInPlace();
|
||||
|
||||
// //Eigen::Vector3f gy; gy << 0, 30, 30;
|
||||
// Eigen::Vector3f avg1 = rotMat * avg;
|
||||
// int i = 0; (void) i;
|
||||
|
||||
// reset();
|
||||
|
||||
// return rotMat;
|
||||
|
||||
// }
|
||||
|
||||
// void reset() {
|
||||
// cnt = 0;
|
||||
// avg = Eigen::Vector3f::Zero();
|
||||
// covar = Eigen::Matrix3f::Zero();
|
||||
// }
|
||||
|
||||
|
||||
// } pca1;
|
||||
|
||||
|
||||
|
||||
|
||||
// /** estimate the smartphones current holding position */
|
||||
// void estimateHolding() {
|
||||
|
||||
// Eigen::Vector3f avg = Eigen::Vector3f::Zero();
|
||||
// Eigen::Matrix3f covar = Eigen::Matrix3f::Zero();
|
||||
|
||||
// for (const AccelerometerData& acc : accData) {
|
||||
//// for (const GyroscopeData& acc : gyroData) {
|
||||
// Eigen::Vector3f vec; vec << std::abs(acc.x), std::abs(acc.y), std::abs(acc.z);
|
||||
//// Eigen::Vector3f vec; vec << (acc.x), (acc.y), (acc.z);
|
||||
// avg += vec;
|
||||
// covar += vec * vec.transpose();
|
||||
// }
|
||||
|
||||
// avg /= accData.size(); // TODO
|
||||
// covar /= accData.size(); //TODO
|
||||
|
||||
// avgAcc = avg.normalized();
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//// 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 px(evec(0,0), evec(1,0), evec(2,0)); //px = px * eval(0);
|
||||
//// K::GnuplotPoint3 py(evec(0,1), evec(1,1), evec(2,1)); //py = py * eval(1);
|
||||
//// K::GnuplotPoint3 pz(evec(0,2), evec(1,2), evec(2,2)); //pz = pz * eval(2);
|
||||
|
||||
//// K::GnuplotPoint3 pa(avg(0), avg(1), avg(2));
|
||||
|
||||
//// lines.addSegment(p0, px);
|
||||
//// lines.addSegment(p0, py);
|
||||
//// lines.addSegment(p0, pz);
|
||||
//// lines.addSegment(p0, pa);
|
||||
|
||||
//// gp.draw(plot);
|
||||
//// gp.flush();
|
||||
|
||||
// }
|
||||
|
||||
};
|
||||
|
||||
#endif // TURNDETECTION_H
|
||||
|
||||
118
sensors/imu/TurnDetectionPlot.h
Normal file
118
sensors/imu/TurnDetectionPlot.h
Normal file
@@ -0,0 +1,118 @@
|
||||
#ifndef INDOOR_IMU_TURNDETECTIONPLOT_H
|
||||
#define INDOOR_IMU_TURNDETECTIONPLOT_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 "GyroscopeData.h"
|
||||
#include "../../data/Timestamp.h"
|
||||
#include "../../math/Matrix3.h"
|
||||
|
||||
class TurnDetectionPlot {
|
||||
|
||||
Timestamp plotRef;
|
||||
Timestamp lastPlot;
|
||||
|
||||
K::Gnuplot gp1;
|
||||
|
||||
K::GnuplotMultiplot multiplot = K::GnuplotMultiplot(1,2);
|
||||
|
||||
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::GnuplotSplot plotPose;
|
||||
K::GnuplotSplotElementLines linePose;
|
||||
|
||||
float plotCurHead = 0;
|
||||
|
||||
public:
|
||||
|
||||
TurnDetectionPlot() {
|
||||
|
||||
gp1 << "set autoscale xfix\n";
|
||||
gp1 << "set view equal xyz\n";
|
||||
|
||||
multiplot.add(&plotGyroRaw);
|
||||
multiplot.add(&plotGyroFix);
|
||||
|
||||
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");
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
void add(Timestamp ts, const float delta, const GyroscopeData& gyro, const Vector3& gyroFixed) {
|
||||
|
||||
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(), gyroFixed.x) );
|
||||
lineGyroFixY.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.y) );
|
||||
lineGyroFixZ.add( K::GnuplotPoint2(tsPlot.ms(), gyroFixed.z) );
|
||||
|
||||
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
|
||||
|
||||
lastPlot = tsPlot;
|
||||
|
||||
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);
|
||||
|
||||
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();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif // INDOOR_IMU_TURNDETECTIONPLOT_H
|
||||
@@ -109,16 +109,17 @@ namespace Offline {
|
||||
|
||||
#warning "some sensors todo:"
|
||||
switch(e.type) {
|
||||
case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
|
||||
case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
|
||||
case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
|
||||
case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break;
|
||||
case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break;
|
||||
case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break;
|
||||
case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
|
||||
case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break;
|
||||
case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
|
||||
default: throw Exception("code error. found not-yet-implemented sensor");
|
||||
case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
|
||||
case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[idx].data); break;
|
||||
case Sensor::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
|
||||
case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[idx].data); break;
|
||||
case Sensor::MAGNETOMETER: listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break;
|
||||
case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break;
|
||||
case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break;
|
||||
case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
|
||||
case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break;
|
||||
case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
|
||||
default: throw Exception("code error. found not-yet-implemented sensor");
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -16,6 +16,7 @@
|
||||
#include "../../sensors/beacon/BeaconMeasurements.h"
|
||||
#include "../../sensors/gps/GPSData.h"
|
||||
#include "../../sensors/imu/CompassData.h"
|
||||
#include "../../sensors/imu/MagnetometerData.h"
|
||||
|
||||
#include "../../geo/Point2.h"
|
||||
#include "../../grid/factory/v2/GridFactory.h"
|
||||
@@ -51,6 +52,7 @@ namespace Offline {
|
||||
std::vector<TS<GravityData>> gravity;
|
||||
std::vector<TS<GPSData>> gps;
|
||||
std::vector<TS<CompassData>> compass;
|
||||
std::vector<TS<MagnetometerData>> magnetometer;
|
||||
|
||||
/** all entries in linear order as they appeared while recording */
|
||||
std::vector<Entry> entries;
|
||||
@@ -88,6 +90,7 @@ namespace Offline {
|
||||
barometer.clear();
|
||||
lin_acc.clear();
|
||||
gravity.clear();
|
||||
magnetometer.clear();
|
||||
}
|
||||
|
||||
const std::vector<Entry>& getEntries() const {return entries;}
|
||||
@@ -113,6 +116,8 @@ namespace Offline {
|
||||
|
||||
const std::vector<TS<GravityData>>& getGravity() const {return gravity;}
|
||||
|
||||
const std::vector<TS<MagnetometerData>>& getMagnetometer() const {return magnetometer;}
|
||||
|
||||
/** get an interpolateable ground-truth based on the time-clicks during recording */
|
||||
GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> groundTruthPoints) const {
|
||||
|
||||
@@ -147,7 +152,7 @@ namespace Offline {
|
||||
void parse(const std::string& file) {
|
||||
|
||||
std::ifstream inp(file);
|
||||
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file" + file);}
|
||||
if (!inp.is_open() || inp.bad() || inp.eof()) {throw Exception("failed to open file: " + file);}
|
||||
|
||||
while(!inp.eof() && !inp.bad()) {
|
||||
|
||||
@@ -172,6 +177,7 @@ namespace Offline {
|
||||
else if (idx == (int)Sensor::GRAVITY) {parseGravity(ts,data);}
|
||||
else if (idx == (int)Sensor::COMPASS) {parseCompass(ts,data);}
|
||||
else if (idx == (int)Sensor::GPS) {parseGPS(ts,data);}
|
||||
else if (idx == (int)Sensor::MAGNETOMETER) {parseMagnetometer(ts,data);}
|
||||
|
||||
// TODO: this is a hack...
|
||||
// the loop is called one additional time after the last entry
|
||||
@@ -343,6 +349,24 @@ namespace Offline {
|
||||
|
||||
}
|
||||
|
||||
void parseMagnetometer(const uint64_t ts, const std::string& data) {
|
||||
|
||||
MagnetometerData mag;
|
||||
Splitter s(data, sep);
|
||||
|
||||
mag.x = s.has(0) ? (s.getFloat(0)) : (NAN);
|
||||
mag.y = s.has(1) ? (s.getFloat(1)) : (NAN);
|
||||
mag.z = s.has(2) ? (s.getFloat(2)) : (NAN);
|
||||
|
||||
TS<MagnetometerData> elem(ts, mag);
|
||||
this->magnetometer.push_back(elem);
|
||||
entries.push_back(Entry(Sensor::MAGNETOMETER, ts, this->magnetometer.size()-1));
|
||||
|
||||
// inform listener
|
||||
//if (listener) {listener->onCompass(Timestamp::fromMS(ts), compass);}
|
||||
|
||||
}
|
||||
|
||||
/** parse the given GPS entry */
|
||||
void parseGPS(const uint64_t ts, const std::string& data) {
|
||||
|
||||
|
||||
@@ -2,11 +2,15 @@
|
||||
#define OFFLINE_LISTENER_H
|
||||
|
||||
#include "../gps/GPSData.h"
|
||||
|
||||
#include "../pressure/BarometerData.h"
|
||||
|
||||
#include "../imu/CompassData.h"
|
||||
#include "../imu/GravityData.h"
|
||||
#include "../pressure/BarometerData.h"
|
||||
#include "../imu/GyroscopeData.h"
|
||||
#include "../imu/AccelerometerData.h"
|
||||
#include "../imu/MagnetometerData.h"
|
||||
|
||||
#include "../radio/WiFiMeasurements.h"
|
||||
|
||||
namespace Offline {
|
||||
@@ -25,6 +29,7 @@ namespace Offline {
|
||||
virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
|
||||
virtual void onGPS(const Timestamp ts, const GPSData data) = 0;
|
||||
virtual void onCompass(const Timestamp ts, const CompassData data) = 0;
|
||||
virtual void onMagnetometer(const Timestamp ts, const MagnetometerData data) = 0;
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -8,6 +8,7 @@ namespace Offline {
|
||||
GRAVITY = 1,
|
||||
LIN_ACC = 2,
|
||||
GYRO = 3,
|
||||
MAGNETOMETER = 4,
|
||||
BARO = 5,
|
||||
COMPASS = 6, // also called "orientatioN"
|
||||
WIFI = 8,
|
||||
|
||||
Reference in New Issue
Block a user