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

many changes

Browse Source 
added new helper class for 3x3 matrices and vec3
added magnetometer data
added compass detection
refactored pose-estimation (single class)
refactored debug plots (move to own class)
minor changes
This commit is contained in:
FrankE
2017-10-31 19:38:08 +01:00
parent d97b325650
commit 284c6b11a6
14 changed files with 1377 additions and 579 deletions
Download Patch File Download Diff File Expand all files Collapse all files

175
math/Matrix3.h Normal file
View File

@@ -0,0 +1,175 @@
#ifndef INDOOR_MATH_MATRIX3_H
#define INDOOR_MATH_MATRIX3_H
#include <initializer_list>
#include <cmath>
class Matrix3 {
private:
float data[9];
public:
Matrix3(std::initializer_list<float> 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

4
math/MovingMedian.h
View File

@@ -41,9 +41,9 @@ public:
// get the median // get the median
if (values.size() % 2 != 0) { if (values.size() % 2 != 0) {
return values[(values.size() + 0) / 2]; return copy[(copy.size() + 0) / 2];
} else { } 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;
} }
} }

74
math/MovingMedianTS.h Normal file
View File

@@ -0,0 +1,74 @@
#ifndef MOVINGMEDIANTS_H
#define MOVINGMEDIANTS_H
#include <vector>
#include <algorithm>
#include "../data/Timestamp.h"
template <typename T> 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<Entry> 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<Entry> 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

112
sensors/imu/CompassDetection.h Normal file
View 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
View 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
View 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
View 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
View File

