From 284c6b11a6ed1d32163b2d6a84b80f9ce5fc6610 Mon Sep 17 00:00:00 2001 From: frank Date: Tue, 31 Oct 2017 19:38:08 +0100 Subject: [PATCH] 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 --- math/Matrix3.h | 175 +++++++++ math/MovingMedian.h | 4 +- math/MovingMedianTS.h | 74 ++++ sensors/imu/CompassDetection.h | 112 ++++++ sensors/imu/CompassDetectionPlot.h | 123 ++++++ sensors/imu/MagnetometerData.h | 81 ++++ sensors/imu/PoseDetection.h | 480 +++++++++++++++++++++++ sensors/imu/PoseDetectionPlot.h | 146 +++++++ sensors/imu/TurnDetection.h | 588 ++--------------------------- sensors/imu/TurnDetectionPlot.h | 118 ++++++ sensors/offline/FilePlayer.h | 21 +- sensors/offline/FileReader.h | 26 +- sensors/offline/Listener.h | 7 +- sensors/offline/Sensors.h | 1 + 14 files changed, 1377 insertions(+), 579 deletions(-) create mode 100644 math/Matrix3.h create mode 100644 math/MovingMedianTS.h create mode 100644 sensors/imu/CompassDetection.h create mode 100644 sensors/imu/CompassDetectionPlot.h create mode 100644 sensors/imu/MagnetometerData.h create mode 100644 sensors/imu/PoseDetection.h create mode 100644 sensors/imu/PoseDetectionPlot.h create mode 100644 sensors/imu/TurnDetectionPlot.h diff --git a/math/Matrix3.h b/math/Matrix3.h new file mode 100644 index 0000000..2ca0b7f --- /dev/null +++ b/math/Matrix3.h @@ -0,0 +1,175 @@ +#ifndef INDOOR_MATH_MATRIX3_H +#define INDOOR_MATH_MATRIX3_H + + +#include +#include + +class Matrix3 { + +private: + + float data[9]; + +public: + + Matrix3(std::initializer_list lst) { + int idx = 0; + for (float f : lst) { + data[idx] = f; ++idx; + } + } + + static Matrix3 identity() { + return Matrix3( {1,0,0, 0,1,0, 0,0,1} ); + } + +// static Matrix3 getRotationDeg(const float degX, const float degY, const float degZ) { +// return getRotationRad(degX/180.0f*M_PI, degY/180.0f*M_PI, degZ/180.0f*M_PI); +// } + +// static Matrix4 getRotationRad(const float radX, const float radY, const float radZ) { + +// const float g = radX; const float b = radY; const float a = radZ; +// 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); +// return Matrix4({ +// a11, a12, a13, 0, +// a21, a22, a23, 0, +// a31, a32, a33, 0, +// 0, 0, 0, 1 +// }); + +// } + + static Matrix3 getTranslation(const float x, const float y) { + return Matrix3({ + 1, 0, x, + 0, 1, y, + 0, 0, 1, + }); + } + + static Matrix3 getScale(const float x, const float y) { + return Matrix3({ + x, 0, 0, + 0, y, 0, + 0, 0, 1, + }); + } + + float operator [] (const int idx) const {return data[idx];} + + bool operator == (const Matrix3& o) const { + for (int i = 0; i < 9; ++i) { + if (data[i] != o.data[i]) {return false;} + } + return true; + } + + Matrix3 operator * (const float v) const { + return Matrix3({ + data[0]*v, data[1]*v, data[2]*v, + data[3]*v, data[4]*v, data[5]*v, + data[6]*v, data[7]*v, data[8]*v, + }); + } + + Matrix3 operator + (const Matrix3& m) const { + return Matrix3({ + data[0]+m.data[0], data[1]+m.data[1], data[2]+m.data[2], + data[3]+m.data[3], data[4]+m.data[4], data[5]+m.data[5], + data[6]+m.data[6], data[7]+m.data[7], data[8]+m.data[8], + }); + } + + Matrix3 operator * (const Matrix3& m) const { + return Matrix3({ + data[0]*m.data[0] + data[1]*m.data[3] + data[2]*m.data[6], data[0]*m.data[1] + data[1]*m.data[4] + data[2]*m.data[7], data[0]*m.data[2] + data[1]*m.data[5] + data[2]*m.data[8], + data[3]*m.data[0] + data[4]*m.data[3] + data[5]*m.data[6], data[3]*m.data[1] + data[4]*m.data[4] + data[5]*m.data[7], data[3]*m.data[2] + data[4]*m.data[5] + data[5]*m.data[8], + data[6]*m.data[0] + data[7]*m.data[3] + data[8]*m.data[6], data[6]*m.data[1] + data[7]*m.data[4] + data[8]*m.data[7], data[6]*m.data[2] + data[7]*m.data[5] + data[8]*m.data[8], + }); + } + +}; + +struct Vector3 { + + float x,y,z; + + Vector3() : x(0), y(0), z(0) {;} + + Vector3(float x, float y, float z) : x(x), y(y), z(z) {;} + + Vector3 operator + (const Vector3 o) const { + return Vector3(x+o.x, y+o.y, z+o.z); + } + Vector3 operator - (const Vector3 o) const { + return Vector3(x-o.x, y-o.y, z-o.z); + } + Vector3 operator * (const Vector3 o) const { + return Vector3(x*o.x, y*o.y, z*o.z); + } + + + Vector3 operator * (const float v) const { + return Vector3(x*v, y*v, z*v); + } + Vector3 operator / (const float v) const { + return Vector3(x/v, y/v, z/v); + } + + bool operator == (const Vector3 o) const { + return (x==o.x) && (y==o.y) && (z==o.z); + } + + float norm() const { + return std::sqrt(x*x + y*y + z*z); + } + + Vector3 normalized() const { + const float n = norm(); + return Vector3(x/n, y/n, z/n); + } + + Vector3 cross(const Vector3 o) const { + return Vector3( + y*o.z - z*o.y, + z*o.x - x*o.z, + x*o.y - y*o.x + ); + } + + float dot(const Vector3 o) const { + return (x*o.x) + (y*o.y) + (z*o.z); + } + +}; + +inline Vector3 operator * (const Matrix3& mat, const Vector3& vec) { + + return Vector3( + (mat[ 0]*vec.x + mat[ 1]*vec.y + mat[ 2]*vec.z), + (mat[ 3]*vec.x + mat[ 4]*vec.y + mat[ 5]*vec.z), + (mat[ 6]*vec.x + mat[ 7]*vec.y + mat[ 8]*vec.z) + ); + +} + +//inline Matrix4 operator * (const Matrix4& m1, const Matrix4& m2) { +// return Matrix4({ +// m1[ 0]*m2[ 0] + m1[ 1]*m2[ 4] + m1[ 2]*m2[ 8] + m1[ 3]*m2[12], m1[ 0]*m2[ 1] + m1[ 1]*m2[ 5] + m1[ 2]*m2[ 9] + m1[ 3]*m2[13], m1[ 0]*m2[ 2] + m1[ 1]*m2[ 6] + m1[ 2]*m2[10] + m1[ 3]*m2[14], m1[ 0]*m2[ 3] + m1[ 1]*m2[ 7] + m1[ 2]*m2[11] + m1[ 3]*m2[15], +// m1[ 4]*m2[ 0] + m1[ 5]*m2[ 4] + m1[ 6]*m2[ 8] + m1[ 7]*m2[12], m1[ 4]*m2[ 1] + m1[ 5]*m2[ 5] + m1[ 6]*m2[ 9] + m1[ 7]*m2[13], m1[ 4]*m2[ 2] + m1[ 5]*m2[ 6] + m1[ 6]*m2[10] + m1[ 7]*m2[14], m1[ 4]*m2[ 3] + m1[ 5]*m2[ 7] + m1[ 6]*m2[11] + m1[ 7]*m2[15], +// m1[ 8]*m2[ 0] + m1[ 9]*m2[ 4] + m1[10]*m2[ 8] + m1[11]*m2[12], m1[ 8]*m2[ 1] + m1[ 9]*m2[ 5] + m1[10]*m2[ 9] + m1[11]*m2[13], m1[ 8]*m2[ 2] + m1[ 9]*m2[ 6] + m1[10]*m2[10] + m1[11]*m2[14], m1[ 8]*m2[ 3] + m1[ 9]*m2[ 7] + m1[10]*m2[11] + m1[11]*m2[15], +// m1[12]*m2[ 0] + m1[13]*m2[ 4] + m1[14]*m2[ 8] + m1[15]*m2[12], m1[12]*m2[ 1] + m1[13]*m2[ 5] + m1[14]*m2[ 9] + m1[15]*m2[13], m1[12]*m2[ 2] + m1[13]*m2[ 6] + m1[14]*m2[10] + m1[15]*m2[14], m1[12]*m2[ 3] + m1[13]*m2[ 7] + m1[14]*m2[11] + m1[15]*m2[15] +// }); +//} + +#endif // INDOOR_MATH_MATRIX3_H diff --git a/math/MovingMedian.h b/math/MovingMedian.h index 9ad4cdd..56c8ee5 100644 --- a/math/MovingMedian.h +++ b/math/MovingMedian.h @@ -41,9 +41,9 @@ public: // get the median if (values.size() % 2 != 0) { - return values[(values.size() + 0) / 2]; + return copy[(copy.size() + 0) / 2]; } else { - return (values[values.size() / 2 - 1] + values[values.size() / 2 + 0]) / 2; + return (copy[copy.size() / 2 - 1] + copy[copy.size() / 2 + 0]) / 2; } } diff --git a/math/MovingMedianTS.h b/math/MovingMedianTS.h new file mode 100644 index 0000000..a3722ca --- /dev/null +++ b/math/MovingMedianTS.h @@ -0,0 +1,74 @@ +#ifndef MOVINGMEDIANTS_H +#define MOVINGMEDIANTS_H + +#include +#include +#include "../data/Timestamp.h" + +template class MovingMedianTS { + +private: + + /** timestamp -> value combination */ + struct Entry { + Timestamp ts; + T value; + Entry(const Timestamp ts, const T& value) : ts(ts), value(value) {;} + }; + + /** the regional window to use */ + Timestamp window; + + /** the value history for the window-size */ + std::vector history; + + + + +public: + + /** ctor with the window-size to use */ + MovingMedianTS(const Timestamp window) : window(window) { + + } + + /** add a new value */ + void add(const Timestamp ts, const T data) { + + // append to history + history.push_back(Entry(ts, data)); + + // remove too-old history entries + const Timestamp oldest = ts - window; + while(history.front().ts < oldest) { + + // remove from history + history.erase(history.begin()); + + } + + } + + /** get the current median */ + T get() const { + + const auto comp = [] (const Entry& a, const Entry& b) { + return a.value < b.value; + }; + + // create a sorted copy (slow, but works) + std::vector copy = history; + std::sort(copy.begin(), copy.end(), comp); + + // get the median + if (copy.size() % 2 != 0) { + return copy[(copy.size() + 0) / 2].value; + } else { + return (copy[copy.size() / 2 - 1].value + copy[copy.size() / 2 + 0].value) / 2; + } + + } + +}; + +#endif // MOVINGMEDIANTS_H diff --git a/sensors/imu/CompassDetection.h b/sensors/imu/CompassDetection.h new file mode 100644 index 0000000..0af0398 --- /dev/null +++ b/sensors/imu/CompassDetection.h @@ -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 +#include + + +/** + * 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 avgIn = MovingAverageTS(Timestamp::fromMS(150), MagnetometerData(0,0,0)); + + //MovingStdDevTS stdDev = MovingStdDevTS(Timestamp::fromMS(2000), MagnetometerData(0,0,0)); + MovingStdDevTS stdDev = MovingStdDevTS(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 diff --git a/sensors/imu/CompassDetectionPlot.h b/sensors/imu/CompassDetectionPlot.h new file mode 100644 index 0000000..2b4db3b --- /dev/null +++ b/sensors/imu/CompassDetectionPlot.h @@ -0,0 +1,123 @@ +#ifndef INDOOR_IMU_COMPASSDETECTIONPLOT_H +#define INDOOR_IMU_COMPASSDETECTIONPLOT_H + +#ifdef WITH_DEBUG_PLOT + + #include + #include + #include + #include + #include + #include + #include + + #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 diff --git a/sensors/imu/MagnetometerData.h b/sensors/imu/MagnetometerData.h new file mode 100644 index 0000000..24136c7 --- /dev/null +++ b/sensors/imu/MagnetometerData.h @@ -0,0 +1,81 @@ +#ifndef INDOOR_IMU_MAGNETOMETERDATA_H +#define INDOOR_IMU_MAGNETOMETERDATA_H + + +#include +#include + +/** + * 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 diff --git a/sensors/imu/PoseDetection.h b/sensors/imu/PoseDetection.h new file mode 100644 index 0000000..f485534 --- /dev/null +++ b/sensors/imu/PoseDetection.h @@ -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 + +#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 avg; + + EstMovingAverage(const Timestamp window) : + avg(MovingAverageTS(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 medianX; + MovingMedianTS medianY; + MovingMedianTS 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 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 diff --git a/sensors/imu/PoseDetectionPlot.h b/sensors/imu/PoseDetectionPlot.h new file mode 100644 index 0000000..e065ad5 --- /dev/null +++ b/sensors/imu/PoseDetectionPlot.h @@ -0,0 +1,146 @@ +#ifndef INDOOR_IMU_POSEDETECTIONPLOT_H +#define INDOOR_IMU_POSEDETECTIONPLOT_H + +#ifdef WITH_DEBUG_PLOT + + #include + #include + #include + #include + #include + #include + #include + #include + #include + + #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>> 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& 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 diff --git a/sensors/imu/TurnDetection.h b/sensors/imu/TurnDetection.h index 2ab0f6d..01dbe7c 100644 --- a/sensors/imu/TurnDetection.h +++ b/sensors/imu/TurnDetection.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 #include #include -#ifdef WITH_DEBUG_PLOT - #include - #include - #include - #include - #include - #include - #include - #include -#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 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 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 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 avg = MovingAverageTS(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 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 diff --git a/sensors/imu/TurnDetectionPlot.h b/sensors/imu/TurnDetectionPlot.h new file mode 100644 index 0000000..a0a9857 --- /dev/null +++ b/sensors/imu/TurnDetectionPlot.h @@ -0,0 +1,118 @@ +#ifndef INDOOR_IMU_TURNDETECTIONPLOT_H +#define INDOOR_IMU_TURNDETECTIONPLOT_H + +#ifdef WITH_DEBUG_PLOT + + #include + #include + #include + #include + #include + #include + #include + #include + + #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 diff --git a/sensors/offline/FilePlayer.h b/sensors/offline/FilePlayer.h index dcb88ba..b257b42 100644 --- a/sensors/offline/FilePlayer.h +++ b/sensors/offline/FilePlayer.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"); } } diff --git a/sensors/offline/FileReader.h b/sensors/offline/FileReader.h index 67ca235..6bf055e 100644 --- a/sensors/offline/FileReader.h +++ b/sensors/offline/FileReader.h @@ -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> gravity; std::vector> gps; std::vector> compass; + std::vector> magnetometer; /** all entries in linear order as they appeared while recording */ std::vector entries; @@ -88,6 +90,7 @@ namespace Offline { barometer.clear(); lin_acc.clear(); gravity.clear(); + magnetometer.clear(); } const std::vector& getEntries() const {return entries;} @@ -113,6 +116,8 @@ namespace Offline { const std::vector>& getGravity() const {return gravity;} + const std::vector>& 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 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 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) { diff --git a/sensors/offline/Listener.h b/sensors/offline/Listener.h index 83cc28c..abdb2c8 100644 --- a/sensors/offline/Listener.h +++ b/sensors/offline/Listener.h @@ -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; }; diff --git a/sensors/offline/Sensors.h b/sensors/offline/Sensors.h index 32168cd..80971b9 100644 --- a/sensors/offline/Sensors.h +++ b/sensors/offline/Sensors.h @@ -8,6 +8,7 @@ namespace Offline { GRAVITY = 1, LIN_ACC = 2, GYRO = 3, + MAGNETOMETER = 4, BARO = 5, COMPASS = 6, // also called "orientatioN" WIFI = 8,