@@ -0,0 +1,146 @@
#ifndef INDOOR_IMU_POSEDETECTIONPLOT_H
#define INDOOR_IMU_POSEDETECTIONPLOT_H
#ifdef WITH_DEBUG_PLOT
#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotPlot.h>
#include <KLib/misc/gnuplot/GnuplotPlotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotMultiplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementEmpty.h>
#include "../../data/Timestamp.h"
#include "../../math/Matrix3.h"
#include "AccelerometerData.h"
class PoseDetectionPlot {
Timestamp plotRef;
Timestamp lastPlot;
K::Gnuplot gp1;
K::Gnuplot gp2;
K::GnuplotPlot plotAcc;
K::GnuplotPlotElementLines lineAccX;
K::GnuplotPlotElementLines lineAccY;
K::GnuplotPlotElementLines lineAccZ;
K::GnuplotSplot plotPose;
K::GnuplotSplotElementLines linePose;
//K::GnuplotSplotElementEmpty emptyPose;
std::vector<std::vector<std::vector<float>>> pose;
public:
/** ctor */
PoseDetectionPlot() {
gp1 << "set autoscale xfix\n";
gp2 << "set view equal xyz\n";
plotAcc.setTitle("Accelerometer");
plotAcc.add(&lineAccX); lineAccX.getStroke().getColor().setHexStr("#ff0000"); lineAccX.setTitle("gyroX");
plotAcc.add(&lineAccY); lineAccY.getStroke().getColor().setHexStr("#00ff00"); lineAccY.setTitle("gyroY");
plotAcc.add(&lineAccZ); lineAccZ.getStroke().getColor().setHexStr("#0000ff"); lineAccZ.setTitle("gyroZ");
plotPose.setTitle("Pose");
plotPose.getView().setEnabled(false);
plotPose.add(&linePose);
//plotPose.add(&emptyPose);
plotPose.getAxisX().setRange(-8,+8);
plotPose.getAxisY().setRange(-8,+8);
plotPose.getAxisZ().setRange(-8,+8);
const float a = 0.05; const float b = 0.95;
pose = {
{{0, 0, 0},{1, 0, 0},{1, 1, 0},{0, 1, 0},{0, 0, 0}}, // boden
{{0, 0, 0},{0, 0, 1},{0, 1, 1},{0, 1, 0},{0, 0, 0}}, // links
{{1, 0, 0},{1, 0, 1},{1, 1, 1},{1, 1, 0},{1, 0, 0}}, // rechts
{{0, 1, 0},{1, 1, 0},{1, 1, 1},{0, 1, 1},{0, 1, 0}}, // oben
{{0, 0, 0},{1, 0, 0},{1, 0, 1},{0, 0, 1},{0, 0, 0}}, // unten
{{0, 0, 1},{1, 0, 1},{1, 1, 1},{0, 1, 1},{0, 0, 1}}, // deckel
{{a, 0.15, 1},{b, 0.15, 1},{b, 0.95, 1},{a, 0.95, 1},{a, 0.15, 1}}, // display
};
//K::GnuplotStroke stroke(K::GnuplotDashtype::SOLID, 1, K::GnuplotColor::fromHexStr("#000000"));
K::GnuplotStroke stroke = K::GnuplotStroke::NONE();
K::GnuplotFill fillOut = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#999999"));
K::GnuplotFill fillSide = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#666666"));
K::GnuplotFill fillDisp = K::GnuplotFill(K::GnuplotFillStyle::SOLID, K::GnuplotColor::fromHexStr("#333333"));
plotPose.getObjects().set(1, new K::GnuplotObjectPolygon(fillOut, stroke));
plotPose.getObjects().set(2, new K::GnuplotObjectPolygon(fillSide, stroke));
plotPose.getObjects().set(3, new K::GnuplotObjectPolygon(fillSide, stroke));
plotPose.getObjects().set(4, new K::GnuplotObjectPolygon(fillSide, stroke));
plotPose.getObjects().set(5, new K::GnuplotObjectPolygon(fillSide, stroke));
plotPose.getObjects().set(6, new K::GnuplotObjectPolygon(fillOut, stroke));
plotPose.getObjects().set(7, new K::GnuplotObjectPolygon(fillDisp, stroke));
}
void add(Timestamp ts, const AccelerometerData& avg, const Matrix3& rotation) {
if (plotRef.isZero()) {plotRef = ts;}
const Timestamp tsPlot = (ts-plotRef);
const Timestamp tsOldest = tsPlot - Timestamp::fromMS(5000);
// acc
lineAccX.add( K::GnuplotPoint2(tsPlot.ms(), avg.x) );
lineAccY.add( K::GnuplotPoint2(tsPlot.ms(), avg.y) );
lineAccZ.add( K::GnuplotPoint2(tsPlot.ms(), avg.z) );
if (lastPlot + Timestamp::fromMS(50) < tsPlot) {
lastPlot = tsPlot;
// update 3D smartphone model
for (size_t i = 0; i < pose.size(); ++i) {
K::GnuplotObjectPolygon* gp = (K::GnuplotObjectPolygon*) plotPose.getObjects().get(i+1); gp->clear();
for (const std::vector<float>& pts : pose[i]) {
const Vector3 vec1(pts[0], pts[1], pts[2]);
const Vector3 vec2 = vec1 - Vector3(0.5, 0.5, 0.5); // center cube at 0,0,0
const Vector3 vec3 = vec2 * Vector3(7, 15, 1); // stretch cube
const Vector3 vec4 = rotation * vec3;
gp->add(K::GnuplotCoordinate3(vec4.x, vec4.y, vec4.z, K::GnuplotCoordinateSystem::FIRST));
}
}
// add coordinate system
const Vector3 vx = rotation * Vector3(2,0,0);
const Vector3 vy = rotation * Vector3(0,3,0);
const Vector3 vz = rotation * Vector3(0,0,5);
linePose.clear();
linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vx.x, vx.y, vx.z));
linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vy.x, vy.y, vy.z));
linePose.addSegment(K::GnuplotPoint3(0,0,0), K::GnuplotPoint3(vz.x, vz.y, vz.z));
// remove old accelerometer entries
auto remove = [tsOldest] (const K::GnuplotPoint2 pt) {return pt.x < tsOldest.ms();};
lineAccX.removeIf(remove);
lineAccY.removeIf(remove);
lineAccZ.removeIf(remove);
// raw accelerometer
gp1.draw(plotAcc);
gp1.flush();
// 3D pose
gp2.draw(plotPose);
gp2.flush();
}
}
};
#endif
#endif // INDOOR_IMU_POSEDETECTIONPLOT_H

588
sensors/imu/TurnDetection.h
View File

@@ -5,23 +5,18 @@
#include "AccelerometerData.h" #include "AccelerometerData.h"
#include "../../data/Timestamp.h" #include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h" #include "../../math/MovingAverageTS.h"
#include "../../math/Matrix3.h"
#include "../../geo/Point3.h" #include "../../geo/Point3.h"
#include "PoseDetection.h"
#include <eigen3/Eigen/Dense> #include <eigen3/Eigen/Dense>
#include <cmath> #include <cmath>
#include <vector> #include <vector>
#ifdef WITH_DEBUG_PLOT #include "TurnDetectionPlot.h"
#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 "../../Assertions.h" #include "../../Assertions.h"
@@ -29,98 +24,29 @@ class TurnDetection {
private: private:
#ifdef WITH_DEBUG_PLOT PoseDetection* pose = nullptr;
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
//std::vector<GyroscopeData> gyroData; //std::vector<GyroscopeData> gyroData;
Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero(); //Eigen::Vector3f prevGyro = Eigen::Vector3f::Zero();
Vector3 prevGyro = Vector3(0,0,0);
Timestamp lastGyroReading; Timestamp lastGyroReading;
#ifdef WITH_DEBUG_PLOT
struct { TurnDetectionPlot plot;
Eigen::Matrix3f rotationMatrix = Eigen::Matrix3f::Identity(); #endif
bool isKnown = false;
Timestamp lastEstimation;
} orientation;
public: public:
/** ctor */ /** ctor */
TurnDetection() { TurnDetection(PoseDetection* pose) : pose(pose) {
;
#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
} }
// does not seem to help... // does not seem to help...
// struct DriftEstimator { // struct DriftEstimator {
// MovingAverageTS<Eigen::Vector3f> avg; // MovingAverageTS<Eigen::Vector3f> avg;
// DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) { // DriftEstimator() : avg(Timestamp::fromSec(5.0), Eigen::Vector3f::Zero()) {
@@ -139,12 +65,8 @@ public:
// } driftEst; // } driftEst;
float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) { float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
// ignore the first reading completely, just remember its timestamp // ignore the first reading completely, just remember its timestamp
if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;} if (lastGyroReading.isZero()) {lastGyroReading = ts; return 0.0f;}
@@ -157,17 +79,20 @@ public:
// ignore readings until the first orientation-estimation is available // ignore readings until the first orientation-estimation is available
// otherwise we would use a wrong rotation matrix which yields wrong results! // 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 // 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 // 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); //driftEst.removeDrift(ts, curGyro);
// area // area
const Eigen::Vector3f area = //const Eigen::Vector3f area =
const Vector3 area =
// Trapezoid rule (should be more accurate but does not always help?!) // Trapezoid rule (should be more accurate but does not always help?!)
//(prevGyro * curDiff.sec()) + // squared region //(prevGyro * curDiff.sec()) + // squared region
@@ -183,485 +108,18 @@ public:
prevGyro = curGyro; prevGyro = curGyro;
// rotation = z-axis only! // rotation = z-axis only!
const float delta = area(2); //const float delta = area(2);
const float delta = area.z;
#ifdef WITH_DEBUG_PLOT #ifdef WITH_DEBUG_PLOT
plot.add(ts, delta, gyro, curGyro);
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);
}
#endif #endif
// done // done
return delta; 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 #endif // TURNDETECTION_H

118
sensors/imu/TurnDetectionPlot.h Normal file
View 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

21
sensors/offline/FilePlayer.h
View File

@@ -109,16 +109,17 @@ namespace Offline {
#warning "some sensors todo:" #warning "some sensors todo:"
switch(e.type) { switch(e.type) {
case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break; case Sensor::ACC: listener->onAccelerometer(ts, reader->getAccelerometer()[idx].data); break;
case Sensor::BARO: listener->onBarometer(ts, reader->getBarometer()[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::BEACON: break;//listener->onBe(ts, reader->getBarometer()[idx].data); break;
case Sensor::COMPASS: listener->onCompass(ts, reader->getCompass()[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::MAGNETOMETER: listener->onMagnetometer(ts, reader->getMagnetometer()[idx].data); break;
case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break; case Sensor::GPS: listener->onGPS(ts, reader->getGPS()[idx].data); break;
case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break; case Sensor::GRAVITY: listener->onGravity(ts, reader->getGravity()[idx].data); break;
case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break; case Sensor::GYRO: listener->onGyroscope(ts, reader->getGyroscope()[idx].data); break;
case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break; case Sensor::LIN_ACC: break;//listener->on(ts, reader->getBarometer()[idx].data); break;
default: throw Exception("code error. found not-yet-implemented sensor"); case Sensor::WIFI: listener->onWiFi(ts, reader->getWiFiGroupedByTime()[idx].data); break;
default: throw Exception("code error. found not-yet-implemented sensor");
} }
} }

26
sensors/offline/FileReader.h
View File

@@ -16,6 +16,7 @@
#include "../../sensors/beacon/BeaconMeasurements.h" #include "../../sensors/beacon/BeaconMeasurements.h"
#include "../../sensors/gps/GPSData.h" #include "../../sensors/gps/GPSData.h"
#include "../../sensors/imu/CompassData.h" #include "../../sensors/imu/CompassData.h"
#include "../../sensors/imu/MagnetometerData.h"
#include "../../geo/Point2.h" #include "../../geo/Point2.h"
#include "../../grid/factory/v2/GridFactory.h" #include "../../grid/factory/v2/GridFactory.h"
@@ -51,6 +52,7 @@ namespace Offline {
std::vector<TS<GravityData>> gravity; std::vector<TS<GravityData>> gravity;
std::vector<TS<GPSData>> gps; std::vector<TS<GPSData>> gps;
std::vector<TS<CompassData>> compass; std::vector<TS<CompassData>> compass;
std::vector<TS<MagnetometerData>> magnetometer;
/** all entries in linear order as they appeared while recording */ /** all entries in linear order as they appeared while recording */
std::vector<Entry> entries; std::vector<Entry> entries;
@@ -88,6 +90,7 @@ namespace Offline {
barometer.clear(); barometer.clear();
lin_acc.clear(); lin_acc.clear();
gravity.clear(); gravity.clear();
magnetometer.clear();
} }
const std::vector<Entry>& getEntries() const {return entries;} 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<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 */ /** get an interpolateable ground-truth based on the time-clicks during recording */
GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> groundTruthPoints) const { GroundTruth getGroundTruth(const Floorplan::IndoorMap* map, const std::vector<int> groundTruthPoints) const {
@@ -147,7 +152,7 @@ namespace Offline {
void parse(const std::string& file) { void parse(const std::string& file) {
std::ifstream inp(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()) { 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::GRAVITY) {parseGravity(ts,data);}
else if (idx == (int)Sensor::COMPASS) {parseCompass(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::GPS) {parseGPS(ts,data);}
else if (idx == (int)Sensor::MAGNETOMETER) {parseMagnetometer(ts,data);}
// TODO: this is a hack... // TODO: this is a hack...
// the loop is called one additional time after the last entry // 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 */ /** parse the given GPS entry */
void parseGPS(const uint64_t ts, const std::string& data) { void parseGPS(const uint64_t ts, const std::string& data) {

7
sensors/offline/Listener.h
View File

@@ -2,11 +2,15 @@
#define OFFLINE_LISTENER_H #define OFFLINE_LISTENER_H
#include "../gps/GPSData.h" #include "../gps/GPSData.h"
#include "../pressure/BarometerData.h"
#include "../imu/CompassData.h" #include "../imu/CompassData.h"
#include "../imu/GravityData.h" #include "../imu/GravityData.h"
#include "../pressure/BarometerData.h"
#include "../imu/GyroscopeData.h" #include "../imu/GyroscopeData.h"
#include "../imu/AccelerometerData.h" #include "../imu/AccelerometerData.h"
#include "../imu/MagnetometerData.h"
#include "../radio/WiFiMeasurements.h" #include "../radio/WiFiMeasurements.h"
namespace Offline { namespace Offline {
@@ -25,6 +29,7 @@ namespace Offline {
virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0; virtual void onBarometer(const Timestamp ts, const BarometerData data) = 0;
virtual void onGPS(const Timestamp ts, const GPSData data) = 0; virtual void onGPS(const Timestamp ts, const GPSData data) = 0;
virtual void onCompass(const Timestamp ts, const CompassData data) = 0; virtual void onCompass(const Timestamp ts, const CompassData data) = 0;
virtual void onMagnetometer(const Timestamp ts, const MagnetometerData data) = 0;
}; };

1
sensors/offline/Sensors.h
View File

@@ -8,6 +8,7 @@ namespace Offline {
GRAVITY = 1, GRAVITY = 1,
LIN_ACC = 2, LIN_ACC = 2,
GYRO = 3, GYRO = 3,
MAGNETOMETER = 4,
BARO = 5, BARO = 5,
COMPASS = 6, // also called "orientatioN" COMPASS = 6, // also called "orientatioN"
WIFI = 8, WIFI = 8,